WIP

nodes/joy_teleop.py

@@ -13,7 +13,7 @@ import curses
 ############# DUALSHOCK 3 ##############
 
 coefAng = -1
-coefLin = 1
+coefLin = 0.2
 
 # BUTTONS
 
@@ -56,37 +56,14 @@ A_R2 = 5
 class JoyTeleopNode():
 
     def joyCallback(self, data):
+        self.buttons = data.buttons
+        self.axes = data.axes
 
-        if (data.buttons[B_START]):
-            # Toggle PWM
-            if self.status == Status.POWER_ON:
-                self.enablePwm(False)
-            else:
-                self.enablePwm(True)
-
-        if (data.buttons[B_L1] and data.buttons[B_R1]):
-            self.setBackClamp(1)
-        elif (data.buttons[B_R1]):
-            self.setBackClamp(11)
-        elif (data.buttons[B_L1]):
-            self.setBackClamp(21)
-        else:
-            self.setBackClamp(1)
-
-
-        if (data.buttons[B_PS]):
-            # shutdown teleop
-            self.forceQuit = True
-
-        lin_vel = data.axes[A_LJOYV] * coefLin
-        ang_vel = data.axes[A_LJOYH] * coefAng
+    def __init__(self, stdscr):
 
-        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
+        self.buttons = None
+        self.axes = None
 
-    def __init__(self, stdscr):
         self.lin_speed_cmd = 0
         self.ang_speed_cmd = 0
 
@@ -97,7 +74,7 @@ class JoyTeleopNode():
         self.update_info = False
 
         self.forceQuit = False
-        self.publisher = rospy.Publisher('cmd_vel', Twist, queue_size=1)
+        self.publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
 
         self.pwm_state_sub = rospy.Subscriber(
             '/mccd/status', Status, self.status_cb)
@@ -108,12 +85,13 @@ class JoyTeleopNode():
 
         rospy.wait_for_service('/mccd/enable_pwm')
         self.change_pwm = rospy.ServiceProxy('/mccd/enable_pwm', SetBool)
-        self.set_back_clamp = rospy.ServiceProxy('/clamp/cmd_backclamp',SetClampPos)
+        self.set_back_clamp = rospy.ServiceProxy(
+            '/clamp/cmd_backclamp', SetClampPos)
 
         self.joySubscriber = rospy.Subscriber('joy', Joy, self.joyCallback)
 
         self.lastTimeUpdate = time.time()
-        self.minUpdatePeriod = 0.3  # resend data every x sec.
+        self.minUpdatePeriod = 0.1  # resend data every x sec.
 
         self.debugRemote = ""
 
@@ -127,6 +105,35 @@ class JoyTeleopNode():
             t = time.time()
 
             if self.lastTimeUpdate + self.minUpdatePeriod < t:
+                if self.buttons is not None and self.axes is not None:
+                    if (self.buttons[B_START]):
+                        # Toggle PWM
+                        if self.status == Status.POWER_ON:
+                            self.enablePwm(False)
+                        else:
+                            self.enablePwm(True)
+
+                    if (self.buttons[B_L1] and self.buttons[B_R1]):
+                        self.setBackClamp(1)
+                    elif (self.buttons[B_R1]):
+                        self.setBackClamp(11)
+                    elif (self.buttons[B_L1]):
+                        self.setBackClamp(21)
+                    else:
+                        self.setBackClamp(1)
+
+                    if (self.buttons[B_PS]):
+                        # shutdown teleop
+                        self.forceQuit = True
+
+                    lin_vel = self.axes[A_LJOYV] * coefLin
+                    ang_vel = self.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
+
                 self.send_vel(self.lin_speed_cmd, self.ang_speed_cmd)
                 self.lastTimeUpdate = t
 
@@ -161,7 +168,7 @@ class JoyTeleopNode():
         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, 0, "Lin. Vel.:")
         stdscr.addstr(
             13, 12, lin_speed_str, curses.A_BOLD | curses.color_pair(1))
 
@@ -173,10 +180,9 @@ class JoyTeleopNode():
         stdscr.addstr(
             17, 12, self.backClampStr, curses.A_BOLD | curses.color_pair(1))
 
-
-        stdscr.addstr(20, 0, "Debug:")
-        stdscr.addstr(
-            20, 12, self.debugRemote, curses.A_BOLD | curses.color_pair(1))
+        # stdscr.addstr(20, 0, "Debug:")
+        # stdscr.addstr(
+        #     20, 12, self.debugRemote, curses.A_BOLD | curses.color_pair(1))
 
         stdscr.refresh()