diff --git a/robots/ninja/CMakeLists.txt b/robots/ninja/CMakeLists.txt index 3d6f46429..021a983bf 100644 --- a/robots/ninja/CMakeLists.txt +++ b/robots/ninja/CMakeLists.txt @@ -32,7 +32,10 @@ catkin_package( INCLUDE_DIRS include LIBRARIES ninja_robot_model ninja_controller ninja_navigation CATKIN_DEPENDS aerial_robot_control aerial_robot_model aerial_robot_msgs pluginlib roscpp gimbalrotor diagnostic_msgs std_msgs geometry_msgs message_runtime beetle -) + ) + +configure_file(script/roslaunch ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_BIN_DESTINATION}/roslaunch COPYONLY) + include_directories( include @@ -67,6 +70,6 @@ install(DIRECTORY script USE_SOURCE_PERMISSIONS ) -# catkin_install_python(PROGRAMS script/ninja/kondo_control.py -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +catkin_install_python(PROGRAMS script/roslaunch + DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) diff --git a/robots/ninja/config/rviz_config b/robots/ninja/config/rviz_config index 8a1e19ea3..08d7e3322 100644 --- a/robots/ninja/config/rviz_config +++ b/robots/ninja/config/rviz_config @@ -13,7 +13,10 @@ Panels: - /RobotModel3 - /RobotModel3/Links1 - /RobotModel4 - Splitter Ratio: 0.6529411673545837 + - /WrenchStamped1 + - /WrenchStamped2 + - /WrenchStamped3 + Splitter Ratio: 0.6541849970817566 Tree Height: 740 - Class: rviz/Selection Name: Selection @@ -237,6 +240,10 @@ Visualization Manager: Value: false ninja1/rotor_parent3: Value: false + ninja1/target_cog_pose: + Value: true + ninja1/target_com_pose: + Value: true ninja1/thrust1: Value: false ninja1/thrust2: @@ -287,6 +294,10 @@ Visualization Manager: Value: false ninja2/rotor_parent3: Value: false + ninja2/target_cog_pose: + Value: true + ninja2/target_com_pose: + Value: true ninja2/thrust1: Value: false ninja2/thrust2: @@ -337,6 +348,10 @@ Visualization Manager: Value: false ninja3/rotor_parent3: Value: false + ninja3/target_cog_pose: + Value: true + ninja3/target_com_pose: + Value: true ninja3/thrust1: Value: false ninja3/thrust2: @@ -347,56 +362,6 @@ Visualization Manager: Value: false ninja3/yaw_dock_link: Value: false - ninja4/battery_1: - Value: false - ninja4/battery_2: - Value: false - ninja4/center_of_moving: - Value: false - ninja4/cog: - Value: true - ninja4/contact_point: - Value: false - ninja4/dummy_arm: - Value: false - ninja4/fc: - Value: false - ninja4/gimbal_link1: - Value: false - ninja4/gimbal_link2: - Value: false - ninja4/gimbal_link3: - Value: false - ninja4/main_body: - Value: false - ninja4/pitch_connect_point: - Value: false - ninja4/pitch_dock_link: - Value: false - ninja4/root: - Value: false - ninja4/rotor_arm1: - Value: false - ninja4/rotor_arm2: - Value: false - ninja4/rotor_arm3: - Value: false - ninja4/rotor_parent1: - Value: false - ninja4/rotor_parent2: - Value: false - ninja4/rotor_parent3: - Value: false - ninja4/thrust1: - Value: false - ninja4/thrust2: - Value: false - ninja4/thrust3: - Value: false - ninja4/yaw_connect_point: - Value: false - ninja4/yaw_dock_link: - Value: false world: Value: false Marker Alpha: 1 @@ -443,6 +408,10 @@ Visualization Manager: {} ninja1/yaw_connect_point: {} + ninja1/target_cog_pose: + {} + ninja1/target_com_pose: + {} ninja2/root: ninja2/cog: ninja2/center_of_moving: @@ -479,6 +448,10 @@ Visualization Manager: {} ninja2/yaw_connect_point: {} + ninja2/target_cog_pose: + {} + ninja2/target_com_pose: + {} ninja3/root: ninja3/cog: ninja3/center_of_moving: @@ -515,42 +488,10 @@ Visualization Manager: {} ninja3/yaw_connect_point: {} - ninja4/root: - ninja4/cog: - ninja4/center_of_moving: - {} - ninja4/contact_point: - {} - ninja4/main_body: - ninja4/battery_1: - {} - ninja4/battery_2: - {} - ninja4/fc: - {} - ninja4/pitch_dock_link: - ninja4/pitch_connect_point: - {} - ninja4/rotor_arm1: - ninja4/gimbal_link1: - ninja4/rotor_parent1: - ninja4/thrust1: - {} - ninja4/rotor_arm2: - ninja4/gimbal_link2: - ninja4/rotor_parent2: - ninja4/thrust2: - {} - ninja4/rotor_arm3: - ninja4/gimbal_link3: - ninja4/rotor_parent3: - ninja4/thrust3: - {} - ninja4/yaw_dock_link: - ninja4/dummy_arm: - {} - ninja4/yaw_connect_point: - {} + ninja3/target_cog_pose: + {} + ninja3/target_com_pose: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -827,128 +768,57 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - battery_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - battery_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - contact_point: - Alpha: 1 - Show Axes: false - Show Trail: false - dummy_arm: - Alpha: 1 - Show Axes: false - Show Trail: false - dummy_gripper: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - dummy_palm: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fc: - Alpha: 1 - Show Axes: false - Show Trail: false - gimbal_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gimbal_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gimbal_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - main_body: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - pitch_connect_point: - Alpha: 1 - Show Axes: false - Show Trail: false - pitch_dock_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - root: - Alpha: 1 - Show Axes: false - Show Trail: false - rotor_arm1: - Alpha: 1 - Show Axes: false - Show Trail: false - rotor_arm2: - Alpha: 1 - Show Axes: false - Show Trail: false - rotor_arm3: - Alpha: 1 - Show Axes: false - Show Trail: false - rotor_parent1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rotor_parent2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - rotor_parent3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - thrust1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - thrust2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - thrust3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - yaw_connect_point: - Alpha: 1 - Show Axes: false - Show Trail: false - yaw_dock_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Name: RobotModel Robot Description: ninja4/robot_description TF Prefix: ninja4 Update Interval: 0 Value: true Visual Enabled: true + - Alpha: 0.5 + Arrow Width: 0.30000001192092896 + Class: rviz/WrenchStamped + Enabled: true + Force Arrow Scale: 0.5 + Force Color: 204; 51; 51 + Hide Small Values: true + History Length: 1 + Name: WrenchStamped + Queue Size: 10 + Topic: /ninja1/internal_wrench + Torque Arrow Scale: 1 + Torque Color: 204; 204; 51 + Unreliable: true + Value: true + - Alpha: 0.5 + Arrow Width: 0.30000001192092896 + Class: rviz/WrenchStamped + Enabled: true + Force Arrow Scale: 0.5 + Force Color: 204; 51; 51 + Hide Small Values: true + History Length: 1 + Name: WrenchStamped + Queue Size: 10 + Topic: /ninja2/internal_wrench + Torque Arrow Scale: 1 + Torque Color: 204; 204; 51 + Unreliable: false + Value: true + - Alpha: 0.5 + Arrow Width: 0.30000001192092896 + Class: rviz/WrenchStamped + Enabled: true + Force Arrow Scale: 0.5 + Force Color: 204; 51; 51 + Hide Small Values: true + History Length: 1 + Name: WrenchStamped + Queue Size: 10 + Topic: /ninja3/internal_wrench + Torque Arrow Scale: 1 + Torque Color: 204; 204; 51 + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -977,7 +847,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.9822680950164795 + Distance: 2.723729372024536 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -985,17 +855,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: -0.0981794074177742 - Y: 0.19234484434127808 - Z: 1.15351140499115 + X: -0.09440077841281891 + Y: -0.12096423655748367 + Z: 0.8789822459220886 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.380398154258728 + Pitch: 0.17039868235588074 Target Frame: - Yaw: 4.798608779907227 + Yaw: 4.708604335784912 Saved: ~ Window Geometry: Displays: diff --git a/robots/ninja/script/ninja/assembly_api.py b/robots/ninja/script/ninja/assembly_api.py index 7d814f90a..a5b539401 100755 --- a/robots/ninja/script/ninja/assembly_api.py +++ b/robots/ninja/script/ninja/assembly_api.py @@ -9,7 +9,7 @@ from spinal.msg import DesireCoord from geometry_msgs.msg import PoseStamped from diagnostic_msgs.msg import KeyValue -from ninja.kondo_control_api import KondoControl +from ninja.dynamixel_control_api import DynamixelControl from ninja.utils import coordTransformer import numpy as np import tf @@ -27,13 +27,10 @@ class StandbyState(smach.State): def __init__(self, robot_name = 'ninja1', robot_id = 1, - male_servo_id = 5, + male_servo_id = 8, female_servo_id = 6, real_machine = True, - unlock_servo_angle_male = 7000, - lock_servo_angle_male = 8800, - unlock_servo_angle_female = 11000, - lock_servo_angle_female = 5600, + standby_servo_angle_male = 2500, leader = 'ninja2', leader_id = 2, airframe_size = 0.818258, @@ -46,7 +43,8 @@ def __init__(self, roll_tol = 0.08, pitch_tol = 0.08, yaw_tol = 0.08, - root_fc_dis = [-0.04695,0,0.0369], + # root_fc_dis = [-0.04695,0,0.0369], + root_fc_dis = [0.0,0,0.0], attach_dir = -1.0, approach_mode = 'nav', run_rate = 40): @@ -58,10 +56,7 @@ def __init__(self, self.male_servo_id = male_servo_id self.female_servo_id = female_servo_id self.real_machine = real_machine - self.unlock_servo_angle_male = unlock_servo_angle_male - self.lock_servo_angle_male = lock_servo_angle_male - self.unlock_servo_angle_female = unlock_servo_angle_female - self.lock_servo_angle_female = lock_servo_angle_female + self.standby_servo_angle_male = standby_servo_angle_male self.leader = leader self.leader_id = leader_id self.airframe_size = airframe_size @@ -93,11 +88,11 @@ def __init__(self, self.follower_att_pub = rospy.Publisher(self.robot_name+"/final_target_baselink_rot", DesireCoord, queue_size=10) if(self.attach_dir < 0): self.follower_docking_pub = rospy.Publisher(self.robot_name+"/docking_cmd", Bool, queue_size=10) - self.kondo_servo = KondoControl(self.leader,self.leader_id,self.female_servo_id,self.real_machine) + self.dynamixel_servo = DynamixelControl(self.robot_name,self.robot_id,self.male_servo_id,self.real_machine) else: self.follower_docking_pub = rospy.Publisher(self.leader+"/docking_cmd", Bool, queue_size=10) - self.kondo_servo = KondoControl(self.robot_name,self.robot_id,self.female_servo_id,self.real_machine) - + self.dynamixel_servo = DynamixelControl(self.leader,self.leader_id,self.male_servo_id,self.real_machine) + # subscriber self.emergency_stop_sub = rospy.Subscriber("/emergency_assembly_interuption",Empty,self.emergencyCb) @@ -118,7 +113,7 @@ def __init__(self, def execute(self, userdata): if not self.follower_male_mech_activated: if self.real_machine: - self.kondo_servo.sendTargetAngle(self.lock_servo_angle_female) + self.dynamixel_servo.sendTargetAngle(self.standby_servo_angle_male) else: self.docking_msg.data = True self.follower_docking_pub.publish(self.docking_msg) @@ -126,21 +121,21 @@ def execute(self, userdata): # calculate current follower position in leader coordinate #TODO: determine leader namespace dynamically try: - leader_from_world = self.listener.lookupTransform('/world', self.leader+'/root', rospy.Time(0)) + leader_from_world = self.listener.lookupTransform('/world', self.leader+'/fc', rospy.Time(0)) except (tf.LookupException, tf_from_neighboring.ConnectivityException, tf.ExtrapolationException): return 'in_process' try: - follower_from_leader = self.listener.lookupTransform(self.leader+'/root', self.robot_name+'/root', rospy.Time(0)) + follower_from_leader = self.listener.lookupTransform(self.leader+'/fc', self.robot_name+'/fc', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return 'in_process' # set target odom in leader coordinate #TODO: determine leader namespace dynamically - self.br.sendTransform((self.target_offset[0] + self.root_fc_dis[0], self.target_offset[1] +self.root_fc_dis[1], self.target_offset[2] + self.root_fc_dis[2]), + self.br.sendTransform((self.target_offset[0], self.target_offset[1], self.target_offset[2]), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "follower_target_odom", - self.leader+"/root") + self.leader+"/fc") # convert target position from leader coord to world coord try: @@ -150,11 +145,16 @@ def execute(self, userdata): target_pos = homo_transformed_target_odom[0] target_att = tf.transformations.euler_from_quaternion(homo_transformed_target_odom[1]) + self.br.sendTransform((target_pos[0], target_pos[1], target_pos[2]), + tf.transformations.quaternion_from_euler(target_att[0], target_att[1], target_att[2]), + rospy.Time.now(), + "follower_target_fc", + "/world") + pos_error = np.array(self.target_offset - follower_from_leader[0]) if(pos_error[0] * self.attach_dir > 0): pos_error[0] = 0.0 att_error = np.array([0,0,0])-tf.transformations.euler_from_quaternion(follower_from_leader[1]) - rospy.loginfo(pos_error) #check if pos and att error are within the torrelance if np.all(np.less(np.abs(pos_error),self.pos_error_tol)) and np.all(np.less(np.abs(att_error),self.att_error_tol)): return 'done' @@ -173,16 +173,16 @@ def execute(self, userdata): nav_msg_follower.target_pos_z = target_pos[2] nav_msg_follower.target_yaw = target_att[2] # Trajectory(x,y,z and yaw) - target_pos_cog = self.coordTransformer.posTransform('fc','cog',target_pos) - traj_msg_follower = PoseStamped() - traj_msg_follower.header.stamp = rospy.Time.now() - traj_msg_follower.pose.position.x = target_pos_cog[0] - traj_msg_follower.pose.position.y = target_pos_cog[1] - traj_msg_follower.pose.position.z = target_pos_cog[2] - traj_msg_follower.pose.orientation.x = homo_transformed_target_odom[1][0] - traj_msg_follower.pose.orientation.y = homo_transformed_target_odom[1][1] - traj_msg_follower.pose.orientation.z = homo_transformed_target_odom[1][2] - traj_msg_follower.pose.orientation.w = homo_transformed_target_odom[1][3] + # traj_msg_follower = PoseStamped() + # target_pos_cog = self.coordTransformer.posTransform('root','cog',target_pos) + # traj_msg_follower.header.stamp = rospy.Time.now() + # traj_msg_follower.pose.position.x = target_pos_cog[0] + # traj_msg_follower.pose.position.y = target_pos_cog[1] + # traj_msg_follower.pose.position.z = target_pos_cog[2] + # traj_msg_follower.pose.orientation.x = homo_transformed_target_odom[1][0] + # traj_msg_follower.pose.orientation.y = homo_transformed_target_odom[1][1] + # traj_msg_follower.pose.orientation.z = homo_transformed_target_odom[1][2] + # traj_msg_follower.pose.orientation.w = homo_transformed_target_odom[1][3] if(self.approach_mode == 'nav'): self.follower_nav_pub.publish(nav_msg_follower) @@ -217,13 +217,9 @@ class ApproachState(smach.State): def __init__(self, robot_name = 'ninja1', robot_id = 1, - male_servo_id = 5, + male_servo_id = 8, female_servo_id = 6, real_machine = True, - unlock_servo_angle_male = 7000, - lock_servo_angle_male = 8800, - unlock_servo_angle_female = 11000, - lock_servo_angle_female = 5600, leader = 'ninja2', leader_id = 2, airframe_size = 0.818258, @@ -236,7 +232,8 @@ def __init__(self, roll_tol = 0.08, pitch_tol = 0.08, yaw_tol = 0.08, - root_fc_dis = [-0.04695,0,0.0369], + # root_fc_dis = [-0.04695,0,0.0369], + root_fc_dis = [0.0,0,0.0], x_danger_thre = 0.02, y_danger_thre = 0.1, z_danger_thre = 0.1, @@ -254,10 +251,6 @@ def __init__(self, self.male_servo_id = male_servo_id self.female_servo_id = female_servo_id self.real_machine = real_machine - self.unlock_servo_angle_male = unlock_servo_angle_male - self.lock_servo_angle_male = lock_servo_angle_male - self.unlock_servo_angle_female = unlock_servo_angle_female - self.lock_servo_angle_female = lock_servo_angle_female self.leader = leader self.leader_id = leader_id self.airframe_size = airframe_size @@ -293,6 +286,13 @@ def __init__(self, self.follower_traj_pub = rospy.Publisher(self.robot_name+"/target_pose", PoseStamped, queue_size=10) self.follower_att_pub = rospy.Publisher(self.robot_name+"/final_target_baselink_rot", DesireCoord, queue_size=10) + if(self.attach_dir < 0): + self.follower_docking_pub = rospy.Publisher(self.robot_name+"/docking_cmd", Bool, queue_size=10) + self.dynamixel_servo = DynamixelControl(self.robot_name,self.robot_id,self.male_servo_id,self.real_machine) + else: + self.follower_docking_pub = rospy.Publisher(self.leader+"/docking_cmd", Bool, queue_size=10) + self.dynamixel_servo = DynamixelControl(self.leader,self.leader_id,self.male_servo_id,self.real_machine) + # subscriber self.emergency_stop_sub = rospy.Subscriber("/emergency_assembly_interuption",Empty,self.emergencyCb) @@ -314,10 +314,12 @@ def __init__(self, self.att_danger_thre = np.array([self.roll_danger_thre, self.pitch_danger_thre, self.yaw_danger_thre]) def execute(self, userdata): + if self.real_machine: + self.dynamixel_servo.torqueEnable(0) # calculate now follower position in leader coordinate #TODO: determine leader namespace dynamically try: - follower_from_leader = self.listener.lookupTransform(self.leader+'/root', self.robot_name+'/root', rospy.Time(0)) + follower_from_leader = self.listener.lookupTransform(self.leader+'/fc', self.robot_name+'/fc', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return 'in_process' @@ -327,7 +329,7 @@ def execute(self, userdata): tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "follower_target_odom", - self.leader+"/root") + self.leader+"/fc") # convert target position from leader coord to world coord try: @@ -362,7 +364,7 @@ def execute(self, userdata): nav_msg_follower.target_pos_z = target_pos[2] nav_msg_follower.target_yaw = target_att[2] # Trajectory(x,y,z and yaw) - target_pos_cog = self.coordTransformer.posTransform('fc','cog',target_pos) + target_pos_cog = self.coordTransformer.posTransform('root','cog',target_pos) traj_msg_follower = PoseStamped() traj_msg_follower.header.stamp = rospy.Time.now() traj_msg_follower.pose.position.x = target_pos_cog[0] @@ -405,13 +407,9 @@ class AssemblyState(smach.State): def __init__(self, robot_name = 'ninja1', robot_id = 1, - male_servo_id = 5, + male_servo_id = 8, female_servo_id = 6, real_machine = True, - unlock_servo_angle_male = 7000, - lock_servo_angle_male = 8800, - unlock_servo_angle_female = 11000, - lock_servo_angle_female = 5600, leader = 'ninja2', leader_id = 2, attach_dir = -1.0): @@ -422,10 +420,6 @@ def __init__(self, self.male_servo_id = male_servo_id self.female_servo_id = female_servo_id self.real_machine = real_machine - self.unlock_servo_angle_male = unlock_servo_angle_male - self.lock_servo_angle_male = lock_servo_angle_male - self.unlock_servo_angle_female = unlock_servo_angle_female - self.lock_servo_angle_female = lock_servo_angle_female self.leader = leader self.leader_id = leader_id self.attach_dir = attach_dir @@ -438,10 +432,10 @@ def __init__(self, self.follower_nav_pub = rospy.Publisher(self.robot_name+"/uav/nav", FlightNav, queue_size=10) self.leader_nav_pub = rospy.Publisher(self.leader+"/uav/nav", FlightNav, queue_size=10) self.assembly_nav_pub = rospy.Publisher("/assembly/uav/nav", FlightNav, queue_size=10) - if(self.attach_dir < 0): - self.kondo_servo = KondoControl(self.robot_name,self.robot_id,self.male_servo_id,self.real_machine) - else: - self.kondo_servo = KondoControl(self.leader,self.leader_id,self.male_servo_id,self.real_machine) + # if(self.attach_dir < 0): + # self.dynamixel_servo = DynamixelControl(self.robot_name,self.robot_id,self.male_servo_id,self.real_machine) + # else: + # self.dynamixel_servo = DynamixelControl(self.leader,self.leader_id,self.male_servo_id,self.real_machine) self.flag_pub = rospy.Publisher('/' + self.robot_name + '/assembly_flag', KeyValue, queue_size = 1) self.flag_pub_leader = rospy.Publisher('/' + self.leader + '/assembly_flag', KeyValue, queue_size = 1) @@ -458,9 +452,9 @@ def __init__(self, def execute(self, userdata): if self.emergency_flag: return 'emergency' - if self.real_machine: - self.kondo_servo.sendTargetAngle(self.lock_servo_angle_male) - time.sleep(1.0) + # if self.real_machine: + # self.dynamixel_servo.sendTargetAngle(self.lock_servo_angle_male) + # time.sleep(1.0) if not self.real_machine: rospy.sleep(1.0) self.nav_msg.pos_xy_nav_mode= 6 diff --git a/robots/ninja/script/ninja/disassembly_api.py b/robots/ninja/script/ninja/disassembly_api.py index a4fc442fa..07f7b898b 100755 --- a/robots/ninja/script/ninja/disassembly_api.py +++ b/robots/ninja/script/ninja/disassembly_api.py @@ -8,7 +8,7 @@ from aerial_robot_msgs.msg import FlightNav from spinal.msg import ServoControlCmd from diagnostic_msgs.msg import KeyValue -from ninja.kondo_control_api import KondoControl +from ninja.dynamixel_control_api import DynamixelControl import numpy as np import tf @@ -28,12 +28,10 @@ class SwitchState(smach.State): def __init__(self, robot_name = 'ninja1', robot_id = 1, - male_servo_id = 5, + male_servo_id = 8, real_machine = False, - unlock_servo_angle_male = 7000, - lock_servo_angle_male = 8300, - unlock_servo_angle_female = 11000, - lock_servo_angle_female = 5600, + unlock_servo_angle_male = 4000, + default_servo_angle_male = 2500, neighboring = 'ninja2', neighboring_id = 2, female_servo_id = 6, @@ -45,20 +43,18 @@ def __init__(self, self.male_servo_id = male_servo_id self.real_machine = real_machine self.unlock_servo_angle_male = unlock_servo_angle_male - self.lock_servo_angle_male = lock_servo_angle_male - self.unlock_servo_angle_female = unlock_servo_angle_female - self.lock_servo_angle_female = lock_servo_angle_female + self.default_servo_angle_male = default_servo_angle_male self.neighboring = neighboring self.neighboring_id = neighboring_id self.female_servo_id = female_servo_id self.separate_dir = separate_dir if(separate_dir > 0): - self.kondo_servo = KondoControl(self.robot_name,self.robot_id,self.female_servo_id,self.real_machine) - self.kondo_servo_neighboring = KondoControl(self.neighboring,self.neighboring_id,self.male_servo_id,self.real_machine) + # self.dynamixel_servo = DynamixelControl(self.robot_name,self.robot_id,self.female_servo_id,self.real_machine) + self.dynamixel_servo_neighboring = DynamixelControl(self.neighboring,self.neighboring_id,self.male_servo_id,self.real_machine) else: - self.kondo_servo = KondoControl(self.robot_name,self.robot_id,self.male_servo_id,self.real_machine) - self.kondo_servo_neighboring = KondoControl(self.neighboring,self.neighboring_id,self.female_servo_id,self.real_machine) + self.dynamixel_servo = DynamixelControl(self.robot_name,self.robot_id,self.male_servo_id,self.real_machine) + # self.dynamixel_servo_neighboring = DynamixelControl(self.neighboring,self.neighboring_id,self.female_servo_id,self.real_machine) self.flag_pub = rospy.Publisher('/' + self.robot_name + '/assembly_flag', KeyValue, queue_size = 1) self.flag_pub_neighboring = rospy.Publisher('/' + self.neighboring + '/assembly_flag', KeyValue, queue_size = 1) @@ -77,20 +73,15 @@ def execute(self, userdata): self.flag_msg.key = str(self.robot_id) self.flag_msg.value = '0' self.flag_pub.publish(self.flag_msg) - rospy.loginfo("ok-1") if self.real_machine: if(self.separate_dir > 0): - rospy.loginfo("ok0") - self.kondo_servo.sendTargetAngle(self.unlock_servo_angle_female) - self.kondo_servo_neighboring.sendTargetAngle(self.unlock_servo_angle_male) + self.dynamixel_servo_neighboring.sendTargetAngle(self.unlock_servo_angle_male) else: - rospy.loginfo("ok-1") - self.kondo_servo.sendTargetAngle(self.unlock_servo_angle_male) - self.kondo_servo_neighboring.sendTargetAngle(self.unlock_servo_angle_female) + self.dynamixel_servo.sendTargetAngle(self.unlock_servo_angle_male) else: self.docking_msg.data = False self.docking_pub.publish(self.docking_msg) - time.sleep(5.0) + time.sleep(3.0) return 'done' class SeparateState(smach.State): @@ -98,7 +89,7 @@ class SeparateState(smach.State): def __init__(self, robot_name = 'ninja1', robot_id = 1, - separate_vel = -0.5, + separate_vel = -0.2, neighboring = 'ninja2', target_dist_from_neighboring = 1.25): diff --git a/robots/ninja/script/ninja/dynamixel_control_api.py b/robots/ninja/script/ninja/dynamixel_control_api.py new file mode 100755 index 000000000..77d94cf69 --- /dev/null +++ b/robots/ninja/script/ninja/dynamixel_control_api.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python + +from spinal.msg import ServoControlCmd, ServoTorqueCmd +from sensor_msgs.msg import JointState + +import rospy +import numpy as np +import math +import time + +class DynamixelControl(): + def __init__(self,robot_name,robot_id,servo_id,real_machine): + + self.DYNAMIXEL_SERVO_POSITION_MIN = 0 + self.DYNAMIXEL_SERVO_POSITION_MAX = 4096 + self.DYNAMIXEL_SERVO_ANGLE_MIN = 0 + self.DYNAMIXEL_SERVO_ANGLE_MAX = 2*math.pi + self.real_machine = real_machine + self.robot_name = robot_name + self.robot_id = robot_id + self.servo_id = servo_id + self.servo_name = 'gimbal' + str(robot_id) + + self.dx_pos_pub = rospy.Publisher('/' + self.robot_name +'/servo/target_states', ServoControlCmd, queue_size=1) + self.dx_torque_pub = rospy.Publisher('/' + self.robot_name +'/servo/torque_enable', ServoTorqueCmd, queue_size=1) + self.gimbal_pub = rospy.Publisher('/' + self.robot_name + '/gimbals_ctrl', JointState, queue_size=1) + + self.servo_pos_msg = ServoControlCmd() + self.servo_pos_msg.index = [self.servo_id] + + self.servo_torque_msg = ServoTorqueCmd() + self.servo_torque_msg.index = [self.servo_id] + + self.joint_state_cmd_msg = JointState() + self.joint_state_cmd_msg.name = [self.servo_name] + + time.sleep(0.5) + + def rad2DynamixelPosConv(self,angle): + dynamixel_pos = int((self.DYNAMIXEL_SERVO_POSITION_MAX-self.DYNAMIXEL_SERVO_POSITION_MIN)*(-angle - self.DYNAMIXEL_SERVO_ANGLE_MIN)/(self.DYNAMIXEL_SERVO_ANGLE_MAX - self.DYNAMIXEL_SERVO_ANGLE_MIN) + self.DYNAMIXEL_SERVO_POSITION_MIN) + return dynamixel_pos + + def sendTargetAngle(self,target_angle): + self.servo_pos_msg.angles = [target_angle] + self.joint_state_cmd_msg.position = [target_angle] + if self.real_machine: + self.dx_pos_pub.publish(self.servo_pos_msg) + else: + self.gimbal_pub.publish(self.joint_state_cmd_msg) + + def torqueEnable(self,torque_enable): + self.servo_torque_msg.torque_enable = [torque_enable] + self.dx_torque_pub.publish(self.servo_torque_msg) + + + diff --git a/robots/ninja/script/roslaunch b/robots/ninja/script/roslaunch new file mode 100755 index 000000000..cd390f1c8 --- /dev/null +++ b/robots/ninja/script/roslaunch @@ -0,0 +1,193 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import re +import socket +import sys +from copy import deepcopy + +import roslaunch +import roslaunch.loader +from roslaunch.xmlloader import XmlLoader +from roslaunch.core import printlog, printerrlog, _printerrlog_handlers +from roslaunch.remoteprocess import SSHChildROSLaunchProcess + +roslaunch.xmlloader._is_default = {'true': True, 'scope': False, 'parent': False, 'false': False, 'never': False } +roslaunch.xmlloader._assignable = {'true': True, 'scope': True, 'parent': True, 'false': True, 'never': False } +# maps machine 'default' attribute to whether this sets the default_machine in +# the loader context +roslaunch.xmlloader._is_context_default = {'true': True, 'scope': True, 'parent': True, 'false': False, 'never': False } + +orig_node_tag = XmlLoader._node_tag + + +def patched_node_tag(self, tag, context, ros_config, default_machine, *args, **kwargs): + result = orig_node_tag(self, tag, context, ros_config, default_machine, *args, **kwargs) + if isinstance(result, roslaunch.Node): + machine, = self.opt_attrs(tag, context, ('machine',)) + if not machine: + if context.default_machine: + machine = context.default_machine.name + elif default_machine: + machine = default_machine.name + result.machine_name = machine + return result + + +XmlLoader._node_tag = patched_node_tag + +orig_machine_tag = XmlLoader._machine_tag + + +def patched_machine_tag(self, tag, context, *args, **kwargs): + m, is_default = orig_machine_tag(self, tag, context, *args, **kwargs) + default, = self.opt_attrs(tag, context, ('default',)) + default = (default or 'false').lower() + + try: + is_context_default = roslaunch.xmlloader._is_context_default[default] + except KeyError as e: + raise roslaunch.xmlloader.XmlParseException("Invalid value for 'attribute': %s" % default) + + if is_context_default: + if default == 'parent': + context.parent.default_machine = m + else: + context.default_machine = m + + return m, is_default + + +XmlLoader._machine_tag = patched_machine_tag + +orig_loader_context_init = roslaunch.loader.LoaderContext.__init__ + + +def patched_loader_context_init(self, *args, **kwargs): + default_machine = None + if 'default_machine' in kwargs: + default_machine = kwargs['default_machine'] + del kwargs['default_machine'] + orig_loader_context_init(self, *args, **kwargs) + self.default_machine = default_machine + + +roslaunch.loader.LoaderContext.__init__ = patched_loader_context_init + + +def patched_loader_context_child(self, ns): + if ns: + if ns[0] == '/': # global (discouraged) + child_ns = ns + elif ns == roslaunch.loader.PRIV_NAME: # ~name + # private names can only be scoped privately or globally + child_ns = roslaunch.loader.PRIV_NAME + else: + child_ns = roslaunch.loader.ns_join(self.ns, ns) + else: + child_ns = self.ns + return roslaunch.loader.LoaderContext(child_ns, self.filename, parent=self, + params=self.params, env_args=self.env_args[:], + resolve_dict=deepcopy(self.resolve_dict), + arg_names=self.arg_names[:], include_resolve_dict=self.include_resolve_dict, + default_machine=self.default_machine) + + +roslaunch.loader.LoaderContext.child = patched_loader_context_child + +SSHChildROSLaunchProcess.line_buf_info = "" +SSHChildROSLaunchProcess.line_buf_err = "" + + +def printlog_raw(msg, file=sys.stderr): + for h in _printerrlog_handlers: + try: + h(msg) + except: + pass + try: + print(msg, file=sys.stderr) + except: + pass + + +ansi_escape = re.compile(r'\x1B(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])') + + +def patched_is_alive(self): + """ + :returns: ``True`` if the process is alive. is_alive needs to be + called periodically as it drains the SSH buffer, ``bool`` + """ + if self.started and not self.ssh: + return False + elif not self.started: + return True # not started is equivalent to alive in our logic + s = self.ssherr + s.channel.settimeout(0) + try: + # drain the pipes + data = s.read(128) + if not len(data): + self.is_dead = True + return False + # #2012 il8n: ssh *should* be UTF-8, but often isn't + # (e.g. Japan) + data = data.decode('utf-8') + lines = data.split("\n") + if len(lines) > 0: + if len(self.line_buf_err) > 0: + lines[0] = self.line_buf_err + lines[0] + self.line_buf_err = "" + if data[-1] != "\n": + self.line_buf_err = lines[-1] + lines = lines[:-1] + for line in lines: + if line.startswith("\033") and len(line) >= 5: + printlog_raw(line[:(5 if line[4] == "m" else 4)] + "remote[%s]:\033[0m %s" % (self.name, line), file=sys.stderr) + elif len(ansi_escape.sub('', line).strip()) > 0: + printerrlog("remote[%s]: %s" % (self.name, line)) + except socket.timeout: + pass + except IOError: + return False + except UnicodeDecodeError: + # #2012: soft fail, printing is not essential. This occurs + # with older machines that don't send utf-8 over ssh + pass + + s = self.sshout + s.channel.settimeout(0) + try: + # drain the pipes + data = s.read(128) + if not len(data): + self.is_dead = True + return False + # #2012 il8n: ssh *should* be UTF-8, but often isn't + # (e.g. Japan) + data = data.decode('utf-8') + lines = data.split("\n") + if len(lines) > 0: + if len(self.line_buf_info) > 0: + lines[0] = self.line_buf_info + lines[0] + self.line_buf_info = "" + if data[-1] != "\n": + self.line_buf_info = lines[-1] + lines = lines[:-1] + for line in lines: + if line.startswith("\033") and len(line) >= 5: + printlog_raw(line[:(5 if line[4] == "m" else 4)] + "remote[%s]:\033[0m %s" % (self.name, line), file=sys.stdout) + elif len(ansi_escape.sub('', line).strip()) > 0: + printerrlog("remote[%s]: %s" % (self.name, line)) + except socket.timeout: + pass + except IOError: + return False + return True + +SSHChildROSLaunchProcess.is_alive = patched_is_alive + + +roslaunch.main() diff --git a/robots/ninja/script/tasks/disassembly_motion.py b/robots/ninja/script/tasks/disassembly_motion.py index f37559bad..2cfe8d2a1 100755 --- a/robots/ninja/script/tasks/disassembly_motion.py +++ b/robots/ninja/script/tasks/disassembly_motion.py @@ -25,7 +25,7 @@ def main(self): direction = 1 if target_follower_id > target_leader_id else -1 with sub_sm: smach.StateMachine.add('SwitchState'+ motion_prefix, - SwitchState(robot_name = 'ninja'+str(target_follower_id), robot_id = target_follower_id, neighboring = 'ninja'+str(target_leader_id), neighboring_id = target_leader_id, separate_dir = direction), + SwitchState(robot_name = 'ninja'+str(target_follower_id), robot_id = target_follower_id,real_machine=self.real_machine, neighboring = 'ninja'+str(target_leader_id), neighboring_id = target_leader_id, separate_dir = direction), transitions={'done':'SeparateState'+motion_prefix}) smach.StateMachine.add('SeparateState'+motion_prefix,