Merge branch 'develop' into 'master'

Curses UI

See merge request cdf2020/tools/devkit/joy-teleop!2

nodes/joy_teleop.py

 #!/usr/bin/env python3
 
-import threading
 import rospy
 from geometry_msgs.msg import Twist
 from std_msgs.msg import Bool
@@ -8,8 +7,7 @@ from sensor_msgs.msg import Joy
 from std_srvs.srv import SetBool
 from mccd_msgs.msg import Status
 import time
-
-from math import pi
+import curses
 
 ############# DUALSHOCK 3 ##############
 
@@ -69,18 +67,21 @@ class JoyTeleopNode():
             # shutdown teleop
             self.forceQuit = True
 
-        self.angular_vel = data.axes[A_LJOYH] * coefAng
-        self.linear_vel = data.axes[A_LJOYV] * coefLin
+        lin_vel = data.axes[A_LJOYV] * coefLin
+        ang_vel = data.axes[A_LJOYH] * coefAng
+
+        if lin_vel != self.lin_speed_cmd or ang_vel != self.ang_speed_cmd:
+            self.lin_speed_cmd = lin_vel
+            self.ang_speed_cmd = ang_vel
+            self.update_info = True
 
-    def __init__(self):
-        self.linear_vel = 0
-        self.angular_vel = 0
-        self.last_linear_vel = -10
-        self.last_angular_vel = -10
+    def __init__(self, stdscr):
+        self.lin_speed_cmd = 0
+        self.ang_speed_cmd = 0
 
         self.status = None
 
-        self.print_info = False
+        self.update_info = False
 
         self.forceQuit = False
         self.publisher = rospy.Publisher('cmd_vel', Twist, queue_size=1)
@@ -88,7 +89,10 @@ class JoyTeleopNode():
         self.pwm_state_sub = rospy.Subscriber(
             '/mccd/status', Status, self.status_cb)
 
-        print("Waiting for /mccd/enable_pwm service...")
+        stdscr.clear()
+        stdscr.addstr(0, 0, "Waiting for /mccd/enable_pwm service...")
+        stdscr.refresh()
+
         rospy.wait_for_service('/mccd/enable_pwm')
         self.change_pwm = rospy.ServiceProxy('/mccd/enable_pwm', SetBool)
 
@@ -97,47 +101,66 @@ class JoyTeleopNode():
         self.lastTimeUpdate = time.time()
         self.minUpdatePeriod = 0.3  # resend data every x sec.
 
-        print("""
-LEFT JOY     change speed
-
-START        toggle pwm
-
-PS       shutdown teleop
-                """)
         while not rospy.is_shutdown():
             if self.forceQuit == True:
                 break
 
-            if self.last_angular_vel != self.angular_vel:
-                self.last_angular_vel = self.angular_vel
-                self.print_info = True
+            if self.update_info:
+                self.print_info(stdscr)
+
+            t = time.time()
+
+            if self.lastTimeUpdate + self.minUpdatePeriod < t:
+                self.send_vel(self.lin_speed_cmd, self.ang_speed_cmd)
+                self.lastTimeUpdate = t
+
+        curses.endwin()
+        rospy.loginfo("Shutting down joy_teleop")
+
+    def print_info(self, stdscr):
+
+        stdscr.clear()
+
+        stdscr.addstr(0, 0, """
+         JOY TELEOP        _
+            ,---.          U
+           ;     \         ;
+       .==\\"/==.  `-.___.-'
+      ((+) .  .:)
+      |`.-(o)-.'|
+      \/  \_/  \/
+        """)
 
-            if self.last_linear_vel != self.linear_vel:
-                self.last_linear_vel = self.linear_vel
-                self.print_info = True
+        stdscr.addstr(9, 0, "{:^10} {:^10s} {:^10s}".format(
+            "LEFT JOY", "START", "PS"))
+        stdscr.addstr(10, 0, "{:^10s} {:^10s} {:^10s}".format(
+            "Speed", "Toggle PWM", "Shutdown"))
 
-            if self.print_info:
-                strToPrint = "PwmState: \x1b[1;32;40m{}\x1b[0m\tn"
-                strToPrint += "linear_vel: \x1b[1;32;40m{:.3f}\x1b[0m\t"
-                strToPrint += "angular_vel: \x1b[1;32;40m{:.3f}\x1b[0m"
-                print("\033[A                             \033[A", flush=True)
-                print(strToPrint.format(self.status_to_string(self.status),
-                                        self.linear_vel,
-                                        self.angular_vel),
-                      flush=True)
+        curses.init_pair(1, curses.COLOR_WHITE, curses.COLOR_GREEN)
 
-                self.print_info = False
+        stdscr.addstr(12, 0, "Status:")
+        stdscr.addstr(12, 12, self.status_to_string(
+            self.status), curses.A_BOLD | curses.color_pair(1))
 
-            if self.lastTimeUpdate + self.minUpdatePeriod < time.time():
-                self.lastTimeUpdate = time.time()
-                self.send_vel()
-        print("")
-        print("ros node shutdown")
+        lin_speed_str = "{:.3f}".format(self.lin_speed_cmd)
+        ang_speed_str = "{:.3f}".format(self.ang_speed_cmd)
+
+        stdscr.addstr(13, 0, "Lin. Vel.:")
+        stdscr.addstr(
+            13, 12, lin_speed_str, curses.A_BOLD | curses.color_pair(1))
+
+        stdscr.addstr(14, 0, "Ang. Vel.:")
+        stdscr.addstr(
+            14, 12, ang_speed_str, curses.A_BOLD | curses.color_pair(1))
+
+        stdscr.refresh()
+
+        self.update_info = False
 
     def status_cb(self, msg):
         if msg.status != self.status:
             self.status = msg.status
-            self.print_info = True
+            self.update_info = True
 
     def status_to_string(self, pwm_state):
         if pwm_state == 0:
@@ -151,14 +174,10 @@ PS       shutdown teleop
         else:
             return "UNKNOWN"
 
-    def send_vel(self):
+    def send_vel(self, lin_vel, ang_vel):
         twist = Twist()
-        twist.linear.x = self.linear_vel
-        twist.linear.y = 0
-        twist.linear.z = 0
-        twist.angular.x = 0
-        twist.angular.y = 0
-        twist.angular.z = self.angular_vel
+        twist.linear.x = lin_vel
+        twist.angular.z = ang_vel
         self.publisher.publish(twist)
 
     def enablePwm(self, enabled):
@@ -170,11 +189,16 @@ PS       shutdown teleop
             print("Service call failed: %s" % e)
 
 
-if __name__ == '__main__':
+def main(stdscr):
 
-    rospy.init_node("joy_teleop")
+    stdscr.clear()
 
+    rospy.init_node("joy_teleop")
     try:
-        JoyTeleopNode()
+        JoyTeleopNode(stdscr)
     except rospy.ROSInterruptException:
         pass
+
+
+if __name__ == '__main__':
+    curses.wrapper(main)