[Ninja][Motion][WIP] workaround to use trajectory generation rather t…

…han simple nav.

robots/ninja/launch/assembly_tasks/assembly_motion.launch

@@ -1,7 +1,9 @@
 <?xml version="1.0"?>
 <launch>
   <arg name = "module_ids" default=""/>
+  <arg name = "real_machine" default="true"/>
   <node pkg="ninja" type="assembly_motion.py" name="" output="screen" >
     <param name = "module_ids" value="$(arg module_ids)"/>
+    <param name = "real_machine" value="$(arg real_machine)"/>
   </node>
 </launch>

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

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

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