diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..396c495 --- /dev/null +++ b/.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 +*~ diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index e9be685..f7b38eb 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/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() diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py b/ros_arduino_python/src/ros_arduino_python/arduino_driver.py index 831a598..68f0a6b 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_driver.py +++ b/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." -