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