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 + '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()
rospy.logwarn('Could not find parameter frame_tracker/tracking_frame.')
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 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 + '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:
# 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()