00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import time
00019 import rospy
00020 import subprocess
00021
00022 from simple_script_server.simple_script_server import simple_script_server
00023 import twist_controller_config as tcc
00024 from dynamic_reconfigure.client import Client
00025
00026 from data_collection import JointStateDataKraken
00027 from data_collection import ObstacleDistanceDataKraken
00028 from data_collection import TwistDataKraken
00029 from data_collection import JointVelocityDataKraken
00030 from data_collection import FrameTrackingDataKraken
00031
00032
00033 def init_dyn_recfg():
00034 cli = tcc.TwistControllerReconfigureClient()
00035 cli.init()
00036
00037 cli.set_config_param(tcc.NUM_FILT, False)
00038 cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
00039 cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
00040 cli.set_config_param(tcc.W_THRESH, 0.05)
00041
00042
00043
00044
00045 cli.set_config_param(tcc.EPS_TRUNC, 0.001)
00046
00047 cli.set_config_param(tcc.PRIO_CA, 100)
00048 cli.set_config_param(tcc.PRIO_JLA, 50)
00049
00050 cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
00051 cli.set_config_param(tcc.K_H, 1.0)
00052
00053 cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
00054 cli.set_config_param(tcc.K_H_CA, -2.0)
00055 cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
00056 cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0)
00057 cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025)
00058 cli.set_config_param(tcc.DAMP_CA, 0.000001)
00059
00060 cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
00061 cli.set_config_param(tcc.K_H_JLA, -1.0)
00062 cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
00063 cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
00064 cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
00065 cli.set_config_param(tcc.DAMP_JLA, 0.00001)
00066
00067 cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
00068 cli.set_config_param(tcc.KEEP_DIR, True)
00069 cli.set_config_param(tcc.ENF_VEL_LIM, True)
00070 cli.set_config_param(tcc.ENF_POS_LIM, True)
00071
00072 cli.update()
00073 cli.close()
00074
00075 cli = Client('frame_tracker')
00076 ft_param = {'cart_min_dist_threshold_lin' : 0.2, 'cart_min_dist_threshold_rot' : 0.2}
00077 cli.update_configuration(ft_param)
00078 cli.close()
00079
00080
00081 def init_pos():
00082 sss = simple_script_server()
00083 sss.move("arm_right", "home")
00084 sss.move("arm_right", [[-0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915]])
00085
00086
00087 if __name__ == "__main__":
00088 rospy.init_node("test_careobot_st_jla_ca_torus")
00089
00090 base_dir = '/home/fxm-mb/bag-files/2015_08_06/'
00091 if rospy.has_param('~base_dir'):
00092 base_dir = rospy.get_param('~base_dir')
00093 else:
00094 rospy.logwarn('Could not find parameter ~base_dir. Using default base_dir: ' + base_dir)
00095
00096 if rospy.has_param('chain_tip_link'):
00097 chain_tip_link = rospy.get_param('chain_tip_link')
00098 else:
00099 rospy.logwarn('Could not find parameter chain_tip_link.')
00100 exit(-1)
00101
00102 if rospy.has_param('frame_tracker/tracking_frame'):
00103 tracking_frame = rospy.get_param('frame_tracker/tracking_frame')
00104 else:
00105 rospy.logwarn('Could not find parameter frame_tracker/tracking_frame.')
00106 exit(-2)
00107
00108 if rospy.has_param('root_frame'):
00109 root_frame = rospy.get_param('root_frame')
00110 else:
00111 rospy.logwarn('Could not find parameter root_frame.')
00112 exit(-3)
00113
00114 t = time.localtime()
00115 launch_time_stamp = time.strftime("%Y%m%d_%H_%M_%S", t)
00116 command = 'rosbag play ' + base_dir + 'careobot_st_jla_ca.bag'
00117
00118 data_krakens = [
00119 JointStateDataKraken(base_dir + 'careobot_st_jla_ca_joint_state_data_' + launch_time_stamp + '.csv'),
00120 ObstacleDistanceDataKraken(base_dir + 'careobot_st_jla_ca_obst_dist_data_' + launch_time_stamp + '.csv'),
00121 TwistDataKraken(base_dir + 'careobot_st_jla_ca_twist_data_' + launch_time_stamp + '.csv'),
00122 JointVelocityDataKraken(base_dir + 'careobot_st_jla_ca_joint_vel_data_' + launch_time_stamp + '.csv'),
00123 FrameTrackingDataKraken(base_dir + 'careobot_st_jla_ca_frame_tracking_data_' + launch_time_stamp + '.csv', root_frame, chain_tip_link, tracking_frame), ]
00124
00125 init_pos()
00126 init_dyn_recfg()
00127
00128 status_open = True
00129 for data_kraken in data_krakens:
00130 status_open = status_open and data_kraken.open()
00131 if status_open:
00132 rospy.loginfo('Subscribers started for data collection ... \nPress CTRL+C to stop program and write data to the file.')
00133
00134 traj_marker_command = 'rosrun cob_twist_controller debug_trajectory_marker_node __ns:=' + rospy.get_namespace()
00135 traj_marker_pid = subprocess.Popen(traj_marker_command, stdin = subprocess.PIPE, shell = True)
00136 pid = subprocess.Popen(command, stdin = subprocess.PIPE, cwd = base_dir, shell = True)
00137
00138
00139 time.sleep(1.5)
00140
00141 if traj_marker_pid.poll() is not None:
00142 rospy.logerr("traj_marker_pid returned code. Aborting ...")
00143 pid.send_signal(subprocess.signal.SIGINT)
00144 pid.kill()
00145
00146 traj_marker_pid.send_signal(subprocess.signal.CTRL_C_EVENT)
00147 traj_marker_pid.kill()
00148 exit()
00149
00150 time.sleep(0.5)
00151
00152 rate = rospy.Rate(10)
00153 try:
00154 while not rospy.is_shutdown():
00155 rate.sleep()
00156 if(pid.poll() is not None):
00157 rospy.loginfo('Bag file finished stop script.')
00158 break
00159
00160 except (KeyboardInterrupt, SystemExit) as e:
00161 rospy.loginfo('KeyboardInterrupt / SystemExit: ' + str(e))
00162
00163 for data_kraken in data_krakens:
00164 data_kraken.writeAllData()
00165 except rospy.ROSInterruptException as e:
00166 rospy.logwarn('ROSInterruptException: ' + str(e))
00167 except:
00168 rospy.logerr('Else exception.')
00169 else:
00170 for data_kraken in data_krakens:
00171 data_kraken.writeAllData()
00172
00173 try:
00174
00175 pid.kill()
00176 pid.send_signal(subprocess.signal.SIGINT)
00177 except Exception as e:
00178 rospy.logerr('Failed to stop rosbag play due to exception: ' + str(e))
00179 try:
00180 traj_marker_pid.kill()
00181 traj_marker_pid.send_signal(subprocess.signal.SIGINT)
00182 except Exception as e:
00183 rospy.logerr('Failed to stop debug_trajectory_marker_node due to exception: ' + str(e))
00184 else:
00185 rospy.logerr('Failed to open DataKraken files.')
00186
00187
00188
00189
00190
00191
00192