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