diff --git a/nodes/joy_teleop.py b/nodes/joy_teleop.py
index 93ed1cbe511a9c45415cdbb0f94a87d6545d686c..9715c1c59c1857d9693165a16a07bafcb94e36c2 100755
--- a/nodes/joy_teleop.py
+++ b/nodes/joy_teleop.py
@@ -1,6 +1,5 @@
#!/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)