diff --git a/robots/ninja/launch/assembly_tasks/assembly_motion.launch b/robots/ninja/launch/assembly_tasks/assembly_motion.launch index 00afd5372..659758de2 100644 --- a/robots/ninja/launch/assembly_tasks/assembly_motion.launch +++ b/robots/ninja/launch/assembly_tasks/assembly_motion.launch @@ -1,7 +1,9 @@ + + diff --git a/robots/ninja/script/ninja/assembly_api.py b/robots/ninja/script/ninja/assembly_api.py index 003b6df08..3e1ae7cd6 100755 --- a/robots/ninja/script/ninja/assembly_api.py +++ b/robots/ninja/script/ninja/assembly_api.py @@ -10,6 +10,7 @@ from geometry_msgs.msg import PoseStamped from diagnostic_msgs.msg import KeyValue from ninja.kondo_control_api import KondoControl +from ninja.utils import coordTransformer import numpy as np import tf @@ -46,7 +47,9 @@ def __init__(self, pitch_tol = 0.08, yaw_tol = 0.08, root_fc_dis = [-0.04695,0,0.0369], - attach_dir = -1.0): + attach_dir = -1.0, + approach_mode = 'nav', + run_rate = 40): smach.State.__init__(self, outcomes=['done','in_process','emergency']) @@ -73,6 +76,8 @@ def __init__(self, self.yaw_tol = yaw_tol self.root_fc_dis = root_fc_dis self.attach_dir = attach_dir + self.approach_mode = approach_mode + self.run_rate = rospy.Rate(run_rate) # flags self.emergency_flag = False @@ -84,6 +89,7 @@ def __init__(self, # publisher self.follower_nav_pub = rospy.Publisher(self.robot_name+"/uav/nav", FlightNav, queue_size=10) + 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) @@ -98,6 +104,9 @@ def __init__(self, # messages self.docking_msg = Bool() + # utils + self.coordTransformer = coordTransformer(self.robot_name) + # position offset while StandbyState if(self.attach_dir < 0): self.target_offset = np.array([-(self.airframe_size + self.x_offset), self.y_offset, self.z_offset]) @@ -152,7 +161,7 @@ def execute(self, userdata): elif self.emergency_flag: return 'emergency' else: - # x,y,z and yaw + # Nav(x,y,z and yaw) nav_msg_follower = FlightNav() nav_msg_follower.target = 0 nav_msg_follower.control_frame = 0 @@ -163,13 +172,32 @@ def execute(self, userdata): nav_msg_follower.target_pos_y = target_pos[1] nav_msg_follower.target_pos_z = target_pos[2] nav_msg_follower.target_yaw = target_att[2] - self.follower_nav_pub.publish(nav_msg_follower) + # 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] + + if(self.approach_mode == 'nav'): + self.follower_nav_pub.publish(nav_msg_follower) + elif(self.approach_mode == 'trajectory'): + self.follower_traj_pub.publish(traj_msg_follower) + else: + rospy.logerr("Invalid approach mode is setted") + return 'fail' # roll and pitch link_rot_follower = DesireCoord() link_rot_follower.roll = target_att[0] link_rot_follower.pitch = target_att[1] # self.follower_att_pub.publish(link_rot_follower) + self.run_rate.sleep() return 'in_process' def emergencyCb(self,msg): @@ -215,7 +243,9 @@ def __init__(self, roll_danger_thre = 0.35, pitch_danger_thre = 0.35, yaw_danger_thre = 0.35, - attach_dir = -1.0): + attach_dir = -1.0, + approach_mode = 'nav', + run_rate = 40): smach.State.__init__(self, outcomes=['done','in_process','fail','emergency']) @@ -248,6 +278,8 @@ def __init__(self, self.pitch_danger_thre = pitch_danger_thre self.yaw_danger_thre = yaw_danger_thre self.attach_dir = attach_dir + self.approach_mode = approach_mode + self.run_rate = rospy.Rate(run_rate) # flags self.emergency_flag = False @@ -258,10 +290,14 @@ def __init__(self, # publisher self.follower_nav_pub = rospy.Publisher(self.robot_name+"/uav/nav", FlightNav, queue_size=10) + 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) # subscriber - self.emergency_stop_sub = rospy.Subscriber("/emergency_assembly_interuption",Empty,self.emergencyCb) + self.emergency_stop_sub = rospy.Subscriber("/emergency_assembly_interuption",Empty,self.emergencyCb) + + # utils + self.coordTransformer = coordTransformer(self.robot_name) # position offset while ApproachState if(self.attach_dir < 0): @@ -314,7 +350,7 @@ def execute(self, userdata): elif np.all(np.greater(np.abs(pos_error),self.pos_danger_thre)) or np.all(np.greater(np.abs(att_error),self.att_danger_thre)): return 'fail' else: - # x,y,z and yaw + # Nav(x,y,z and yaw) nav_msg_follower = FlightNav() nav_msg_follower.target = 0 nav_msg_follower.control_frame = 0 @@ -325,14 +361,31 @@ def execute(self, userdata): nav_msg_follower.target_pos_y = target_pos[1] nav_msg_follower.target_pos_z = target_pos[2] nav_msg_follower.target_yaw = target_att[2] - self.follower_nav_pub.publish(nav_msg_follower) + # 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] + if(self.approach_mode == 'nav'): + self.follower_nav_pub.publish(nav_msg_follower) + elif(self.approach_mode == 'trajectory'): + self.follower_traj_pub.publish(traj_msg_follower) + else: + rospy.logerr("Invalid approach mode is setted") + return 'fail' # roll and pitch link_rot_follower = DesireCoord() link_rot_follower.roll = target_att[0] link_rot_follower.pitch = target_att[1] # self.follower_att_pub.publish(link_rot_follower) - + self.run_rate.sleep() return 'in_process' def emergencyCb(self,msg): diff --git a/robots/ninja/script/ninja/utils.py b/robots/ninja/script/ninja/utils.py new file mode 100755 index 000000000..9c3e0d985 --- /dev/null +++ b/robots/ninja/script/ninja/utils.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python + +import rospy +from geometry_msgs.msg import PoseStamped +import numpy as np +import tf + +class coordTransformer(): + def __init__(self, + robot_name = 'ninja1'): + self.robot_name = robot_name + self.listener = tf.TransformListener() + + def posTransform(self, curr_coord, dist_coord, target_pos_curr): + try: + self.listener.waitForTransform(self.robot_name+'/'+curr_coord, self.robot_name+'/'+dist_coord, rospy.Time(), rospy.Duration(5.0)) + + (trans, rot) = self.listener.lookupTransform(self.robot_name+'/'+curr_coord, self.robot_name+'/'+dist_coord, rospy.Time(0)) + + transform_matrix = tf.transformations.quaternion_matrix(rot) + transform_matrix[:3, 3] = trans + + target_pos_curr_4d = np.array(list(target_pos_curr) + [1.0]) + + target_pos_dist_4d = np.dot(transform_matrix, target_pos_curr_4d) + + target_pos_dist = target_pos_dist_4d[:3] + + return target_pos_dist + except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: + rospy.logerr("Transform error: %s", e) + return None + +if __name__ == '__main__': + try: + rospy.init_node("coordTransformer") + trans = coordTransformer(robot_name = 'ninja1'); + target_base = [1,0,0] + rospy.loginfo(trans.posTransform('fc', 'cog', target_base)) + rospy.spin() + except rospy.ROSInterruptException: pass diff --git a/robots/ninja/script/tasks/assembly_motion.py b/robots/ninja/script/tasks/assembly_motion.py index f31c06573..f3e710ee6 100755 --- a/robots/ninja/script/tasks/assembly_motion.py +++ b/robots/ninja/script/tasks/assembly_motion.py @@ -8,8 +8,9 @@ from ninja.assembly_api import * class AssemblyDemo(): - def __init__(self,module_ids): + def __init__(self,module_ids,real_machine): self.module_ids = module_ids + self.real_machine = real_machine rospy.loginfo(self.module_ids) def main(self): sm_top = smach.StateMachine(outcomes=['succeeded','interupted']) @@ -21,16 +22,17 @@ def main(self): target_leader_id = self.module_ids[i-1] motion_prefix = str(target_follower_id) + '_' + str(target_leader_id) sub_sm = smach.StateMachine(outcomes=['succeeded_'+motion_prefix,'interupted_'+motion_prefix]) + direction = 1 if target_follower_id > target_leader_id else -1 with sub_sm: smach.StateMachine.add('StandbyState'+ motion_prefix, - StandbyState(robot_name = 'ninja'+str(target_follower_id), robot_id = target_follower_id, leader = 'ninja'+str(target_leader_id), leader_id = target_leader_id, attach_dir = -1.0), + StandbyState(robot_name = 'ninja'+str(target_follower_id), robot_id = target_follower_id, leader = 'ninja'+str(target_leader_id), leader_id = target_leader_id, attach_dir = direction, real_machine = self.real_machine), transitions={'done':'ApproachState' + motion_prefix, 'in_process': 'StandbyState'+motion_prefix, 'emergency':'interupted_'+motion_prefix}) smach.StateMachine.add('ApproachState' + motion_prefix, - ApproachState(robot_name = 'ninja'+str(target_follower_id), robot_id = target_follower_id, leader = 'ninja'+str(target_leader_id), leader_id = target_leader_id, attach_dir = -1.0), + ApproachState(robot_name = 'ninja'+str(target_follower_id), robot_id = target_follower_id, leader = 'ninja'+str(target_leader_id), leader_id = target_leader_id, attach_dir = direction, real_machine = self.real_machine), transitions={'done':'AssemblyState'+ motion_prefix, 'in_process':'ApproachState'+motion_prefix, 'fail':'StandbyState'+motion_prefix, 'emergency':'interupted_'+motion_prefix}) - smach.StateMachine.add('AssemblyState'+motion_prefix, AssemblyState(robot_name = 'ninja1', robot_id = 1, leader = 'ninja2', leader_id = 2, attach_dir = -1.0), + smach.StateMachine.add('AssemblyState'+motion_prefix, AssemblyState(robot_name = 'ninja'+str(target_follower_id), robot_id = target_follower_id, leader = 'ninja'+str(target_leader_id), leader_id = target_leader_id, attach_dir = direction, real_machine = self.real_machine), transitions={'done':'succeeded_'+motion_prefix, 'emergency':'interupted_'+motion_prefix}) if(i == len(self.module_ids)-1): smach.StateMachine.add('SUB'+str(i), @@ -49,12 +51,13 @@ def main(self): if __name__ == '__main__': rospy.init_node("assembly_motion") modules_str = rospy.get_param("module_ids", default="") + rm = rospy.get_param("real_machine", default=True) modules = [] if(modules_str): modules = [int(x) for x in modules_str.split(',')] else: rospy.loginfo("No module ID is designated!") try: - assembly_motion = AssemblyDemo(module_ids = modules); + assembly_motion = AssemblyDemo(module_ids = modules, real_machine=rm); assembly_motion.main() except rospy.ROSInterruptException: pass