fix hbrobotics#10 issue

lukeliuli · Aug 11, 2017 · 72f37ac · 72f37ac
1 parent 589d670
commit 72f37ac
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py
@@ -26,6 +26,7 @@
 import os, time, sys, traceback
 from serial.serialutil import SerialException, SerialTimeoutException
 from serial import Serial
+import rospy
 
 SERVO_MAX = 180
 SERVO_MIN = 0
@@ -47,7 +48,7 @@ def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5):
         self.encoder_count = 0
         self.writeTimeout = timeout
         self.interCharTimeout = timeout / 30.
-
+        self.motors_reversed = rospy.get_param("~motors_reversed", False)
         # Keep things thread safe
         self.mutex = thread.allocate_lock()
 
@@ -268,6 +269,8 @@ def get_encoder_counts(self):
             raise SerialException
             return None
         else:
+            if self.motors_reversed:
+                values[0], values[1] = -values[0], -values[1]
             return values
 
     def reset_encoders(self):
@@ -278,6 +281,8 @@ def reset_encoders(self):
     def drive(self, right, left):
         ''' Speeds are given in encoder ticks per PID interval
         '''
+        if self.motors_reversed:
+            left, right = -left, -right
         return self.execute_ack('m %d %d' %(right, left))
 
     def drive_m_per_s(self, right, left):