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()