yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-03 03:04:09 +05:30
Code Issues Projects Releases Wiki Activity

Merge pull request #41 from Sunlcy/indigo-devel

Browse Source 
Fix #40 issue and add .gitignore file
This commit is contained in:
pirobot 2017-08-18 06:45:32 -07:00 committed by GitHub
commit 641e015b4e
3 changed files with 207 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
.gitignore vendored Normal file
View File

@ -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
*~

82
ros_arduino_python/nodes/arduino_node.py
View File

@ -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()

117
ros_arduino_python/src/ros_arduino_python/arduino_driver.py
View File

@ -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."