Commit 9494e368 authored by Lorenz Halt's avatar Lorenz Halt 🔀
Browse files

New srvs_proxy, cleanup

parent 6ae541e8
#!/usr/bin/env python
import sys
import rospy
from ilc.srv import *
def ilc_client(file_in, file_out, L):
print("waiting for ilc_server")
rospy.wait_for_service('ilc_service')
print("ilc_server available")
print(L)
print(type(L))
try:
ilc_service= rospy.ServiceProxy('ilc_service', TriggerILC)
print("calling ilc_server")
print(L)
resp1 = ilc_service(file_in, file_out, L)
return resp1
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
def usage():
return "%s [x y]"%sys.argv[0]
if __name__ == "__main__":
if len(sys.argv) == 4:
file_in = sys.argv[1]
file_out = sys.argv[2]
L = float(sys.argv[3])
else:
print(usage())
sys.exit(1)
print("Reading: %s Writing: %s"%(file_in, file_out))
print("%s , %s, %s".format(file_in, file_out, ilc_client( file_in, file_out, L)))
#!/usr/bin/env python
from __future__ import print_function
from ilc.srv import TriggerILC, TriggerILCResponse
import rospy
import numpy as np
import ilc_lib
import os
import argparse
class Ilc_serviceproxy:
def __init__(self, args):
self.decay_time = None
self.initialize = True
self.T = args.T # sampling time in ms
self.fs = 1.0/(self.T * 0.001) # sampling frequency
self.service = rospy.Service('ilc_service', TriggerILC, self.callback_ilc_calc)
self.folder = args.folder
def callback_ilc_calc(self, req):
try:
### Read Error data
data_org, header = ilc_lib.read(req.file_in)
#ilc_lib.external_data_store("/tmp", "filtered_ilc_data.xxl", data_org, header)
### Resample
data_rs = ilc_lib.resample(data_org, 8)
### Trajectory generation
if self.decay_time is None:
self.decay_time = data_rs[-1, 0]
trajectory = ilc_lib.traj_handle(data_rs, "cos", decay_time=self.decay_time)
### Relative Error: data-trajectory
data_diff = ilc_lib.combine_data(data_rs, trajectory, "diff")
### ILC Gain
data_ff = ilc_lib.ilc_calculations(data_diff, req.L)
### Combine with last iteration, if existing
if not self.initialize and os.path.exists(req.file_out):
last_ff, _ = ilc_lib.read(req.file_out)
data_ff, last_ff = ilc_lib.pad_data(data_ff, last_ff)
data_ilc = ilc_lib.combine_data(data_ff, last_ff, "sum")
else:
data_ilc = data_ff
self.initialize = False
#Filter data
data_ilc_q = ilc_lib.butter_lowpass_filter(data_ilc, req.cutoff_freq, self.fs, 5)
#Write
ilc_lib.write(req.file_out, data_ilc_q, header)
message = "file written sucessfully"
if self.folder is not None:
ilc_lib.external_data_store(self.folder + "/read_error", "read_error", data_org, header)
ilc_lib.external_data_store(self.folder + "/resampled_error", "resampled_error", data_rs ,header)
ilc_lib.external_data_store(self.folder + "/trajectory", "trajectory", trajectory, header)
ilc_lib.external_data_store(self.folder + "/ff_data", "ff_data", data_ff, header)
ilc_lib.external_data_store(self.folder + "/ilc_data", "ilc_data", data_ilc, header)
ilc_lib.external_data_store(self.folder + "/filtered_ilc_data", "filtered_ilc_data", data_ilc_q, header)
return TriggerILCResponse(success=True, message=message)
except Exception as e:
message = str(e)
return TriggerILCResponse(success=False, message=message)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='args')
parser.add_argument('--folder', type=str, required=False, help='Folder to store files in')
parser.add_argument('--T', type=int, default=8, help='Sampling time in ms')
args = parser.parse_args()
rospy.init_node('ilc_server')
Ilc_serviceproxy(args)
print("Ready to serve.")
rospy.spin()
#!/usr/bin/env python
from __future__ import print_function
from ilc.srv import TriggerILC, TriggerILCResponse
import rospy
import numpy as np
import ilc_lib
import os
import argparse
def handle_server(req):
try:
parser = argparse.ArgumentParser(description='args')
parser.add_argument('--folder', type=str, required= True, help='an integer for the accumulator')
args = parser.parse_args()
#Read Error data
data, header = ilc_lib.read(req.file_in)
ilc_lib.external_data_store(args.folder + "/read_error", "read_error",data, header)
#resample
data = ilc_lib.resample(data, 8)
ilc_lib.external_data_store(args.folder + "/resampled_error", "resampled_error", data ,header)
#Handle Trajectory Data (decay time results from length of data)
trajectory, traj_header = ilc_lib.traj_handle(data, req.traj_type)
ilc_lib.external_data_store(args.folder + "/trajectory", "trajectory", header)
#Form relative Error
data = ilc_lib.combine_data(data,trajectory,"diff")
#ILC Calculations
ff_data = ilc_lib.ilc_calculations(data, req.L)
ilc_lib.external_data_store(args.folder + "/ff_data","ff_data",trajectory,header)
#Combine with old Feedforward
if os.path.exists(req.file_out):
old_ff, old_header = ilc_lib.read(req.file_out)
ilc_data = ilc_lib.combine_data(ff_data, old_ff,"sum")
else:
ilc_data = ff_data
ilc_lib.external_data_store(args.folder + "/ilc_data","ilc_data",trajectory,header)
#Filter data
ilc_data_t = list( zip(*ilc_data) )
ilc_data_t[1:4][:] = ilc_lib.butter_lowpass_filter(ilc_data_t[1:4][:], req.cutoff_freq, 120, 5)
ilc_data = list( zip(*ilc_data_t) )
ilc_lib.external_data_store(args.folder + "/filtered_ilc_data","filtered_ilc_data",ilc_data,header)
#Write
ilc_lib.write(req.file_out,ilc_data,header,len(ilc_data[0])-1)
message = "file written sucessfully"
print("---------------------------")
except Exception as e:
message = str(e)
return TriggerILCResponse(success=True, message=message)
def ilc_server():
rospy.init_node('ilc_server')
s = rospy.Service('ilc_service', TriggerILC, handle_server)
print("Ready to serve.")
rospy.spin()
if __name__ == "__main__":
ilc_server()
#!/usr/bin/env python
import rospy
import pandas as pd
import os
from geometry_msgs.msg import WrenchStamped
import argparse
import numpy as np
def callback(data):
always=True
if always==True:
transformed_data = "{},{},{},{},{},{}" .format( data.wrench.force.x, data.wrench.force.y ,data.wrench.force.z ,data.wrench.torque.x ,data.wrench.torque.y ,data.wrench.torque.z)
df = pd.DataFrame(eval(transformed_data))
df = df.T
df.columns = ["force.x","force.y","force.z","torque.x","torque.y","torque.z"]
df.index = ["{}".format(data.header.stamp.secs*1000 + data.header.stamp.nsecs/1000000)]
with open(wrench_store_path + "/data.csv", 'a') as f:
df.to_csv(f, header=f.tell()==0)
else:
if os.path.exists( wrench_store_path + "/data.csv" ) == False:
df = pd.DataFrame(np.array([0,0,0,0,0,0]))
df = df.T
df.columns = ["force.x","force.y","force.z","torque.x","torque.y","torque.z"]
df.index = ["{}".format(data.header.stamp.secs*1000 + data.header.stamp.nsecs/1000000)]
with open(wrench_store_path + "/data.csv", 'a') as f:
df.to_csv(f, header=f.tell()==0)
csv_data = pd.read_csv(wrench_store_path+"/data.csv", dtype=float, usecols=[1,2,3,4,5,6], header=0)
csv_data = csv_data.to_numpy()
if np.array_equal( csv_data[len(csv_data)-1],np.array([0,0,0,0,0,0]) ) == False:
df = pd.DataFrame(np.array([0,0,0,0,0,0]))
df = df.T
df.columns = ["force.x","force.y","force.z","torque.x","torque.y","torque.z"]
df.index = ["{}{}".format(data.header.stamp.secs*1000 + data.header.stamp.nsecs/1000000)]
with open(wrench_store_path + "/data.csv", 'a') as f:
df.to_csv(f, header=f.tell()==0)
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/wrench", WrenchStamped, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Changes ILC-behaviour')
parser.add_argument('--file', type = str, default="wrench_data", required = True, help="path to store wrench data to" )
args = parser.parse_args()
wrench_store_path = args.file
if os.path.exists( wrench_store_path ) == False:
# oldmask = os.umask(000)
os.makedirs(wrench_store_path)
listener()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment