<s> '''<STR_LIT>''' <EOL> import sys <EOL> import copy <EOL> import rospy <EOL> import math <EOL> from brics_actuator . msg import JointPositions , JointValue <EOL> from std_msgs . msg import String <EOL> from tf . transformations import quaternion_from_euler <EOL> import moveit_commander <EOL> import moveit_msgs . msg <EOL> import geometry_msgs . msg <EOL> from trajectory_msgs . msg import JointTrajectory , JointTrajectoryPoint <EOL> import control_msgs . msg <EOL> import actionlib <EOL> from std_msgs . msg import String <EOL> '''<STR_LIT>''' <EOL> arm_1_topic_name = "<STR_LIT>" <EOL> arm_1_msg_type = JointPositions <EOL> grip_1_topic_name = "<STR_LIT>" <EOL> grip_1_msg_type = JointPositions <EOL> arm_2_topic_name = "<STR_LIT>" <EOL> arm_2_msg_type = JointPositions <EOL> grip_2_topic_name = "<STR_LIT>" <EOL> grip_2_msg_type = JointPositions <EOL> joint_uri_1 = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] <EOL> joint_uri_2 = [ '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' , '<STR_LIT>' ] <EOL> def make_grip_msg ( opening_mm , joint_uri ) : <EOL> left = opening_mm / <NUM_LIT> <EOL> right = opening_mm / <NUM_LIT> <EOL> jp = JointPositions ( ) <EOL> jvl = JointValue ( ) <EOL> jvr = JointValue ( ) <EOL> jvl . joint_uri = joint_uri [ <NUM_LIT:5> ] <EOL> jvl . unit = '<STR_LIT:m>' <EOL> jvl . value = left <EOL> jvr . joint_uri = joint_uri [ <NUM_LIT:6> ] <EOL> jvr . unit = '<STR_LIT:m>' <EOL> jvr . value = right <EOL> jp . positions . append ( copy . deepcopy ( jvl ) ) <EOL> jp . positions . append ( copy . deepcopy ( jvr ) ) <EOL> return jp <EOL> def make_arm_msg ( arm_js , joint_uri ) : <EOL> A1 = arm_js [ <NUM_LIT:0> ] <EOL> A2 = arm_js [ <NUM_LIT:1> ] <EOL> A3 = arm_js [ <NUM_LIT:2> ] <EOL> A4 = arm_js [ <NUM_LIT:3> ] <EOL> A5 = arm_js [ <NUM_LIT:4> ] <EOL> jp = JointPositions ( ) <EOL> jv1 = JointValue ( ) <EOL> jv2 = JointValue ( ) <EOL> jv3 = JointValue ( ) <EOL> jv4 = JointValue ( ) <EOL> jv5 = JointValue ( ) <EOL> jv1 . joint_uri = joint_uri [ <NUM_LIT:0> ] <EOL> jv1 . unit = '<STR_LIT>' <EOL> jv1 . value = A1 <EOL> jv2 . joint_uri = joint_uri [ <NUM_LIT:1> ] <EOL> jv2 . unit = '<STR_LIT>' <EOL> jv2 . value = A2 <EOL> jv3 . joint_uri = joint_uri [ <NUM_LIT:2> ] <EOL> jv3 . unit = '<STR_LIT>' <EOL> jv3 . value = A3 <EOL> jv4 . joint_uri = joint_uri [ <NUM_LIT:3> ] <EOL> jv4 . unit = '<STR_LIT>' <EOL> jv4 . value = A4 <EOL> jv5 . joint_uri = joint_uri [ <NUM_LIT:4> ] <EOL> jv5 . unit = '<STR_LIT>' <EOL> jv5 . value = A5 <EOL> jp . positions . append ( copy . deepcopy ( jv1 ) ) <EOL> jp . positions . append ( copy . deepcopy ( jv2 ) ) <EOL> jp . positions . append ( copy . deepcopy ( jv3 ) ) <EOL> jp . positions . append ( copy . deepcopy ( jv4 ) ) <EOL> jp . positions . append ( copy . deepcopy ( jv5 ) ) <EOL> return jp <EOL> def move_gripper_1 ( opening_mm ) : <EOL> gripper_1_command_publisher = rospy . Publisher ( grip_1_topic_name , grip_1_msg_type , queue_size = <NUM_LIT:5> ) <EOL> r = rospy . Rate ( <NUM_LIT:1> ) <EOL> jp = make_grip_msg ( opening_mm , joint_uri_1 ) <EOL> time = rospy . get_rostime ( ) <EOL> t0 = time . secs <EOL> while not rospy . is_shutdown ( ) : <EOL> rospy . loginfo ( "<STR_LIT>" ) <EOL> gripper_1_command_publisher . publish ( jp ) <EOL> r . sleep ( ) <EOL> time = rospy . get_rostime ( ) <EOL> t1 = time . secs <EOL> if ( t1 - t0 ) > <NUM_LIT:1> : <EOL> break <EOL> def move_gripper_2 ( opening_mm ) : <EOL> gripper_2_command_publisher = rospy . Publisher ( grip_2_topic_name , grip_2_msg_type , queue_size = <NUM_LIT:5> ) <EOL> r = rospy . Rate ( <NUM_LIT:1> ) <EOL> jp = make_grip_msg ( opening_mm , joint_uri_2 ) <EOL> time = rospy . get_rostime ( ) <EOL> t0 = time . secs <EOL> while not rospy . is_shutdown ( ) : <EOL> rospy . loginfo ( "<STR_LIT>" ) <EOL> gripper_2_command_publisher . publish ( jp ) <EOL> r . sleep ( ) <EOL> time = rospy . get_rostime ( ) <EOL> t1 = time . secs <EOL> if ( t1 - t0 ) > <NUM_LIT:1> : <EOL> break <EOL> def move_grippers ( opening_1_mm , opening_2_mm ) : <EOL> gripper_1_command_publisher = rospy . Publisher ( grip_1_topic_name , grip_1_msg_type , queue_size = <NUM_LIT:5> ) <EOL> gripper_2_command_publisher = rospy . Publisher ( grip_2_topic_name , grip_2_msg_type , queue_size = <NUM_LIT:5> ) <EOL> r = rospy . Rate ( <NUM_LIT:1> ) <EOL> jp1 = make_grip_msg ( opening_1_mm , joint_uri_1 ) <EOL> jp2 = make_grip_msg ( opening_2_mm , joint_uri_2 ) <EOL> time = rospy . get_rostime ( ) <EOL> t0 = time . secs <EOL> while not rospy . is_shutdown ( ) : <EOL> rospy . loginfo ( "<STR_LIT>" ) <EOL> gripper_1_command_publisher . publish ( jp1 ) <EOL> gripper_2_command_publisher . publish ( jp2 ) <EOL> r . sleep ( ) <EOL> time = rospy . get_rostime ( ) <EOL> t1 = time . secs <EOL> if ( t1 - t0 ) > <NUM_LIT:1> : <EOL> break <EOL> def move_arm_1 ( cmd ) : <EOL> arm_command_publisher = rospy . Publisher ( arm_1_topic_name , arm_1_msg_type , queue_size = <NUM_LIT:5> ) <EOL> r = rospy . Rate ( <NUM_LIT:1> ) <EOL> arm_cmd = make_arm_msg ( cmd , joint_uri_1 ) <EOL> time = rospy . get_rostime ( ) <EOL> t0 = time . secs <EOL> while not rospy . is_shutdown ( ) : <EOL> rospy . loginfo ( "<STR_LIT>" ) <EOL> arm_command_publisher . publish ( arm_cmd ) <EOL> r . sleep ( ) <EOL> time = rospy . get_rostime ( ) <EOL> t1 = time . secs <EOL> if ( t1 - t0 ) > <NUM_LIT:1> : <EOL> break <EOL> def move_arm_2 ( cmd ) : <EOL> arm_command_publisher = rospy . Publisher ( arm_2_topic_name , arm_2_msg_type , queue_size = <NUM_LIT:5> ) <EOL> r = rospy . Rate ( <NUM_LIT:1> ) <EOL> arm_cmd = make_arm_msg ( cmd , joint_uri_2 ) <EOL> time = rospy . get_rostime ( ) <EOL> t0 = time . secs <EOL> while not rospy . is_shutdown ( ) : <EOL> rospy . loginfo ( "<STR_LIT>" ) <EOL> arm_command_publisher . publish ( arm_cmd ) <EOL> r . sleep ( ) <EOL> time = rospy . get_rostime ( ) <EOL> t1 = time . secs <EOL> if ( t1 - t0 ) > <NUM_LIT:1> : <EOL> break <EOL> def move_arms ( cmd_1 , cmd_2 ) : <EOL> arm_1_command_publisher = rospy . Publisher ( arm_1_topic_name , arm_1_msg_type , queue_size = <NUM_LIT:5> ) <EOL> arm_2_command_publisher = rospy . Publisher ( arm_2_topic_name , arm_2_msg_type , queue_size = <NUM_LIT:5> ) <EOL> r = rospy . Rate ( <NUM_LIT:1> ) <EOL> arm_1_cmd = make_arm_msg ( cmd_1 , joint_uri_1 ) <EOL> arm_2_cmd = make_arm_msg ( cmd_2 , joint_uri_2 ) <EOL> time = rospy . get_rostime ( ) <EOL> t0 = time . secs <EOL> while not rospy . is_shutdown ( ) : <EOL> rospy . loginfo ( "<STR_LIT>" ) <EOL> arm_1_command_publisher . publish ( arm_1_cmd ) <EOL> arm_2_command_publisher . publish ( arm_2_cmd ) <EOL> r . sleep ( ) <EOL> time = rospy . get_rostime ( ) <EOL> t1 = time . secs <EOL> if ( t1 - t0 ) > <NUM_LIT:1> : <EOL> break <EOL> def youbot_exec ( arm_1_topic_name , arm_1_msg_type , grip_1_topic_name , grip_1_msg_type , arm_2_topic_name , arm_2_msg_type , grip_2_topic_name , grip_2_msg_type ) : <EOL> rospy . init_node ( '<STR_LIT>' , <EOL> anonymous = False ) <EOL> prepick_rob2 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> pick_rob2 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> cand = [ <NUM_LIT> , <NUM_LIT:1.0> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> cand1 = [ <NUM_LIT> , <NUM_LIT:1.0> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> cand2 = [ <NUM_LIT> , <NUM_LIT:1.0> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> trans1 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> pretrans2 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> trans2 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> posttrans2 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> premach2 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> mach2 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> preload = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> load = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> premach1 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> mach1 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> prepick_rob1 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> pick_rob1 = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> ready_tight_spot = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> pretight_spot_place = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> tight_spot = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> posttight_spot_pickA = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> posttight_spot_pickB = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> posttight_spot_place = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> opening = <NUM_LIT> <EOL> closing = <NUM_LIT:9> <EOL> SPECIAL = [ <NUM_LIT> , <NUM_LIT> , - <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] <EOL> while not rospy . is_shutdown ( ) : <EOL> move_arms ( cand1 , cand ) <EOL> rospy . sleep ( <NUM_LIT> ) <EOL> move_arms ( cand2 , cand ) <EOL> rospy . sleep ( <NUM_LIT> ) <EOL> continue <EOL> if __name__ == '<STR_LIT:__main__>' : <EOL> try : <EOL> youbot_exec ( arm_1_topic_name , <EOL> arm_1_msg_type , <EOL> grip_1_topic_name , <EOL> grip_1_msg_type , <EOL> arm_2_topic_name , <EOL> arm_2_msg_type , <EOL> grip_2_topic_name , <EOL> grip_2_msg_type ) <EOL> except rospy . ROSInterruptException : <EOL> pass </s>
