23 from simple_script_server
import simple_script_server
24 import twist_controller_config
as tcc
25 from dynamic_reconfigure.client
import Client
27 from data_collection
import JointStateDataKraken
28 from data_collection
import ObstacleDistanceDataKraken
29 from data_collection
import TwistDataKraken
30 from data_collection
import JointVelocityDataKraken
31 from data_collection
import FrameTrackingDataKraken
35 cli = tcc.TwistControllerReconfigureClient()
38 cli.set_config_param(tcc.NUM_FILT,
False)
39 cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
40 cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
41 cli.set_config_param(tcc.W_THRESH, 0.05)
46 cli.set_config_param(tcc.EPS_TRUNC, 0.001)
48 cli.set_config_param(tcc.PRIO_CA, 100)
49 cli.set_config_param(tcc.PRIO_JLA, 50)
51 cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
52 cli.set_config_param(tcc.K_H, 1.0)
54 cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
55 cli.set_config_param(tcc.K_H_CA, -2.0)
56 cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
57 cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0)
58 cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025)
59 cli.set_config_param(tcc.DAMP_CA, 0.000001)
61 cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
62 cli.set_config_param(tcc.K_H_JLA, -1.0)
63 cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
64 cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
65 cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
66 cli.set_config_param(tcc.DAMP_JLA, 0.00001)
68 cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
69 cli.set_config_param(tcc.KEEP_DIR,
True)
70 cli.set_config_param(tcc.ENF_VEL_LIM,
True)
71 cli.set_config_param(tcc.ENF_POS_LIM,
True)
76 cli = Client(
'frame_tracker')
77 ft_param = {
'cart_min_dist_threshold_lin' : 0.2,
'cart_min_dist_threshold_rot' : 0.2}
78 cli.update_configuration(ft_param)
84 sss.move(
"arm_right",
"home")
85 sss.move(
"arm_right", [[-0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915]])
88 if __name__ ==
"__main__":
89 rospy.init_node(
"test_careobot_st_jla_ca_torus")
91 base_dir =
'/home/fxm-mb/bag-files/2015_08_06/' 92 if rospy.has_param(
'~base_dir'):
93 base_dir = rospy.get_param(
'~base_dir')
95 rospy.logwarn(
'Could not find parameter ~base_dir. Using default base_dir: ' + base_dir)
97 if rospy.has_param(
'chain_tip_link'):
98 chain_tip_link = rospy.get_param(
'chain_tip_link')
100 rospy.logwarn(
'Could not find parameter chain_tip_link.')
103 if rospy.has_param(
'frame_tracker/tracking_frame'):
104 tracking_frame = rospy.get_param(
'frame_tracker/tracking_frame')
106 rospy.logwarn(
'Could not find parameter frame_tracker/tracking_frame.')
109 if rospy.has_param(
'root_frame'):
110 root_frame = rospy.get_param(
'root_frame')
112 rospy.logwarn(
'Could not find parameter root_frame.')
116 launch_time_stamp = time.strftime(
"%Y%m%d_%H_%M_%S", t)
117 command =
'rosbag play ' + base_dir +
'careobot_st_jla_ca.bag' 120 JointStateDataKraken(base_dir +
'careobot_st_jla_ca_joint_state_data_' + launch_time_stamp +
'.csv'),
121 ObstacleDistanceDataKraken(base_dir +
'careobot_st_jla_ca_obst_dist_data_' + launch_time_stamp +
'.csv'),
122 TwistDataKraken(base_dir +
'careobot_st_jla_ca_twist_data_' + launch_time_stamp +
'.csv'),
123 JointVelocityDataKraken(base_dir +
'careobot_st_jla_ca_joint_vel_data_' + launch_time_stamp +
'.csv'),
124 FrameTrackingDataKraken(base_dir +
'careobot_st_jla_ca_frame_tracking_data_' + launch_time_stamp +
'.csv', root_frame, chain_tip_link, tracking_frame), ]
130 for data_kraken
in data_krakens:
131 status_open = status_open
and data_kraken.open()
133 rospy.loginfo(
'Subscribers started for data collection ... \nPress CTRL+C to stop program and write data to the file.')
135 traj_marker_command =
'rosrun cob_twist_controller debug_trajectory_marker_node __ns:=' + rospy.get_namespace()
136 traj_marker_pid = subprocess.Popen(traj_marker_command, stdin = subprocess.PIPE, shell =
True)
137 pid = subprocess.Popen(command, stdin = subprocess.PIPE, cwd = base_dir, shell =
True)
142 if traj_marker_pid.poll()
is not None:
143 rospy.logerr(
"traj_marker_pid returned code. Aborting ...")
144 pid.send_signal(signal.SIGINT)
147 traj_marker_pid.send_signal(signal.SIGTERM)
148 traj_marker_pid.kill()
153 rate = rospy.Rate(10)
155 while not rospy.is_shutdown():
157 if(pid.poll()
is not None):
158 rospy.loginfo(
'Bag file finished stop script.')
161 except (KeyboardInterrupt, SystemExit)
as e:
162 rospy.loginfo(
'KeyboardInterrupt / SystemExit: ' + str(e))
164 for data_kraken
in data_krakens:
165 data_kraken.writeAllData()
167 rospy.logerr(
'Else exception.')
169 for data_kraken
in data_krakens:
170 data_kraken.writeAllData()
175 pid.send_signal(signal.SIGINT)
176 except Exception
as e:
177 rospy.logerr(
'Failed to stop rosbag play due to exception: ' + str(e))
179 traj_marker_pid.kill()
180 traj_marker_pid.send_signal(signal.SIGINT)
181 except Exception
as e:
182 rospy.logerr(
'Failed to stop debug_trajectory_marker_node due to exception: ' + str(e))
184 rospy.logerr(
'Failed to open DataKraken files.')