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 + '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()
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 + 'base_active_arm_right.bag'
data_krakens = [
JointStateDataKraken(base_dir + 'careobot_base_joint_state_data_' +
launch_time_stamp + '.csv'),
ObstacleDistanceDataKraken(base_dir + 'careobot_base_obst_dist_data_' +
launch_time_stamp + '.csv'),
TwistDataKraken(
base_dir + 'careobot_base_twist_data_' + launch_time_stamp +
'.csv', True, False),
TwistDataKraken(
base_dir + 'careobot_base_twist_controller_commanded_twist_data_' +
launch_time_stamp + '.csv', False, False),
JointVelocityDataKraken(base_dir + 'careobot_base_joint_vel_data_' +
launch_time_stamp + '.csv'),
OdometryDataKraken(base_dir + 'careobot_base_odometry_data_' +
launch_time_stamp + '.csv'),
]
init_pos()
init_dyn_recfg()
exit()
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 + 'careobot_st_jla_ca_sphere.bag'
# command = 'rosbag play -u 10 ' + base_dir + 'careobot_st_jla_ca_sphere.bag'
data_krakens = [
JointStateDataKraken(base_dir + 'joint_state_data_' +
launch_time_stamp + '.csv'),
ObstacleDistanceDataKraken(base_dir + 'obst_dist_data_' +
launch_time_stamp + '.csv'),
TwistDataKraken(base_dir + 'twist_data_' + launch_time_stamp + '.csv'),
JointVelocityDataKraken(base_dir + 'joint_vel_data_' +
launch_time_stamp + '.csv'),
FrameTrackingDataKraken(
base_dir + '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(
exit(-2)
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:
# has to be startet with ns param: rosrun cob_twist_controller collect_twist_control_eval_data.py __ns:=arm_right
if __name__ == "__main__":
rospy.init_node("collect_twist_control_eval_data")
base_dir = '/home/fxm-mb/Scripts/Tests/FinalTests/'
if rospy.has_param('~base_dir'):
base_dir = rospy.get_param('~base_dir')
else:
rospy.logwarn(
'Could not find parameter ~base_dir. Using default base_dir: ' +
base_dir)
data_krakens = [
JointStateDataKraken(base_dir + 'joint_state_data.csv'),
ObstacleDistanceDataKraken(base_dir + 'obst_dist_data.csv'),
TwistDataKraken(base_dir + 'twist_data.csv'),
JointVelocityDataKraken(base_dir + 'joint_vel_data.csv'),
]
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.'
)
rate = rospy.Rate(10)
try:
while not rospy.is_shutdown():
rate.sleep()
except (KeyboardInterrupt, SystemExit) as e:
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 + '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: