[Navigation] improve keyboard teleopration that allows to move the ro…

…bot with uniform linear motion

aerial_robot_base/scripts/keyboard_command.py

@@ -1,13 +1,34 @@
 #!/usr/bin/env python
-import rospy
 
+import sys, select, termios, tty
+
+import rospy
 from std_msgs.msg import Empty
-from std_msgs.msg import Int8
-from std_msgs.msg import UInt16
-from std_msgs.msg import UInt8
+from aerial_robot_msgs.msg import FlightNav
 import rosgraph
 
-import sys, select, termios, tty
+msg = """
+Instruction:
+
+---------------------------
+
+r:  arming motor (please do before takeoff)
+t:  takeoff
+l:  land
+f:  force landing
+h:  halt (force stop motor)
+
+     q           w           e           [
+(turn left)  (forward)  (turn right)  (move up)
+
+     a           s           d           ]
+(move left)  (backward) (move right) (move down)
+
+
+Please don't have caps lock on.
+CTRL+c to quit
+---------------------------
+"""
 
 def getKey():
         tty.setraw(sys.stdin.fileno())
@@ -16,10 +37,14 @@ def getKey():
         termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
         return key
 
+def printMsg(msg, msg_len = 50):
+        print(msg.ljust(msg_len) + "\r", end="")
+
 if __name__=="__main__":
         settings = termios.tcgetattr(sys.stdin)
         rospy.init_node("keyboard_command")
         robot_ns = rospy.get_param("~robot_ns", "");
+        print(msg)
 
         if not robot_ns:
                 master = rosgraph.Master('/rostopic')
@@ -39,43 +64,86 @@ def getKey():
         start_pub = rospy.Publisher(ns + '/start', Empty, queue_size=1)
         takeoff_pub = rospy.Publisher(ns + '/takeoff', Empty, queue_size=1)
         force_landing_pub = rospy.Publisher(ns + '/force_landing', Empty, queue_size=1)
-        ctrl_mode_pub = rospy.Publisher(ns + '/ctrl_mode', Int8, queue_size=1)
-        motion_start_pub = rospy.Publisher('task_start', Empty, queue_size=1)
+        nav_pub = rospy.Publisher(robot_ns + '/uav/nav', FlightNav, queue_size=1)
 
+        xy_vel   = rospy.get_param("xy_vel", 0.2)
+        z_vel    = rospy.get_param("z_vel", 0.2)
+        yaw_vel  = rospy.get_param("yaw_vel", 0.2)
+
+        motion_start_pub = rospy.Publisher('task_start', Empty, queue_size=1)
 
-        #the way to write publisher in python
-        comm=Int8()
-        gain=UInt16()
         try:
                 while(True):
+                        nav_msg = FlightNav()
+                        nav_msg.control_frame = FlightNav.WORLD_FRAME
+                        nav_msg.target = FlightNav.COG
+
                         key = getKey()
-                        print("the key value is {}".format(ord(key)))
-                        # takeoff and landing
+
+                        msg = ""
+
                         if key == 'l':
                                 land_pub.publish(Empty())
-                                #for hydra joints
+                                msg = "send land command"
                         if key == 'r':
                                 start_pub.publish(Empty())
-                                #for hydra joints
+                                msg = "send motor-arming command"
                         if key == 'h':
                                 halt_pub.publish(Empty())
-                                 #for hydra joints
+                                msg = "send motor-disarming (halt) command"
                         if key == 'f':
                                 force_landing_pub.publish(Empty())
+                                msg = "send force landing command"
                         if key == 't':
                                 takeoff_pub.publish(Empty())
-                        if key == 'u':
-                                stair_pub.publish(Empty())
+                                msg = "send takeoff command"
                         if key == 'x':
                                 motion_start_pub.publish()
-                        if key == 'v':
-                                comm.data = 1
-                                ctrl_mode_pub.publish(comm)
-                        if key == 'p':
-                                comm.data = 0
-                                ctrl_mode_pub.publish(comm)
+                                msg = "send task-start command"
+                        if key == 'w':
+                                nav_msg.pos_xy_nav_mode = FlightNav.VEL_MODE
+                                nav_msg.target_vel_x = xy_vel
+                                nav_pub.publish(nav_msg)
+                                msg = "send +x vel command"
+                        if key == 's':
+                                nav_msg.pos_xy_nav_mode = FlightNav.VEL_MODE
+                                nav_msg.target_vel_x = -xy_vel
+                                nav_pub.publish(nav_msg)
+                                msg = "send -x vel command"
+                        if key == 'a':
+                                nav_msg.pos_xy_nav_mode = FlightNav.VEL_MODE
+                                nav_msg.target_vel_y = xy_vel
+                                nav_pub.publish(nav_msg)
+                                msg = "send +y vel command"
+                        if key == 'd':
+                                nav_msg.pos_xy_nav_mode = FlightNav.VEL_MODE
+                                nav_msg.target_vel_y = -xy_vel
+                                nav_pub.publish(nav_msg)
+                                msg = "send -y vel command"
+                        if key == 'q':
+                                nav_msg.yaw_nav_mode = FlightNav.VEL_MODE
+                                nav_msg.target_omega_z = yaw_vel
+                                nav_pub.publish(nav_msg)
+                                msg = "send +yaw vel command"
+                        if key == 'e':
+                                nav_msg.yaw_nav_mode = FlightNav.VEL_MODE
+                                nav_msg.target_omega_z = -yaw_vel
+                                msg = "send -yaw vel command"
+                                nav_pub.publish(nav_msg)
+                        if key == '[':
+                                nav_msg.pos_z_nav_mode = FlightNav.VEL_MODE
+                                nav_msg.target_vel_z = z_vel
+                                nav_pub.publish(nav_msg)
+                                msg = "send +z vel command"
+                        if key == ']':
+                                nav_msg.pos_z_nav_mode = FlightNav.VEL_MODE
+                                nav_msg.target_vel_z = -z_vel
+                                nav_pub.publish(nav_msg)
+                                msg = "send -z vel command"
                         if key == '\x03':
                                 break
+
+                        printMsg(msg)
                         rospy.sleep(0.001)
 
         except Exception as e: