if rospy.has_param('root_frame'):
root_frame = rospy.get_param('root_frame')
else:
rospy.logwarn('Could not find parameter root_frame.')
exit(-3)
t = time.localtime()
launch_time_stamp = time.strftime("%Y%m%d_%H_%M_%S", t)
command = 'rosbag play ' + base_dir + 'arm_left_to_head_internal_motions.bag'
data_krakens = [
JointStateDataKraken(base_dir + 'internal_motion_joint_state_data_' + launch_time_stamp + '.csv'),
ObstacleDistanceDataKraken(base_dir + 'internal_motion_obst_dist_data_' + launch_time_stamp + '.csv'),
TwistDataKraken(base_dir + 'internal_motion_twist_data_' + launch_time_stamp + '.csv'),
JointVelocityDataKraken(base_dir + 'internal_motion_joint_vel_data_' + launch_time_stamp + '.csv'),
FrameTrackingDataKraken(base_dir + 'internal_motion_frame_tracking_data_' + launch_time_stamp + '.csv', root_frame, chain_tip_link, tracking_frame), ]
init_pos()
init_dyn_recfg()
# exit()
status_open = True
for data_kraken in data_krakens:
status_open = status_open and data_kraken.open()
if status_open:
rospy.loginfo('Subscribers started for data collection ... \nPress CTRL+C to stop program and write data to the file.')
pid = subprocess.Popen(command, stdin = subprocess.PIPE, cwd = base_dir, shell = True)
command = 'rosbag play ' + base_dir + 'careobot_st_jla_ca.bag'
data_krakens = [
JointStateDataKraken(base_dir +
'careobot_GPM_jla_ca_joint_state_data_' +
launch_time_stamp + '.csv'),
ObstacleDistanceDataKraken(base_dir +
'careobot_GPM_jla_ca_obst_dist_data_' +
launch_time_stamp + '.csv'),
TwistDataKraken(base_dir + 'careobot_GPM_jla_ca_twist_data_' +
launch_time_stamp + '.csv'),
JointVelocityDataKraken(base_dir +
'careobot_GPM_jla_ca_joint_vel_data_' +
launch_time_stamp + '.csv'),
FrameTrackingDataKraken(
base_dir + 'careobot_GPM_jla_ca_frame_tracking_data_' +
launch_time_stamp + '.csv', root_frame, chain_tip_link,
tracking_frame),
]
init_pos()
init_dyn_recfg()
status_open = True
for data_kraken in data_krakens:
status_open = status_open and data_kraken.open()
if status_open:
rospy.loginfo(
'Subscribers started for data collection ... \nPress CTRL+C to stop program and write data to the file.'
)
traj_marker_command = 'rosrun cob_twist_controller debug_trajectory_marker_node __ns:=' + rospy.get_namespace(
t = time.localtime()
launch_time_stamp = time.strftime("%Y%m%d_%H_%M_%S", t)
command = 'rosbag play ' + base_dir + 'raw31_sca.bag'
data_krakens = [
JointStateDataKraken(base_dir + 'raw31_sca_joint_state_data_' +
launch_time_stamp + '.csv'),
ObstacleDistanceDataKraken(base_dir + 'raw31_sca_obst_dist_data_' +
launch_time_stamp + '.csv'),
TwistDataKraken(base_dir +
'raw31_sca_twist_controller_commanded_twist_data_' +
launch_time_stamp + '.csv'),
JointVelocityDataKraken(base_dir + 'raw31_sca_joint_vel_data_' +
launch_time_stamp + '.csv'),
FrameTrackingDataKraken(
base_dir + 'raw31_sca_frame_tracking_data_' + launch_time_stamp +
'.csv', root_frame, chain_tip_link, tracking_frame)
]
init_pos()
init_dyn_recfg()
status_open = True
for data_kraken in data_krakens:
status_open = status_open and data_kraken.open()
if status_open:
rospy.loginfo(
'Subscribers started for data collection ... \nPress CTRL+C to stop program and write data to the file.'
)
pid = subprocess.Popen(command,