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

Updated arduino_node.py including adding a diagnostics rate parameters

Browse Source
This commit is contained in:
Patrick Goebel 2016-09-12 05:49:12 -07:00
parent d0a61ed572
commit 0fcdcd02e5
1 changed files with 165 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

243
ros_arduino_python/nodes/arduino_node.py
View File

@ -23,24 +23,26 @@ import rospy
from ros_arduino_python.arduino_driver import Arduino from ros_arduino_python.arduino_driver import Arduino
from ros_arduino_python.arduino_sensors import * from ros_arduino_python.arduino_sensors import *
from ros_arduino_msgs.srv import * from ros_arduino_msgs.srv import *
from ros_arduino_python.diagnostics import DiagnosticsUpdater, DiagnosticsPublisher
from dynamic_reconfigure.server import Server from dynamic_reconfigure.server import Server
import dynamic_reconfigure.client import dynamic_reconfigure.client
from ros_arduino_python.cfg import ROSArduinoBridgeConfig from ros_arduino_python.cfg import ROSArduinoBridgeConfig
from ros_arduino_python.base_controller import BaseController from ros_arduino_python.base_controller import BaseController
from ros_arduino_python.servo_controller import Servo, ServoController from ros_arduino_python.servo_controller import Servo, ServoController
#from ros_arduino_python.follow_controller import FollowController from ros_arduino_python.follow_controller import FollowController
from ros_arduino_python.joint_state_publisher import JointStatePublisher from ros_arduino_python.joint_state_publisher import JointStatePublisher
from geometry_msgs.msg import Twist from geometry_msgs.msg import Twist
from std_srvs.srv import Empty, EmptyResponse from std_srvs.srv import Empty, EmptyResponse
import os, time import os, time, thread
import thread
from math import radians from math import radians
from serial.serialutil import SerialException
#controller_types = { "follow_controller" : FollowController } controller_types = { "follow_controller" : FollowController }
class ArduinoROS(): class ArduinoROS():
def __init__(self): def __init__(self):
rospy.init_node('arduino', log_level=rospy.DEBUG) self.name = 'arduino'
rospy.init_node(self.name, log_level=rospy.INFO)
# Cleanup when termniating the node # Cleanup when termniating the node
rospy.on_shutdown(self.shutdown) rospy.on_shutdown(self.shutdown)
@ -51,15 +53,22 @@ class ArduinoROS():
self.base_frame = rospy.get_param("~base_frame", 'base_link') self.base_frame = rospy.get_param("~base_frame", 'base_link')
# Overall loop rate: should be faster than fastest sensor rate # Overall loop rate: should be faster than fastest sensor rate
self.rate = int(rospy.get_param("~rate", 50)) self.rate = int(rospy.get_param("~rate", 30))
r = rospy.Rate(self.rate) r = rospy.Rate(self.rate)
# Rate at which summary SensorState message is published. Individual sensors publish # 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.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
# Are we using the base_controller?
self.use_base_controller = rospy.get_param("~use_base_controller", False) self.use_base_controller = rospy.get_param("~use_base_controller", False)
# Default error threshold (percent) before getting a diagnostics warning
self.diagnotics_error_threshold = rospy.get_param("~diagnotics_error_threshold", 10)
# Diagnostics update rate
self.diagnotics_rate = rospy.get_param("~diagnotics_rate", 1.0)
# Assume we don't have any joints by default # Assume we don't have any joints by default
self.have_joints = False self.have_joints = False
@ -111,31 +120,41 @@ class ArduinoROS():
# A service to read the value of an analog sensor # A service to read the value of an analog sensor
rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler) rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
# A service to reset the odometry values to 0 # Initialize the device
rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler) self.device = Arduino(self.port, self.baud, self.timeout, debug=True)
# A service to update the PID parameters Kp, Kd, Ki, and Ko # Make the connection
rospy.Service('~update_pid', UpdatePID, self.UpdatePIDHandler) if self.device.connect():
rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
else:
rospy.logerr("No serial connection found.")
os._exit(0)
#rospy.signal_shutdown("No serial connection found.")
# Initialize the base controller if used
if self.use_base_controller:
self.base_controller = BaseController(self.device, self.base_frame)
# A service to reset the odometry values to 0
rospy.Service('~reset_odometry', Empty, self.ResetOdometryHandler)
# A service to update the PID parameters Kp, Kd, Ki, and Ko
rospy.Service('~update_pid', UpdatePID, self.UpdatePIDHandler)
# Fire up the dynamic_reconfigure server # Fire up the dynamic_reconfigure server
dyn_server = Server(ROSArduinoBridgeConfig, self.dynamic_reconfigure_server_callback) dyn_server = Server(ROSArduinoBridgeConfig, self.dynamic_reconfigure_server_callback)
# Connect to the dynamic_reconfigure client # Connect to the dynamic_reconfigure client
dyn_client = dynamic_reconfigure.client.Client("arduino", timeout=5) dyn_client = dynamic_reconfigure.client.Client("arduino", timeout=5)
# Initialize the device
self.device = Arduino(self.port, self.baud, self.timeout)
# Make the connection
self.device.connect()
rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
# Reserve a thread lock # Reserve a thread lock
mutex = thread.allocate_lock() mutex = thread.allocate_lock()
# Initialize any sensors # Initialize any sensors
self.sensors = list() self.device.sensors = list()
# Keep a list of IMUs or gyros used for odometry so they can be reset
self.imu_for_odom = list()
# Read in the sensors parameter dictionary # Read in the sensors parameter dictionary
sensor_params = rospy.get_param("~sensors", dict({})) sensor_params = rospy.get_param("~sensors", dict({}))
@ -144,12 +163,14 @@ class ArduinoROS():
for name, params in sensor_params.iteritems(): for name, params in sensor_params.iteritems():
if params['type'].lower() == 'Ping'.lower(): if params['type'].lower() == 'Ping'.lower():
sensor = Ping(self.device, name, **params) sensor = Ping(self.device, name, **params)
elif params['type'].lower() == 'GP2D12'.lower(): elif params['type'].lower() == 'GP2D12'.lower() or params['type'].lower() == 'GP2Y0A21YK0F'.lower():
sensor = GP2D12(self.device, name, **params) sensor = GP2D12(self.device, name, **params)
elif params['type'].lower() == 'Digital'.lower(): elif params['type'].lower() == 'Digital'.lower():
sensor = DigitalSensor(self.device, name, **params) sensor = DigitalSensor(self.device, name, **params)
elif params['type'].lower() == 'Analog'.lower(): elif params['type'].lower() == 'Analog'.lower():
sensor = AnalogSensor(self.device, name, **params) sensor = AnalogSensor(self.device, name, **params)
elif params['type'].lower() == 'AnalogFloat'.lower():
sensor = AnalogFloatSensor(self.device, name, **params)
elif params['type'].lower() == 'PololuMotorCurrent'.lower(): elif params['type'].lower() == 'PololuMotorCurrent'.lower():
sensor = PololuMotorCurrent(self.device, name, **params) sensor = PololuMotorCurrent(self.device, name, **params)
elif params['type'].lower() == 'PhidgetsVoltage'.lower(): elif params['type'].lower() == 'PhidgetsVoltage'.lower():
@ -158,12 +179,28 @@ class ArduinoROS():
sensor = PhidgetsCurrent(self.device, name, **params) sensor = PhidgetsCurrent(self.device, name, **params)
elif params['type'].lower() == 'IMU'.lower(): elif params['type'].lower() == 'IMU'.lower():
sensor = IMU(self.device, name, **params) sensor = IMU(self.device, name, **params)
try:
if params['use_for_odom']:
self.imu_for_odom.append(sensor)
except:
pass
elif params['type'].lower() == 'Gyro'.lower():
try:
sensor = Gyro(self.device, name, base_controller=self.base_controller, **params)
except:
sensor = Gyro(self.device, name, **params)
try:
if params['use_for_odom']:
self.imu_for_odom.append(sensor)
except:
pass
# if params['type'].lower() == 'MaxEZ1'.lower(): # if params['type'].lower() == 'MaxEZ1'.lower():
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin'] # self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
self.sensors.append(sensor) self.device.sensors.append(sensor)
if params['rate'] != None and params['rate'] != 0: if params['rate'] != None and params['rate'] != 0:
rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
@ -173,9 +210,9 @@ class ArduinoROS():
else: else:
rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write") rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write")
# If at least one IMU or gyro is used for odometry, set the use_imu_heading flag on the base controller
# Initialize any joints (servos) if self.use_base_controller and len(self.imu_for_odom) > 0:
self.device.joints = dict() self.base_controller.use_imu_heading = True
# Read in the joints (if any) # Read in the joints (if any)
joint_params = rospy.get_param("~joints", dict()) joint_params = rospy.get_param("~joints", dict())
@ -183,13 +220,17 @@ class ArduinoROS():
if len(joint_params) != 0: if len(joint_params) != 0:
self.have_joints = True self.have_joints = True
self.device.joints = dict()
self.device.joint_update_rate = float(rospy.get_param("~joint_update_rate", 10))
# Configure each servo # Configure each servo
for name, params in joint_params.iteritems(): for name, params in joint_params.iteritems():
try: try:
if params['continuous'] == True: if params['continuous'] == True:
self.device.joints[name] = ContinuousServo(self.device, name) self.device.joints[name] = ContinuousServo(self.device, name)
else: else:
self.device.joints[name] = Servo(self.device, name) self.device.joints[name] = Servo(self.device, name)
except: except:
self.device.joints[name] = Servo(self.device, name) self.device.joints[name] = Servo(self.device, name)
@ -202,64 +243,87 @@ class ArduinoROS():
# The joint state publisher publishes the latest joint values on the /joint_states topic # The joint state publisher publishes the latest joint values on the /joint_states topic
self.joint_state_publisher = JointStatePublisher() self.joint_state_publisher = JointStatePublisher()
# # Initialize any trajectory action follow controllers # Create the diagnostics updater for the Arduino device
# controllers = rospy.get_param("~controllers", dict()) self.device.diagnostics = DiagnosticsUpdater(self, self.name, self.diagnotics_error_threshold, self.diagnotics_rate, create_watchdog=True)
#
# self.device.controllers = list() # Create the overall diagnostics publisher
# self.diagnostics_publisher = DiagnosticsPublisher(self)
# for name, params in controllers.items():
# try: # Initialize any trajectory action follow controllers
# controller = controller_types[params["type"]](self.device, name) controllers = rospy.get_param("~controllers", dict())
# self.device.controllers.append(controller)
# except Exception as e: self.device.controllers = list()
# if type(e) == KeyError:
# rospy.logerr("Unrecognized controller: " + params["type"]) for name, params in controllers.items():
# else: try:
# rospy.logerr(str(type(e)) + str(e)) controller = controller_types[params["type"]](self.device, name)
# self.device.controllers.append(controller)
# for controller in self.device.controllers: except Exception as e:
# controller.startup() if type(e) == KeyError:
rospy.logerr("Unrecognized controller: " + params["type"])
# Initialize the base controller if used else:
if self.use_base_controller: rospy.logerr(str(type(e)) + str(e))
self.base_controller = BaseController(self.device, self.base_frame)
for controller in self.device.controllers:
controller.startup()
print "\n==> ROS Arduino Bridge ready for action!" print "\n==> ROS Arduino Bridge ready for action!"
# Start polling the sensors, base controller, and servo controller # Start polling the sensors, base controller, and servo controller
while not rospy.is_shutdown(): while not rospy.is_shutdown():
# Heartbeat test for the serial connection # Heartbeat/watchdog test for the serial connection
try: try:
# Update read counters
self.device.diagnostics.reads += 1
self.device.diagnostics.total_reads += 1
self.device.serial_port.inWaiting() self.device.serial_port.inWaiting()
# Add this heartbeat to the frequency status diagnostic task
self.device.diagnostics.freq_diag.tick()
# Let the diagnostics updater know we're still alive
self.device.diagnostics.watchdog = True
except IOError: except IOError:
# Update error counter
self.device.diagnostics.errors += 1
rospy.loginfo("Lost serial connection. Waiting to reconnect...") rospy.loginfo("Lost serial connection. Waiting to reconnect...")
# Let the diagnostics updater know that we're down
self.device.diagnostics.watchdog = False
self.device.close() self.device.close()
while True: with mutex:
try: while True:
self.device.open() try:
rospy.loginfo("Serial connection re-established.") self.device.open()
break while True:
except: self.device.serial_port.write('\r')
r.sleep() test = self.device.serial_port.readline().strip('\n').strip('\r')
continue rospy.loginfo("Waking up serial port...")
if test == 'Invalid Command':
for sensor in self.sensors: self.device.serial_port.flushInput()
if sensor.rate != 0: self.device.serial_port.flushOutput()
mutex.acquire() break
sensor.poll() rospy.loginfo("Serial connection re-established.")
mutex.release() break
except:
if self.use_base_controller: r.sleep()
mutex.acquire() self.diagnostics_publisher.update()
self.base_controller.poll() continue
mutex.release()
if self.have_joints:
mutex.acquire()
self.servo_controller.poll()
self.joint_state_publisher.poll(self.device.joints.values())
mutex.release()
# Poll any sensors
for sensor in self.device.sensors:
if sensor.rate != 0:
with mutex:
sensor.poll()
# Poll the base controller
if self.use_base_controller:
with mutex:
self.base_controller.poll()
# Poll any joints
if self.have_joints:
with mutex:
self.servo_controller.poll()
self.joint_state_publisher.poll(self.device.joints.values())
# Publish all sensor values on a single topic for convenience # Publish all sensor values on a single topic for convenience
now = rospy.Time.now() now = rospy.Time.now()
@ -267,16 +331,19 @@ class ArduinoROS():
msg = SensorState() msg = SensorState()
msg.header.frame_id = self.base_frame msg.header.frame_id = self.base_frame
msg.header.stamp = now msg.header.stamp = now
for i in range(len(self.sensors)): for i in range(len(self.device.sensors)):
if self.sensors[i].message_type != MessageType.IMU: if self.device.sensors[i].message_type != MessageType.IMU:
msg.name.append(self.sensors[i].name) msg.name.append(self.device.sensors[i].name)
msg.value.append(self.sensors[i].value) msg.value.append(self.device.sensors[i].value)
try: try:
self.sensorStatePub.publish(msg) self.sensorStatePub.publish(msg)
except: except:
pass pass
self.t_next_sensors = now + self.t_delta_sensors self.t_next_sensors = now + self.t_delta_sensors
# Update diagnostics and publish
self.diagnostics_publisher.update()
r.sleep() r.sleep()
@ -342,6 +409,8 @@ class ArduinoROS():
def ResetOdometryHandler(self, req): def ResetOdometryHandler(self, req):
if self.use_base_controller: if self.use_base_controller:
self.base_controller.reset_odometry() self.base_controller.reset_odometry()
for imu in self.imu_for_odom:
imu.reset()
else: else:
rospy.logwarn("Not using base controller!") rospy.logwarn("Not using base controller!")
return EmptyResponse() return EmptyResponse()
@ -373,6 +442,12 @@ class ArduinoROS():
self.base_controller.accel_limit = config['accel_limit'] self.base_controller.accel_limit = config['accel_limit']
self.base_controller.max_accel = self.base_controller.accel_limit * self.base_controller.ticks_per_meter / self.base_controller.rate self.base_controller.max_accel = self.base_controller.accel_limit * self.base_controller.ticks_per_meter / self.base_controller.rate
if self.base_controller.odom_linear_scale_correction != config['odom_linear_scale_correction']:
self.base_controller.odom_linear_scale_correction = config['odom_linear_scale_correction']
if self.base_controller.odom_angular_scale_correction != config['odom_angular_scale_correction']:
self.base_controller.odom_angular_scale_correction = config['odom_angular_scale_correction']
self.device.update_pid(self.base_controller.Kp, self.base_controller.Kd, self.base_controller.Ki, self.base_controller.Ko) self.device.update_pid(self.base_controller.Kp, self.base_controller.Kd, self.base_controller.Ki, self.base_controller.Ko)
rospy.loginfo("Updating PID parameters: Kp=%0.2f, Kd=%0.2f, Ki=%0.2f, Ko=%0.2f, accel_limit=%0.2f" %(self.base_controller.Kp, self.base_controller.Kd, self.base_controller.Ki, self.base_controller.Ko, self.base_controller.accel_limit)) rospy.loginfo("Updating PID parameters: Kp=%0.2f, Kd=%0.2f, Ki=%0.2f, Ko=%0.2f, accel_limit=%0.2f" %(self.base_controller.Kp, self.base_controller.Kd, self.base_controller.Ki, self.base_controller.Ko, self.base_controller.accel_limit))
@ -383,7 +458,7 @@ class ArduinoROS():
def shutdown(self): def shutdown(self):
rospy.loginfo("Shutting down Arduino node...") rospy.loginfo("Shutting down Arduino node...")
# Stop the robot # Stop the robot
if self.use_base_controller: if self.use_base_controller:
rospy.loginfo("Stopping the robot...") rospy.loginfo("Stopping the robot...")
@ -396,6 +471,18 @@ class ArduinoROS():
for joint in self.device.joints.values(): for joint in self.device.joints.values():
self.device.detach_servo(joint.pin) self.device.detach_servo(joint.pin)
rospy.sleep(0.2) rospy.sleep(0.2)
# Close the serial port
#self.device.serial_port.close()
if __name__ == '__main__': if __name__ == '__main__':
myArduino = ArduinoROS() try:
myArduino = ArduinoROS()
except KeyboardInterrupt:
try:
myArduino.device.serial_port.close()
except:
os._exit(0)
except SerialException:
os._exit(0)