fix #40 issue and add .gitignore file

* Make the parameter "motors_reversed" take effect:
    Before returning encoder data, check parameter "motors_reversed" if
    true. If true, make these data opposite. So does motors speed data.
* Add .gitignore to ignore files like pyc generated by PVM and temporary
  files generated by vim or other editors

.gitignore

@@ -0,0 +1,106 @@
+
+# Created by https://www.gitignore.io/api/python
+
+### Python ###
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+env/
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*,cover
+.hypothesis/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# pyenv
+.python-version
+
+# celery beat schedule file
+celerybeat-schedule
+
+# SageMath parsed files
+*.sage.py
+
+# dotenv
+.env
+
+# virtualenv
+.venv
+venv/
+ENV/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# End of https://www.gitignore.io/api/python
+# temporary files
+*~

ros_arduino_python/nodes/arduino_node.py

@@ -2,7 +2,7 @@
 
 """
     A ROS Node for the Arduino microcontroller
-    
+
     Created for the Pi Robot Project: http://www.pirobot.org
     Copyright (c) 2012 Patrick Goebel.  All rights reserved.
 
@@ -10,12 +10,12 @@
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.
-    
+
     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details at:
-    
+
     http://www.gnu.org/licenses/gpl.html
 """
 
@@ -43,76 +43,76 @@ class ArduinoROS():
         self.baud = int(rospy.get_param("~baud", 57600))
         self.timeout = rospy.get_param("~timeout", 0.5)
         self.base_frame = rospy.get_param("~base_frame", 'base_link')
-
+        self.motors_reversed = rospy.get_param("~motors_reversed", False)
         # Overall loop rate: should be faster than fastest sensor rate
         self.rate = int(rospy.get_param("~rate", 50))
         r = rospy.Rate(self.rate)
 
         # Rate at which summary SensorState message is published. Individual sensors publish
-        # at their own rates.        
+        # at their own rates.
         self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
-        
+
         self.use_base_controller = rospy.get_param("~use_base_controller", False)
-        
+
         # Set up the time for publishing the next SensorState message
         now = rospy.Time.now()
         self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
         self.t_next_sensors = now + self.t_delta_sensors
-        
+
         # Initialize a Twist message
         self.cmd_vel = Twist()
-  
+
         # A cmd_vel publisher so we can stop the robot when shutting down
         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
-        
+
         # The SensorState publisher periodically publishes the values of all sensors on
         # a single topic.
         self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)
-        
+
         # A service to position a PWM servo
         rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
-        
+
         # A service to read the position of a PWM servo
         rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
-        
+
         # A service to turn set the direction of a digital pin (0 = input, 1 = output)
         rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
-        
+
         # A service to turn a digital sensor on or off
         rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
-        
+
         # A service to read the value of a digital sensor
-        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler) 
+        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)
 
         # A service to set pwm values for the pins
         rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
-        
+
         # A service to read the value of an analog sensor
         rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
 
         # Initialize the controlller
-        self.controller = Arduino(self.port, self.baud, self.timeout)
+        self.controller = Arduino(self.port, self.baud, self.timeout, self.motors_reversed)
-        
+
         # Make the connection
         self.controller.connect()
-        
+
         rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
-     
+
         # Reserve a thread lock
         mutex = thread.allocate_lock()
 
         # Initialize any sensors
         self.mySensors = list()
-        
+
         sensor_params = rospy.get_param("~sensors", dict({}))
-        
+
         for name, params in sensor_params.iteritems():
             # Set the direction to input if not specified
             try:
                 params['direction']
             except:
                 params['direction'] = 'input'
-                
+
             if params['type'] == "Ping":
                 sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame)
             elif params['type'] == "GP2D12":
@@ -127,7 +127,7 @@ class ArduinoROS():
                 sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame)
             elif params['type'] == 'PhidgetsCurrent':
                 sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
-                
+
 #                if params['type'] == "MaxEZ1":
 #                    self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
 #                    self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
@@ -136,26 +136,26 @@ class ArduinoROS():
                 rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
             except:
                 rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
-              
+
         # Initialize the base controller if used
         if self.use_base_controller:
             self.myBaseController = BaseController(self.controller, self.base_frame, self.name + "_base_controller")
-    
+
         # Start polling the sensors and base controller
         while not rospy.is_shutdown():
             for sensor in self.mySensors:
                 mutex.acquire()
                 sensor.poll()
                 mutex.release()
-                    
+
             if self.use_base_controller:
                 mutex.acquire()
                 self.myBaseController.poll()
                 mutex.release()
-            
+
             # Publish all sensor values on a single topic for convenience
             now = rospy.Time.now()
-            
+
             if now > self.t_next_sensors:
                 msg = SensorState()
                 msg.header.frame_id = self.base_frame
@@ -167,40 +167,40 @@ class ArduinoROS():
                     self.sensorStatePub.publish(msg)
                 except:
                     pass
-                
+
                 self.t_next_sensors = now + self.t_delta_sensors
-            
+
             r.sleep()
-    
+
     # Service callback functions
     def ServoWriteHandler(self, req):
         self.controller.servo_write(req.id, req.value)
         return ServoWriteResponse()
-    
+
     def ServoReadHandler(self, req):
         pos = self.controller.servo_read(req.id)
         return ServoReadResponse(pos)
-    
+
     def DigitalSetDirectionHandler(self, req):
         self.controller.pin_mode(req.pin, req.direction)
         return DigitalSetDirectionResponse()
-    
+
     def DigitalWriteHandler(self, req):
         self.controller.digital_write(req.pin, req.value)
         return DigitalWriteResponse()
-    
+
     def DigitalReadHandler(self, req):
         value = self.controller.digital_read(req.pin)
         return DigitalReadResponse(value)
-              
+
     def AnalogWriteHandler(self, req):
         self.controller.analog_write(req.pin, req.value)
         return AnalogWriteResponse()
-    
+
     def AnalogReadHandler(self, req):
         value = self.controller.analog_read(req.pin)
         return AnalogReadResponse(value)
- 
+
     def shutdown(self):
         rospy.loginfo("Shutting down Arduino Node...")
 
@@ -211,7 +211,7 @@ class ArduinoROS():
             rospy.sleep(2)
         except:
             pass
-        
+
         # Close the serial port
         try:
             self.controller.close()

ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -3,7 +3,7 @@
 """
     A Python driver for the Arduino microcontroller running the
     ROSArduinoBridge firmware.
-    
+
     Created for the Pi Robot Project: http://www.pirobot.org
     Copyright (c) 2012 Patrick Goebel.  All rights reserved.
 
@@ -11,12 +11,12 @@
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.
-    
+
     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details at:
-    
+
     http://www.gnu.org/licenses/gpl.html
 
 """
@@ -34,31 +34,31 @@ SERVO_MIN = 0
 
 class Arduino:
     ''' Configuration Parameters
-    '''    
+    '''
     N_ANALOG_PORTS = 6
     N_DIGITAL_PORTS = 12
-    
+
-    def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5):
+    def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, motors_reversed=False):
-        
+
         self.PID_RATE = 30 # Do not change this!  It is a fixed property of the Arduino PID controller.
         self.PID_INTERVAL = 1000 / 30
-        
+
         self.port = port
         self.baudrate = baudrate
         self.timeout = timeout
         self.encoder_count = 0
         self.writeTimeout = timeout
         self.interCharTimeout = timeout / 30.
-    
+        self.motors_reversed = motors_reversed
         # Keep things thread safe
         self.mutex = thread.allocate_lock()
-            
+
         # An array to cache analog sensor readings
         self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS
-        
+
         # An array to cache digital sensor readings
         self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
-    
+
     def connect(self):
         try:
             print "Connecting to Arduino on port", self.port, "..."
@@ -68,7 +68,7 @@ class Arduino:
             test = self.get_baud()
             if test != self.baudrate:
                 time.sleep(1)
-                test = self.get_baud()   
+                test = self.get_baud()
                 if test != self.baudrate:
                     raise SerialException
             print "Connected at", self.baudrate
@@ -82,16 +82,16 @@ class Arduino:
             print "Cannot connect to Arduino!"
             os._exit(1)
 
-    def open(self): 
+    def open(self):
         ''' Open the serial port.
         '''
         self.port.open()
 
-    def close(self): 
+    def close(self):
         ''' Close the serial port.
         '''
-        self.port.close() 
+        self.port.close()
-    
+
     def send(self, cmd):
         ''' This command should not be used on its own: it is called by the execute commands
             below in a thread safe manner.
@@ -100,7 +100,7 @@ class Arduino:
 
     def recv(self, timeout=0.5):
         timeout = min(timeout, self.timeout)
-        ''' This command should not be used on its own: it is called by the execute commands   
+        ''' This command should not be used on its own: it is called by the execute commands
             below in a thread safe manner.  Note: we use read() instead of readline() since
             readline() tends to return garbage characters from the Arduino
         '''
@@ -117,7 +117,7 @@ class Arduino:
         value = value.strip('\r')
 
         return value
-            
+
     def recv_ack(self):
         ''' This command should not be used on its own: it is called by the execute commands
             below in a thread safe manner.
@@ -149,15 +149,15 @@ class Arduino:
         ''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
         '''
         self.mutex.acquire()
-        
+
         try:
             self.port.flushInput()
         except:
             pass
-        
+
         ntries = 1
         attempts = 0
-        
+
         try:
             self.port.write(cmd + '\r')
             value = self.recv(self.timeout)
@@ -173,7 +173,7 @@ class Arduino:
             self.mutex.release()
             print "Exception executing command: " + cmd
             value = None
-        
+
         self.mutex.release()
         return int(value)
 
@@ -181,15 +181,15 @@ class Arduino:
         ''' Thread safe execution of "cmd" on the Arduino returning an array.
         '''
         self.mutex.acquire()
-        
+
         try:
             self.port.flushInput()
         except:
             pass
-        
+
         ntries = 1
         attempts = 0
-        
+
         try:
             self.port.write(cmd + '\r')
             values = self.recv_array()
@@ -206,7 +206,7 @@ class Arduino:
             print "Exception executing command: " + cmd
             raise SerialException
             return []
-        
+
         try:
             values = map(int, values)
         except:
@@ -214,20 +214,20 @@ class Arduino:
 
         self.mutex.release()
         return values
-        
+
     def execute_ack(self, cmd):
         ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
         '''
         self.mutex.acquire()
-        
+
         try:
             self.port.flushInput()
         except:
             pass
-        
+
         ntries = 1
         attempts = 0
-        
+
         try:
             self.port.write(cmd + '\r')
             ack = self.recv(self.timeout)
@@ -244,16 +244,16 @@ class Arduino:
             print "execute_ack exception when executing", cmd
             print sys.exc_info()
             return 0
-        
+
         self.mutex.release()
-        return ack == 'OK'   
+        return ack == 'OK'
-    
+
     def update_pid(self, Kp, Kd, Ki, Ko):
         ''' Set the PID parameters on the Arduino
         '''
         print "Updating PID parameters"
         cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko)
-        self.execute_ack(cmd)                          
+        self.execute_ack(cmd)
 
     def get_baud(self):
         ''' Get the current baud rate on the serial port.
@@ -270,18 +270,22 @@ class Arduino:
             raise SerialException
             return None
         else:
+            if self.motors_reversed:
+                values[0], values[1] = -values[0], -values[1]
             return values
 
     def reset_encoders(self):
         ''' Reset the encoder counts to 0
         '''
         return self.execute_ack('r')
-    
+
     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):
         ''' Set the motor speeds in meters per second.
         '''
@@ -292,37 +296,37 @@ class Arduino:
         right_ticks_per_loop  = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
 
         self.drive(right_ticks_per_loop , left_ticks_per_loop )
-        
+
     def stop(self):
         ''' Stop both motors.
         '''
         self.drive(0, 0)
-            
+
     def analog_read(self, pin):
         return self.execute('a %d' %pin)
-    
+
     def analog_write(self, pin, value):
         return self.execute_ack('x %d %d' %(pin, value))
-    
+
     def digital_read(self, pin):
         return self.execute('d %d' %pin)
-    
+
     def digital_write(self, pin, value):
         return self.execute_ack('w %d %d' %(pin, value))
-    
+
     def pin_mode(self, pin, mode):
         return self.execute_ack('c %d %d' %(pin, mode))
 
     def servo_write(self, id, pos):
         ''' Usage: servo_write(id, pos)
             Position is given in radians and converted to degrees before sending
-        '''        
+        '''
         return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos)))))
-    
+
     def servo_read(self, id):
         ''' Usage: servo_read(id)
             The returned position is converted from degrees to radians
-        '''        
+        '''
         return radians(self.execute('t %d' %id))
 
     def ping(self, pin):
@@ -331,7 +335,7 @@ class Arduino:
             and returns the range in cm.  Sonar distance resolution is integer based.
         '''
         return self.execute('p %d' %pin);
-    
+
 #    def get_maxez1(self, triggerPin, outputPin):
 #        ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar
 #            sensor connected to the General Purpose I/O lines, triggerPin, and
@@ -342,8 +346,8 @@ class Arduino:
 #            (inches). The sensor distance resolution is integer based. Also, the
 #            maxsonar trigger pin is RX, and the echo pin is PW.
 #        '''
-#        return self.execute('z %d %d' %(triggerPin, outputPin)) 
+#        return self.execute('z %d %d' %(triggerPin, outputPin))
- 
+
 
 """ Basic test for connectivity """
 if __name__ == "__main__":
@@ -351,15 +355,15 @@ if __name__ == "__main__":
         portName = "/dev/ttyACM0"
     else:
         portName = "COM43" # Windows style COM port.
-        
+
     baudRate = 57600
 
     myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
     myArduino.connect()
-     
+
     print "Sleeping for 1 second..."
-    time.sleep(1)   
+    time.sleep(1)
-    
+
     print "Reading on analog port 0", myArduino.analog_read(0)
     print "Reading on digital port 0", myArduino.digital_read(0)
     print "Blinking the LED 3 times"
@@ -367,11 +371,10 @@ if __name__ == "__main__":
         myArduino.digital_write(13, 1)
         time.sleep(1.0)
     #print "Current encoder counts", myArduino.encoders()
-    
+
     print "Connection test successful.",
-    
+
     myArduino.stop()
     myArduino.close()
-    
+
     print "Shutting down Arduino."
-