class ArduinoROS():
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG)
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
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')
# 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.
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)
# The SensorState publisher periodically publishes the values of all sensors on
# a single topic.
self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState)
# 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 set pwm values for the pins
rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
# Initialize the controlller
self.controller = Arduino(self.port, self.baud, self.timeout)
# 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":
sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == 'PhidgetsVoltage':
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']
self.mySensors.append(sensor)
rospy.loginfo(name + " " + str(params))
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.controller, self.base_frame)
# 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
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
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):
self.controller.servo_read(req.id)
return ServoReadResponse()
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 AnalogWriteHandler(self, req):
self.controller.analog_write(req.pin, req.value)
return AnalogWriteResponse()
def shutdown(self):
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
rospy.loginfo("Shutting down Arduino Node...")
class ArduinoROS():
def __init__(self):
rospy.init_node('mobilebase_arduino_node', log_level=rospy.INFO)
# Get the actual node name in case it is set in the launch file
self.name = rospy.get_name()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 57600))
self.timeout = rospy.get_param("~timeout", 0.7)
self.base_frame = rospy.get_param("~base_frame", 'base_footprint')
# Overall loop rate: should be faster than fastest sensor rate
self.rate = int(rospy.get_param("~rate", 20))
r = rospy.Rate(self.rate)
# Rate at which summary SensorState message is published. Individual sensors publish
# at their own rates.
self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
self.use_base_controller = rospy.get_param("~use_base_controller",
True)
# 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=20)
# 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 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)
# 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)
# A service to set alarm sensor
rospy.Service('~alarm_write', AlarmWrite, self.AlarmWriteHandler)
# A service to set light show
rospy.Service('~light_show', LightShow, self.LightShowHandler)
# Initialize the controlller
self.controller = Arduino(self.port, self.baud, self.timeout)
# Make the connection
self.controller.connect()
self.controller.alarm_write(1)
self.controller.light_show(1)
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":
sensor = GP2D12(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == "IR2Y0A02":
sensor = IR2Y0A02(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == "GP2Y0A41":
sensor = GP2Y0A41(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'Voltage':
sensor = VoltageSensor(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name,
params['pin'], params['rate'],
self.base_frame)
elif params['type'] == 'PhidgetsVoltage':
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)
elif params['type'] == 'MotorTotalCurrent':
sensor = MotorTotalCurrent(self.controller, name,
params['pin'], params['rate'],
self.base_frame)
try:
self.mySensors.append(sensor)
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
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
self.sensorStatePub.publish(msg)
except:
pass
self.t_next_sensors = now + self.t_delta_sensors
r.sleep()
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 AlarmWriteHandler(self, req):
self.controller.alarm_write(req.value)
return AlarmWriteResponse()
def LightShowHandler(self, req):
self.controller.light_show(req.value)
return LightShowResponse()
def shutdown(self):
rospy.logwarn("Shutting down Arduino Node...")
# Stop the robot
try:
rospy.logwarn("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
# Close the serial port
try:
self.controller.close()
except:
pass
finally:
rospy.logwarn("Serial port closed.")
os._exit(0)
class ArduinoROS():
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.INFO)
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
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')
# 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.
self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
self.use_base_controller = rospy.get_param("~use_base_controller", False)
# Assume we don't have any joints by default
self.have_joints = 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 attach a PWM servo to a specified pin
rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler)
# A service to detach a PWM servo to a specified pin
rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler)
# 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_pin_mode', DigitalPinMode, self.DigitalPinModeHandler)
# Obsolete: Use DigitalPinMode instead
rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
# A service to turn set the direction of an analog pin (0 = input, 1 = output)
rospy.Service('~analog_pin_mode', AnalogPinMode, self.AnalogPinModeHandler)
# 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)
# 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 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
mutex = thread.allocate_lock()
# Initialize any sensors
self.mySensors = list()
# Read in the sensors parameter dictionary
sensor_params = rospy.get_param("~sensors", dict({}))
# Initialize individual sensors appropriately
for name, params in sensor_params.iteritems():
if params['type'].lower() == 'Ping'.lower():
sensor = Ping(self.device, name, **params)
elif params['type'].lower() == 'GP2D12'.lower():
sensor = GP2D12(self.device, name, **params)
elif params['type'].lower() == 'Digital'.lower():
sensor = DigitalSensor(self.device, name, **params)
elif params['type'].lower() == 'Analog'.lower():
sensor = AnalogSensor(self.device, name, **params)
elif params['type'].lower() == 'PololuMotorCurrent'.lower():
sensor = PololuMotorCurrent(self.device, name, **params)
elif params['type'].lower() == 'PhidgetsVoltage'.lower():
sensor = PhidgetsVoltage(self.device, name, **params)
elif params['type'].lower() == 'PhidgetsCurrent'.lower():
sensor = PhidgetsCurrent(self.device, name, **params)
# if params['type'].lower() == 'MaxEZ1'.lower():
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
self.mySensors.append(sensor)
if params['rate'] != None and params['rate'] != 0:
rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
else:
if sensor.direction == "input":
rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/read")
else:
rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write")
# Initialize any joints (servos)
self.device.joints = dict()
# Read in the joints (if any)
joint_params = rospy.get_param("~joints", dict())
if len(joint_params) != 0:
self.have_joints = True
# Configure each servo
for name, params in joint_params.iteritems():
self.device.joints[name] = Servo(self.device, name)
# Display the joint setup on the terminal
rospy.loginfo(name + " " + str(params))
# The servo controller determines when to read and write position values to the servos
self.servo_controller = ServoController(self.device, "ServoController")
# The joint state publisher publishes the latest joint values on the /joint_states topic
self.joint_state_publisher = JointStatePublisher()
# # Initialize any trajectory action follow controllers
# controllers = rospy.get_param("~controllers", dict())
#
# self.device.controllers = list()
#
# for name, params in controllers.items():
# try:
# controller = controller_types[params["type"]](self.device, name)
# self.device.controllers.append(controller)
# except Exception as e:
# if type(e) == KeyError:
# rospy.logerr("Unrecognized controller: " + params["type"])
# else:
# rospy.logerr(str(type(e)) + str(e))
#
# for controller in self.device.controllers:
# controller.startup()
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.device, self.base_frame)
print "\n==> ROS Arduino Bridge ready for action!"
# Start polling the sensors, base controller, and servo controller
while not rospy.is_shutdown():
for sensor in self.mySensors:
if sensor.rate != 0:
mutex.acquire()
sensor.poll()
mutex.release()
if self.use_base_controller:
mutex.acquire()
self.myBaseController.poll()
mutex.release()
if self.have_joints:
mutex.acquire()
self.servo_controller.poll()
self.joint_state_publisher.poll(self.device.joints.values())
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
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
self.sensorStatePub.publish(msg)
except:
pass
self.t_next_sensors = now + self.t_delta_sensors
r.sleep()
# Service callback functions
def ServoAttachHandler(self, req):
self.device.attach_servo(req.id)
return ServoAttachResponse()
def ServoDetachHandler(self, req):
self.device.detach_servo(req.id)
return ServoDetachResponse()
def ServoWriteHandler(self, req):
self.device.servo_write(req.id, req.position)
return ServoWriteResponse()
# def ServoSpeedWriteHandler(self, req):
# delay =
# self.device.servo_delay(req.id, delay)
# return ServoSpeedResponse()
#
# # Convert servo speed in deg/s to a step delay in milliseconds
# step_delay = self.device.joints[name].get_step_delay(req.value)
#
# # Update the servo speed
# self.device.config_servo(pin, step_delay)
#
# return SetServoSpeedResponse()
def ServoReadHandler(self, req):
position = self.device.servo_read(req.id)
return ServoReadResponse(position)
def DigitalPinModeHandler(self, req):
self.device.digital_pin_mode(req.pin, req.direction)
return DigitalPinModeResponse()
# Obsolete: use DigitalPinMode instead
def DigitalSetDirectionHandler(self, req):
self.device.digital_pin_mode(req.pin, req.direction)
return DigitalSetDirectionResponse()
def DigitalWriteHandler(self, req):
self.device.digital_write(req.pin, req.value)
return DigitalWriteResponse()
def DigitalReadHandler(self, req):
value = self.device.digital_read(req.pin)
return DigitalReadResponse(value)
def AnalogPinModeHandler(self, req):
self.device.analog_pin_mode(req.pin, req.direction)
return AnalogPinModeResponse()
def AnalogWriteHandler(self, req):
self.device.analog_write(req.pin, req.value)
return AnalogWriteResponse()
def AnalogReadHandler(self, req):
value = self.device.analog_read(req.pin)
return AnalogReadResponse(value)
def shutdown(self):
rospy.loginfo("Shutting down Arduino node...")
# Stop the robot
if self.use_base_controller:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(2)
# Detach any servos
if self.have_joints:
rospy.loginfo("Detaching servos...")
for joint in self.device.joints.values():
self.device.detach_servo(joint.pin)
rospy.sleep(0.2)
class ArduinoROS():
def __init__(self):
rospy.init_node('ArbotixM', log_level=rospy.DEBUG)
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyArb")
self.baud = int(rospy.get_param("~baud", 38400))
self.timeout = rospy.get_param("~timeout", 0.5)
self.base_frame = rospy.get_param("~base_frame", 'base_link')
# Overall loop rate: should be faster than fastest sensor rate
self.rate = int(rospy.get_param("~rate", 50))
r = rospy.Rate(self.rate)
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()
# 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)
# Initialize the controlller
self.controller = ArbotixM(self.port, self.baud, self.timeout)
# Make the connection
#self.controller.connect() <<<<<<<<<<<Debug
rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
# Reserve a thread lock
mutex = thread.allocate_lock()
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.controller, self.base_frame)
# Start polling the sensors and base controller
while not rospy.is_shutdown():
if self.use_base_controller:
mutex.acquire()
self.myBaseController.poll()
mutex.release()
now = rospy.Time.now()
r.sleep()
def shutdown(self):
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
rospy.loginfo("Shutting down Arduino Node...")
class ArduinoROS():
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG)
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
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.l_scale = rospy.get_param("~linear_scale", 10)
self.a_scale = rospy.get_param("~angular_scale", 90)
# 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.
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
rospy.Subscriber("html_joy_stick", Joy, self.JoystickCallback)
# 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=10)
# A pan_tilt publisher so we can control the position of the pan/tilt gimble and center it on shutdown
self.pan_tilt_pub = rospy.Publisher('pan_tilt', PanTilt, queue_size=10)
# The SensorState publisher periodically publishes the values of all sensors on
# a single topic.
self.sensorStatePub = rospy.Publisher('~sensor_state',
SensorState,
queue_size=10)
# 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 set pwm values for the pins
rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
# Initialize the controller
self.controller = None
driver = rospy.get_param("~arduino_driver", "rab_driver")
params = {'pid rate': rospy.get_param("~base_controller_rate", 10)}
if driver == "rab_driver":
from ros_arduino_python.rab_driver import RABDriver
self.controller = RABDriver(params=params,
port=self.port,
baud=self.baud,
timeout=self.timeout)
elif driver == "pymata_driver":
from ros_arduino_python.pymata_driver import PymataDriver
self.controller = PymataDriver(params=params,
port=self.port,
baud=self.baud,
timeout=self.timeout)
else:
rospy.loginfo("Unknown Arduino driver")
return
# 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":
sensor = GP2D12(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name,
params['pin'], params['rate'],
self.base_frame)
elif params['type'] == 'PhidgetsVoltage':
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)
elif params['type'] == 'DigitalArray':
sensor = LineSensor(self.controller,
name,
params['pins'],
params['pullup'],
self.base_frame,
rate=params['rate'],
direction=params['direction'])
# if params['type'] == "MaxEZ1":
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
self.mySensors.append(sensor)
rospy.loginfo(name + " " + str(params))
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.controller,
self.base_frame)
# 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
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
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):
self.controller.servo_read(req.id)
return ServoReadResponse()
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 AnalogWriteHandler(self, req):
self.controller.analog_write(req.pin, req.value)
return AnalogWriteResponse()
def JoystickCallback(self, data):
#rospy.loginfo("Received joystick msg: " + str(data))
# axes 0 and 1 are assigned to the drive train
# axes 2 and 3 are assigned to the pan/tilt
# Handle motor drive/turning
twist = Twist()
twist.linear.x = self.l_scale * data.axes[0] # forward/backward speed
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = self.a_scale * data.axes[1] # turning speed
self.cmd_vel_pub.publish(twist)
# Handle pan/tilt
# The value received on axes 3 and 4 ranges from -1.0 to 1.0
pan_tilt = PanTilt()
pan_tilt.pan = data.axes[2]
pan_tilt.tilt = data.axes[3]
#rospy.loginfo("Publishing: pan %f tilt %f" % (pan_tilt.pan, pan_tilt.tilt))
self.pan_tilt_pub.publish(pan_tilt)
def shutdown(self):
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
rospy.loginfo("Shutting down Arduino Node...")
class ArduinoROS():
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.INFO)
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
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')
# 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.
self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
self.use_base_controller = rospy.get_param("~use_base_controller", False)
# Assume we don't have any joints by default
self.have_joints = 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 attach a PWM servo to a specified pin
rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler)
# A service to detach a PWM servo to a specified pin
rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler)
# 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_pin_mode', DigitalPinMode, self.DigitalPinModeHandler)
# Obsolete: Use DigitalPinMode instead
rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
# A service to turn set the direction of an analog pin (0 = input, 1 = output)
rospy.Service('~analog_pin_mode', AnalogPinMode, self.AnalogPinModeHandler)
# 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)
# 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 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
mutex = thread.allocate_lock()
# Initialize any sensors
self.mySensors = list()
# Read in the sensors parameter dictionary
sensor_params = rospy.get_param("~sensors", dict({}))
# Initialize individual sensors appropriately
for name, params in sensor_params.iteritems():
if params['type'].lower() == 'Ping'.lower():
sensor = Ping(self.device, name, **params)
elif params['type'].lower() == 'GP2D12'.lower():
sensor = GP2D12(self.device, name, **params)
elif params['type'].lower() == 'Digital'.lower():
sensor = DigitalSensor(self.device, name, **params)
elif params['type'].lower() == 'Analog'.lower():
sensor = AnalogSensor(self.device, name, **params)
elif params['type'].lower() == 'PololuMotorCurrent'.lower():
sensor = PololuMotorCurrent(self.device, name, **params)
elif params['type'].lower() == 'PhidgetsVoltage'.lower():
sensor = PhidgetsVoltage(self.device, name, **params)
elif params['type'].lower() == 'PhidgetsCurrent'.lower():
sensor = PhidgetsCurrent(self.device, name, **params)
# if params['type'].lower() == 'MaxEZ1'.lower():
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
self.mySensors.append(sensor)
if params['rate'] != None and params['rate'] != 0:
rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
else:
if sensor.direction == "input":
rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/read")
else:
rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write")
# Initialize any joints (servos)
self.device.joints = dict()
# Read in the joints (if any)
joint_params = rospy.get_param("~joints", dict())
if len(joint_params) != 0:
self.have_joints = True
# Configure each servo
for name, params in joint_params.iteritems():
self.device.joints[name] = Servo(self.device, name)
# Display the joint setup on the terminal
rospy.loginfo(name + " " + str(params))
# The servo controller determines when to read and write position values to the servos
self.servo_controller = ServoController(self.device, "ServoController")
# The joint state publisher publishes the latest joint values on the /joint_states topic
self.joint_state_publisher = JointStatePublisher()
# # Initialize any trajectory action follow controllers
# controllers = rospy.get_param("~controllers", dict())
#
# self.device.controllers = list()
#
# for name, params in controllers.items():
# try:
# controller = controller_types[params["type"]](self.device, name)
# self.device.controllers.append(controller)
# except Exception as e:
# if type(e) == KeyError:
# rospy.logerr("Unrecognized controller: " + params["type"])
# else:
# rospy.logerr(str(type(e)) + str(e))
#
# for controller in self.device.controllers:
# controller.startup()
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.device, self.base_frame)
print "\n==> ROS Arduino Bridge ready for action!"
# Start polling the sensors, base controller, and servo controller
while not rospy.is_shutdown():
for sensor in self.mySensors:
if sensor.rate != 0:
mutex.acquire()
sensor.poll()
mutex.release()
if self.use_base_controller:
mutex.acquire()
self.myBaseController.poll()
mutex.release()
if self.have_joints:
mutex.acquire()
self.servo_controller.poll()
self.joint_state_publisher.poll(self.device.joints.values())
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
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
self.sensorStatePub.publish(msg)
except:
pass
self.t_next_sensors = now + self.t_delta_sensors
r.sleep()
# Service callback functions
def ServoAttachHandler(self, req):
self.device.attach_servo(req.id)
return ServoAttachResponse()
def ServoDetachHandler(self, req):
self.device.detach_servo(req.id)
return ServoDetachResponse()
def ServoWriteHandler(self, req):
self.device.servo_write(req.id, req.position)
return ServoWriteResponse()
# def ServoSpeedWriteHandler(self, req):
# delay =
# self.device.servo_delay(req.id, delay)
# return ServoSpeedResponse()
#
# # Convert servo speed in deg/s to a step delay in milliseconds
# step_delay = self.device.joints[name].get_step_delay(req.value)
#
# # Update the servo speed
# self.device.config_servo(pin, step_delay)
#
# return SetServoSpeedResponse()
def ServoReadHandler(self, req):
position = self.device.servo_read(req.id)
return ServoReadResponse(position)
def DigitalPinModeHandler(self, req):
self.device.digital_pin_mode(req.pin, req.direction)
return DigitalPinModeResponse()
# Obsolete: use DigitalPinMode instead
def DigitalSetDirectionHandler(self, req):
self.device.digital_pin_mode(req.pin, req.direction)
return DigitalSetDirectionResponse()
def DigitalWriteHandler(self, req):
self.device.digital_write(req.pin, req.value)
return DigitalWriteResponse()
def DigitalReadHandler(self, req):
value = self.device.digital_read(req.pin)
return DigitalReadResponse(value)
def AnalogPinModeHandler(self, req):
self.device.analog_pin_mode(req.pin, req.direction)
return AnalogPinModeResponse()
def AnalogWriteHandler(self, req):
self.device.analog_write(req.pin, req.value)
return AnalogWriteResponse()
def AnalogReadHandler(self, req):
value = self.device.analog_read(req.pin)
return AnalogReadResponse(value)
def shutdown(self):
rospy.loginfo("Shutting down Arduino node...")
# Stop the robot
if self.use_base_controller:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(2)
# Detach any servos
if self.have_joints:
rospy.loginfo("Detaching servos...")
for joint in self.device.joints.values():
self.device.detach_servo(joint.pin)
rospy.sleep(0.2)
class ArduinoROS():
def __init__(self):
rospy.init_node('arduino', log_level=rospy.INFO)
rospy.Subscriber("motors_speed", Vector3, self.callback)
# Get the actual node name in case it is set in the launch file
self.name = rospy.get_name()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.IP = rospy.get_param("~IP", "192.168.43.168")
self.socket = int(rospy.get_param("~socket", 4445))
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.
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)
# 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.IP, self.socket, self.timeout,
self.motors_reversed)
# Make the connection
self.controller.connect()
rospy.loginfo("Connected to Arduino on port " + self.IP + " at " +
str(self.socket) + " 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":
sensor = GP2D12(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name,
params['pin'], params['rate'],
self.base_frame)
elif params['type'] == 'PhidgetsVoltage':
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']
try:
self.mySensors.append(sensor)
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
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
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()
# Topico de la velocidad de los 3 motores
def callback(self, data):
self.controller.motors_speed(data.x, data.y, data.z)
rospy.loginfo("El mensaje recibido es: [%f,%f,%f]", data.x, data.y,
data.z)
return
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...")
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
# Close the serial port
try:
self.controller.close()
except:
pass
finally:
rospy.loginfo("Serial port closed.")
os._exit(0)
class ArduinoROS():
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG)
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 57600))
self.timeout = rospy.get_param("~timeout", 0.5)
# 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.
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)
# The SensorState publisher periodically publishes the values of all sensors on
# a single topic.
self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState)
# 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)
# Initialize the controlller
self.controller = Arduino(self.port, self.baud, self.timeout)
# 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'], params['frame_id'])
elif params['type'] == "PointCloudPing":
sensor = PointCloudPing(self.controller, name, params['pin'],
params['rate'], params['frame_id'])
elif params['type'] == "GP2D12":
sensor = GP2D12(self.controller, name, params['pin'],
params['rate'])
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller,
name,
params['pin'],
params['rate'],
direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller,
name,
params['pin'],
params['rate'],
direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name,
params['pin'], params['rate'])
elif params['type'] == 'PhidgetsVoltage':
sensor = PhidgetsVoltage(self.controller, name, params['pin'],
params['rate'])
elif params['type'] == 'PhidgetsCurrent':
sensor = PhidgetsCurrent(self.controller, name, params['pin'],
params['rate'])
# if params['type'] == "MaxEZ1":
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
self.mySensors.append(sensor)
rospy.loginfo(name + " " + str(params))
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.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 = 'base_link'
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
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):
self.controller.servo_read(req.id)
return ServoReadResponse()
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 shutdown(self):
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
rospy.loginfo("Shutting down Arduino Node...")
class ArduinoROS():
def __init__(self):
rospy.init_node('arduino', log_level=rospy.INFO)
# Get the actual node name in case it is set in the launch file
self.name = rospy.get_name()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
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_footprint')
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)
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()
# Initialize a Twist message
self.cmd_vel = Twist()
# Initialize the controlller
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 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():
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()
r.sleep()
def shutdown(self):
rospy.loginfo("Shutting down Arduino Node...")
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
# Close the serial port
try:
self.controller.close()
except:
pass
finally:
rospy.loginfo("Serial port closed.")
os._exit(0)
class ArduinoROS():
def __init__(self):
rospy.init_node('arduino', log_level=rospy.INFO)
# Get the actual node name in case it is set in the launch file
self.name = rospy.get_name()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyUSB0")
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.
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)
# 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.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'] == "DHT22":
sensor = DHT22(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == "GP2D12":
sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == 'PhidgetsVoltage':
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)
try:
self.mySensors.append(sensor)
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()
class ArduinoROS():
def __init__(self):
rospy.init_node('arduino', log_level=rospy.INFO)
# Find the actual node name in case it is set in the launch file
self.name = rospy.get_name()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
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", 30))
r = rospy.Rate(self.rate)
# Rate at which summary SensorState message is published. Individual sensors publish
# at their own rates.
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)
# 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
self.have_joints = 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 attach a PWM servo to a specified pin
rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler)
# A service to detach a PWM servo to a specified pin
rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler)
# 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_pin_mode', DigitalPinMode,
self.DigitalPinModeHandler)
# Obsolete: Use DigitalPinMode instead
rospy.Service('~digital_set_direction', DigitalSetDirection,
self.DigitalSetDirectionHandler)
# A service to turn set the direction of an analog pin (0 = input, 1 = output)
rospy.Service('~analog_pin_mode', AnalogPinMode,
self.AnalogPinModeHandler)
# 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)
# 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 device
self.device = Arduino(self.port,
self.baud,
self.timeout,
self.motors_reversed,
debug=True)
# Make the connection
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)
# Initialize the base controller if used
if self.use_base_controller:
self.base_controller = BaseController(
self.device, self.base_frame, self.name + "_base_controller")
# 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
dyn_server = dynamic_reconfigure.server.Server(
ROSArduinoBridgeConfig,
self.dynamic_reconfigure_server_callback,
namespace=self.name)
# Connect to the dynamic_reconfigure client
dyn_client = dynamic_reconfigure.client.Client(self.name,
timeout=5)
# Reserve a thread lock
mutex = thread.allocate_lock()
# Initialize any sensors
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
sensor_params = rospy.get_param("~sensors", dict({}))
# Initialize individual sensors appropriately
for name, params in sensor_params.iteritems():
if params['type'].lower() == 'Ping'.lower():
sensor = Ping(self.device, name, **params)
elif params['type'].lower() == 'GP2D12'.lower(
) or params['type'].lower() == 'GP2Y0A21YK0F'.lower():
sensor = GP2D12(self.device, name, **params)
elif params['type'].lower() == 'Digital'.lower():
sensor = DigitalSensor(self.device, name, **params)
elif params['type'].lower() == 'Analog'.lower():
sensor = AnalogSensor(self.device, name, **params)
elif params['type'].lower() == 'AnalogFloat'.lower():
sensor = AnalogFloatSensor(self.device, name, **params)
elif params['type'].lower() == 'PololuMotorCurrent'.lower():
sensor = PololuMotorCurrent(self.device, name, **params)
elif params['type'].lower() == 'PhidgetsVoltage'.lower():
sensor = PhidgetsVoltage(self.device, name, **params)
elif params['type'].lower() == 'PhidgetsCurrent'.lower():
sensor = PhidgetsCurrent(self.device, name, **params)
elif params['type'].lower() == 'IMU'.lower():
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():
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
try:
self.device.sensors.append(sensor)
if params['rate'] != None and params['rate'] != 0:
rospy.loginfo(name + " " + str(params) +
" published on topic " + rospy.get_name() +
"/sensor/" + name)
else:
if sensor.direction == "input":
rospy.loginfo(name + " service ready at " +
rospy.get_name() + "/" + name + "/read")
else:
rospy.loginfo(name + " service ready at " +
rospy.get_name() + "/" + name + "/write")
except:
rospy.logerr("Sensor type " +
str(params['type'] + " not recognized."))
# If at least one IMU or gyro is used for odometry, set the use_imu_heading flag on the base controller
if self.use_base_controller and len(self.imu_for_odom) > 0:
self.base_controller.use_imu_heading = True
# Read in the joints (if any)
joint_params = rospy.get_param("~joints", dict())
if len(joint_params) != 0:
self.have_joints = True
self.device.joints = dict()
self.device.joint_update_rate = float(
rospy.get_param("~joint_update_rate", 10))
# Configure each servo
for name, params in joint_params.iteritems():
try:
if params['continuous'] == True:
self.device.joints[name] = ContinuousServo(
self.device, name)
else:
self.device.joints[name] = Servo(self.device, name)
except:
self.device.joints[name] = Servo(self.device, name)
# Display the joint setup on the terminal
rospy.loginfo(name + " " + str(params))
# The servo controller determines when to read and write position values to the servos
self.servo_controller = ServoController(self.device,
"ServoController")
# The joint state publisher publishes the latest joint values on the /joint_states topic
self.joint_state_publisher = JointStatePublisher()
# Create the diagnostics updater for the Arduino device
self.device.diagnostics = DiagnosticsUpdater(
self,
self.name,
self.diagnotics_error_threshold,
self.diagnotics_rate,
create_watchdog=True)
# Create the overall diagnostics publisher
self.diagnostics_publisher = DiagnosticsPublisher(self)
# Initialize any trajectory action follow controllers
controllers = rospy.get_param("~controllers", dict())
self.device.controllers = list()
for name, params in controllers.items():
try:
controller = controller_types[params["type"]](self.device,
name)
self.device.controllers.append(controller)
except Exception as e:
if type(e) == KeyError:
rospy.logerr("Unrecognized controller: " + params["type"])
else:
rospy.logerr(str(type(e)) + str(e))
for controller in self.device.controllers:
controller.startup()
print "\n==> ROS Arduino Bridge ready for action!"
# Start polling the sensors, base controller, and servo controller
while not rospy.is_shutdown():
# Heartbeat/watchdog test for the serial connection
try:
# Update read counters
self.device.diagnostics.reads += 1
self.device.diagnostics.total_reads += 1
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:
# Update error counter
self.device.diagnostics.errors += 1
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()
with mutex:
while True:
try:
self.device.open()
while True:
self.device.serial_port.write('\r')
test = self.device.serial_port.readline(
).strip('\n').strip('\r')
rospy.loginfo("Waking up serial port...")
if test == 'Invalid Command':
self.device.serial_port.flushInput()
self.device.serial_port.flushOutput()
break
rospy.loginfo("Serial connection re-established.")
break
except:
r.sleep()
self.diagnostics_publisher.update()
continue
# 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
now = rospy.Time.now()
if now > self.t_next_sensors:
msg = SensorState()
msg.header.frame_id = self.base_frame
msg.header.stamp = now
for i in range(len(self.device.sensors)):
if self.device.sensors[i].message_type != MessageType.IMU:
msg.name.append(self.device.sensors[i].name)
msg.value.append(self.device.sensors[i].value)
try:
self.sensorStatePub.publish(msg)
except:
pass
self.t_next_sensors = now + self.t_delta_sensors
# Update diagnostics and publish
self.diagnostics_publisher.update()
r.sleep()
# Service callback functions
def ServoAttachHandler(self, req):
self.device.attach_servo(req.id)
return ServoAttachResponse()
def ServoDetachHandler(self, req):
self.device.detach_servo(req.id)
return ServoDetachResponse()
def ServoWriteHandler(self, req):
self.device.servo_write(req.id, req.position)
return ServoWriteResponse()
# def ServoSpeedWriteHandler(self, req):
# delay =
# self.device.servo_delay(req.id, delay)
# return ServoSpeedResponse()
#
# # Convert servo speed in deg/s to a step delay in milliseconds
# step_delay = self.device.joints[name].get_step_delay(req.value)
#
# # Update the servo speed
# self.device.config_servo(pin, step_delay)
#
# return SetServoSpeedResponse()
def ServoReadHandler(self, req):
position = self.device.servo_read(req.id)
return ServoReadResponse(position)
def DigitalPinModeHandler(self, req):
self.device.digital_pin_mode(req.pin, req.direction)
return DigitalPinModeResponse()
# Obsolete: use DigitalPinMode instead
def DigitalSetDirectionHandler(self, req):
self.device.digital_pin_mode(req.pin, req.direction)
return DigitalSetDirectionResponse()
def DigitalWriteHandler(self, req):
self.device.digital_write(req.pin, req.value)
return DigitalWriteResponse()
def DigitalReadHandler(self, req):
value = self.device.digital_read(req.pin)
return DigitalReadResponse(value)
def AnalogPinModeHandler(self, req):
self.device.analog_pin_mode(req.pin, req.direction)
return AnalogPinModeResponse()
def AnalogWriteHandler(self, req):
self.device.analog_write(req.pin, req.value)
return AnalogWriteResponse()
def AnalogReadHandler(self, req):
value = self.device.analog_read(req.pin)
return AnalogReadResponse(value)
def ResetOdometryHandler(self, req):
if self.use_base_controller:
self.base_controller.reset_odometry()
for imu in self.imu_for_odom:
imu.reset()
else:
rospy.logwarn("Not using base controller!")
return EmptyResponse()
def UpdatePIDHandler(self, req):
if self.use_base_controller:
self.device.update_pid(req.Kp, req.Kd, req.Ki, req.Ko)
rospy.loginfo(
"Updating PID parameters: Kp=%0.3f, Kd=%0.3f, Ki=%0.3f, Ko=%0.3f"
% (req.Kp, req.Kd, req.Ki, req.Ko))
else:
rospy.logwarn("Not using base controller!")
return UpdatePIDResponse()
def dynamic_reconfigure_server_callback(self, config, level):
if self.use_base_controller:
try:
if self.base_controller.Kp != config['Kp']:
self.base_controller.Kp = config['Kp']
if self.base_controller.Kd != config['Kd']:
self.base_controller.Kd = config['Kd']
if self.base_controller.Ki != config['Ki']:
self.base_controller.Ki = config['Ki']
if self.base_controller.Ko != config['Ko']:
self.base_controller.Ko = config['Ko']
if 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
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)
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))
except Exception as e:
print e
return config
def shutdown(self):
rospy.loginfo("Shutting down Arduino node...")
# Stop the robot
if self.use_base_controller:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(2)
# Detach any servos
if self.have_joints:
rospy.loginfo("Detaching servos...")
for joint in self.device.joints.values():
self.device.detach_servo(joint.pin)
rospy.sleep(0.2)
# Close the serial port
self.device.close()
class ArduinoROS():
def __init__(self):
rospy.init_node('arduino', log_level=rospy.INFO)
# Find the actual node name in case it is set in the launch file
self.name = rospy.get_name()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
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", 30))
r = rospy.Rate(self.rate)
# Rate at which summary SensorState message is published. Individual sensors publish
# at their own rates.
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)
# 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
self.have_joints = 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 attach a PWM servo to a specified pin
rospy.Service('~servo_attach', ServoAttach, self.ServoAttachHandler)
# A service to detach a PWM servo to a specified pin
rospy.Service('~servo_detach', ServoDetach, self.ServoDetachHandler)
# 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_pin_mode', DigitalPinMode, self.DigitalPinModeHandler)
# Obsolete: Use DigitalPinMode instead
rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
# A service to turn set the direction of an analog pin (0 = input, 1 = output)
rospy.Service('~analog_pin_mode', AnalogPinMode, self.AnalogPinModeHandler)
# 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)
# 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 device
self.device = Arduino(self.port, self.baud, self.timeout, self.motors_reversed, debug=True)
# Make the connection
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)
# Initialize the base controller if used
if self.use_base_controller:
self.base_controller = BaseController(self.device, self.base_frame, self.name + "_base_controller")
# 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
dyn_server = dynamic_reconfigure.server.Server(ROSArduinoBridgeConfig, self.dynamic_reconfigure_server_callback, namespace=self.name)
# Connect to the dynamic_reconfigure client
dyn_client = dynamic_reconfigure.client.Client (self.name, timeout=5)
# Reserve a thread lock
mutex = thread.allocate_lock()
# Initialize any sensors
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
sensor_params = rospy.get_param("~sensors", dict({}))
# Initialize individual sensors appropriately
for name, params in sensor_params.iteritems():
if params['type'].lower() == 'Ping'.lower():
sensor = Ping(self.device, name, **params)
elif params['type'].lower() == 'GP2D12'.lower() or params['type'].lower() == 'GP2Y0A21YK0F'.lower():
sensor = GP2D12(self.device, name, **params)
elif params['type'].lower() == 'Digital'.lower():
sensor = DigitalSensor(self.device, name, **params)
elif params['type'].lower() == 'Analog'.lower():
sensor = AnalogSensor(self.device, name, **params)
elif params['type'].lower() == 'AnalogFloat'.lower():
sensor = AnalogFloatSensor(self.device, name, **params)
elif params['type'].lower() == 'PololuMotorCurrent'.lower():
sensor = PololuMotorCurrent(self.device, name, **params)
elif params['type'].lower() == 'PhidgetsVoltage'.lower():
sensor = PhidgetsVoltage(self.device, name, **params)
elif params['type'].lower() == 'PhidgetsCurrent'.lower():
sensor = PhidgetsCurrent(self.device, name, **params)
elif params['type'].lower() == 'IMU'.lower():
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():
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
try:
self.device.sensors.append(sensor)
if params['rate'] != None and params['rate'] != 0:
rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
else:
if sensor.direction == "input":
rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/read")
else:
rospy.loginfo(name + " service ready at " + rospy.get_name() + "/" + name + "/write")
except:
rospy.logerr("Sensor type " + str(params['type'] + " not recognized."))
# If at least one IMU or gyro is used for odometry, set the use_imu_heading flag on the base controller
if self.use_base_controller and len(self.imu_for_odom) > 0:
self.base_controller.use_imu_heading = True
# Read in the joints (if any)
joint_params = rospy.get_param("~joints", dict())
if len(joint_params) != 0:
self.have_joints = True
self.device.joints = dict()
self.device.joint_update_rate = float(rospy.get_param("~joint_update_rate", 10))
# Configure each servo
for name, params in joint_params.iteritems():
try:
if params['continuous'] == True:
self.device.joints[name] = ContinuousServo(self.device, name)
else:
self.device.joints[name] = Servo(self.device, name)
except:
self.device.joints[name] = Servo(self.device, name)
# Display the joint setup on the terminal
rospy.loginfo(name + " " + str(params))
# The servo controller determines when to read and write position values to the servos
self.servo_controller = ServoController(self.device, "ServoController")
# The joint state publisher publishes the latest joint values on the /joint_states topic
self.joint_state_publisher = JointStatePublisher()
# Create the diagnostics updater for the Arduino device
self.device.diagnostics = DiagnosticsUpdater(self, self.name, self.diagnotics_error_threshold, self.diagnotics_rate, create_watchdog=True)
# Create the overall diagnostics publisher
self.diagnostics_publisher = DiagnosticsPublisher(self)
# Initialize any trajectory action follow controllers
controllers = rospy.get_param("~controllers", dict())
self.device.controllers = list()
for name, params in controllers.items():
try:
controller = controller_types[params["type"]](self.device, name)
self.device.controllers.append(controller)
except Exception as e:
if type(e) == KeyError:
rospy.logerr("Unrecognized controller: " + params["type"])
else:
rospy.logerr(str(type(e)) + str(e))
for controller in self.device.controllers:
controller.startup()
print "\n==> ROS Arduino Bridge ready for action!"
# Start polling the sensors, base controller, and servo controller
while not rospy.is_shutdown():
# Heartbeat/watchdog test for the serial connection
try:
# Update read counters
self.device.diagnostics.reads += 1
self.device.diagnostics.total_reads += 1
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:
# Update error counter
self.device.diagnostics.errors += 1
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()
with mutex:
while True:
try:
self.device.open()
while True:
self.device.serial_port.write('\r')
test = self.device.serial_port.readline().strip('\n').strip('\r')
rospy.loginfo("Waking up serial port...")
if test == 'Invalid Command':
self.device.serial_port.flushInput()
self.device.serial_port.flushOutput()
break
rospy.loginfo("Serial connection re-established.")
break
except:
r.sleep()
self.diagnostics_publisher.update()
continue
# 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
now = rospy.Time.now()
if now > self.t_next_sensors:
msg = SensorState()
msg.header.frame_id = self.base_frame
msg.header.stamp = now
for i in range(len(self.device.sensors)):
if self.device.sensors[i].message_type != MessageType.IMU:
msg.name.append(self.device.sensors[i].name)
msg.value.append(self.device.sensors[i].value)
try:
self.sensorStatePub.publish(msg)
except:
pass
self.t_next_sensors = now + self.t_delta_sensors
# Update diagnostics and publish
self.diagnostics_publisher.update()
r.sleep()
# Service callback functions
def ServoAttachHandler(self, req):
self.device.attach_servo(req.id)
return ServoAttachResponse()
def ServoDetachHandler(self, req):
self.device.detach_servo(req.id)
return ServoDetachResponse()
def ServoWriteHandler(self, req):
self.device.servo_write(req.id, req.position)
return ServoWriteResponse()
# def ServoSpeedWriteHandler(self, req):
# delay =
# self.device.servo_delay(req.id, delay)
# return ServoSpeedResponse()
#
# # Convert servo speed in deg/s to a step delay in milliseconds
# step_delay = self.device.joints[name].get_step_delay(req.value)
#
# # Update the servo speed
# self.device.config_servo(pin, step_delay)
#
# return SetServoSpeedResponse()
def ServoReadHandler(self, req):
position = self.device.servo_read(req.id)
return ServoReadResponse(position)
def DigitalPinModeHandler(self, req):
self.device.digital_pin_mode(req.pin, req.direction)
return DigitalPinModeResponse()
# Obsolete: use DigitalPinMode instead
def DigitalSetDirectionHandler(self, req):
self.device.digital_pin_mode(req.pin, req.direction)
return DigitalSetDirectionResponse()
def DigitalWriteHandler(self, req):
self.device.digital_write(req.pin, req.value)
return DigitalWriteResponse()
def DigitalReadHandler(self, req):
value = self.device.digital_read(req.pin)
return DigitalReadResponse(value)
def AnalogPinModeHandler(self, req):
self.device.analog_pin_mode(req.pin, req.direction)
return AnalogPinModeResponse()
def AnalogWriteHandler(self, req):
self.device.analog_write(req.pin, req.value)
return AnalogWriteResponse()
def AnalogReadHandler(self, req):
value = self.device.analog_read(req.pin)
return AnalogReadResponse(value)
def ResetOdometryHandler(self, req):
if self.use_base_controller:
self.base_controller.reset_odometry()
for imu in self.imu_for_odom:
imu.reset()
else:
rospy.logwarn("Not using base controller!")
return EmptyResponse()
def UpdatePIDHandler(self, req):
if self.use_base_controller:
self.device.update_pid(req.Kp, req.Kd, req.Ki, req.Ko)
rospy.loginfo("Updating PID parameters: Kp=%0.3f, Kd=%0.3f, Ki=%0.3f, Ko=%0.3f" %(req.Kp, req.Kd, req.Ki, req.Ko))
else:
rospy.logwarn("Not using base controller!")
return UpdatePIDResponse()
def dynamic_reconfigure_server_callback(self, config, level):
if self.use_base_controller:
try:
if self.base_controller.Kp != config['Kp']:
self.base_controller.Kp = config['Kp']
if self.base_controller.Kd != config['Kd']:
self.base_controller.Kd = config['Kd']
if self.base_controller.Ki != config['Ki']:
self.base_controller.Ki = config['Ki']
if self.base_controller.Ko != config['Ko']:
self.base_controller.Ko = config['Ko']
if 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
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)
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))
except Exception as e:
print e
return config
def shutdown(self):
rospy.loginfo("Shutting down Arduino node...")
# Stop the robot
if self.use_base_controller:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(2)
# Detach any servos
if self.have_joints:
rospy.loginfo("Detaching servos...")
for joint in self.device.joints.values():
self.device.detach_servo(joint.pin)
rospy.sleep(0.2)
# Close the serial port
self.device.close()
class ArduinoROS():
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG)
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0")
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.l_scale = rospy.get_param("~linear_scale", 10)
self.a_scale = rospy.get_param("~angular_scale", 90)
# 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.
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
rospy.Subscriber("html_joy_stick", Joy, self.JoystickCallback)
# 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=10)
# A pan_tilt publisher so we can control the position of the pan/tilt gimble and center it on shutdown
self.pan_tilt_pub = rospy.Publisher('pan_tilt', PanTilt, queue_size=10)
# The SensorState publisher periodically publishes the values of all sensors on
# a single topic.
self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=10)
# 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 set pwm values for the pins
rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
# Initialize the controller
self.controller = None
driver = rospy.get_param("~arduino_driver", "rab_driver")
params = { 'pid rate' : rospy.get_param("~base_controller_rate", 10) }
if driver == "rab_driver":
from ros_arduino_python.rab_driver import RABDriver
self.controller = RABDriver(params=params, port=self.port, baud=self.baud, timeout=self.timeout)
elif driver == "pymata_driver":
from ros_arduino_python.pymata_driver import PymataDriver
self.controller = PymataDriver(params=params, port=self.port, baud=self.baud, timeout=self.timeout)
else:
rospy.loginfo("Unknown Arduino driver")
return
# 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":
sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
elif params['type'] == 'PhidgetsVoltage':
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)
elif params['type'] == 'DigitalArray':
sensor = LineSensor(self.controller, name, params['pins'], params['pullup'], self.base_frame, rate=params['rate'], direction=params['direction'])
# if params['type'] == "MaxEZ1":
# self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
# self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
self.mySensors.append(sensor)
rospy.loginfo(name + " " + str(params))
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(self.controller, self.base_frame)
# 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
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
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):
self.controller.servo_read(req.id)
return ServoReadResponse()
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 AnalogWriteHandler(self, req):
self.controller.analog_write(req.pin, req.value)
return AnalogWriteResponse()
def JoystickCallback(self, data):
#rospy.loginfo("Received joystick msg: " + str(data))
# axes 0 and 1 are assigned to the drive train
# axes 2 and 3 are assigned to the pan/tilt
# Handle motor drive/turning
twist = Twist()
twist.linear.x = self.l_scale*data.axes[0] # forward/backward speed
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = self.a_scale*data.axes[1] # turning speed
self.cmd_vel_pub.publish(twist)
# Handle pan/tilt
# The value received on axes 3 and 4 ranges from -1.0 to 1.0
pan_tilt = PanTilt()
pan_tilt.pan = data.axes[2]
pan_tilt.tilt = data.axes[3]
#rospy.loginfo("Publishing: pan %f tilt %f" % (pan_tilt.pan, pan_tilt.tilt))
self.pan_tilt_pub.publish(pan_tilt)
def shutdown(self):
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
rospy.loginfo("Shutting down Arduino Node...")
class ArduinoROS():
def __init__(self):
# 初始化节点 设置 日志 等级
rospy.init_node('arduino', log_level=rospy.INFO)
# 得到实际的节点名称 （参数设置文件可能会更改名字）
self.name = rospy.get_name()
# 关闭时清理节点
rospy.on_shutdown(self.shutdown)
self.port = rospy.get_param("~port", "/dev/ttyACM0") # 端口
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.rate = int(rospy.get_param("~rate", 50))
r = rospy.Rate(self.rate)
# 传感器信息发布频率
self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
# 是否使用 ros内置的 基础控制器
self.use_base_controller = rospy.get_param("~use_base_controller",
False)
# 设置下一次发布传感器信息的时间
now = rospy.Time.now()
self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
self.t_next_sensors = now + self.t_delta_sensors
# 初始化 姿态信息（线速度、角速度）
self.cmd_vel = Twist()
# 发布速度指令消息到 cmd_vel话题
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# 发布传感器信息（值）到sensor_state话题
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)
# 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)
# 初始化ARDUINO控制器
self.controller = Arduino(self.port, self.baud, self.timeout)
# 连接
self.controller.connect()
rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
str(self.baud) + " baud")
# 创建一个锁对象LockType，使多个线程同步访问共享资源。 多线程
mutex = thread.allocate_lock()
# 初始化传感器列表
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": # ping传感器？
sensor = Ping(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == "GP2D12":
sensor = GP2D12(self.controller, name, params['pin'],
params['rate'], self.base_frame)
elif params['type'] == 'Digital':
sensor = DigitalSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'Analog':
sensor = AnalogSensor(self.controller,
name,
params['pin'],
params['rate'],
self.base_frame,
direction=params['direction'])
elif params['type'] == 'PololuMotorCurrent':
sensor = PololuMotorCurrent(self.controller, name,
params['pin'], params['rate'],
self.base_frame)
elif params['type'] == 'PhidgetsVoltage':
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']
try:
self.mySensors.append(sensor)
rospy.loginfo(name + " " + str(params) +
" published on topic " + rospy.get_name() +
"/sensor/" + name)
except:
rospy.logerr("Sensor type " + str(params['type']) +
" not recognized.")
# 初始化基本控制器
if self.use_base_controller:
self.myBaseController = BaseController(
self.controller, self.base_frame,
self.name + "_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(
) # 得到轮子当前的速度，发布里程记信息到 话题上，获取cmd_cel话题上的速度命令解析后得到轮子的速度，简单单限值处理后下发到下位机
mutex.release()
# 在 ~sensor_state 话题上发布所有使用的传感器的数据信息
now = rospy.Time.now()
if now > self.t_next_sensors:
msg = SensorState()
msg.header.frame_id = self.base_frame
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name) #列表
msg.value.append(self.mySensors[i].value)
try:
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...")
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
# Close the serial port
try:
self.controller.close()
except:
pass
finally:
rospy.loginfo("Serial port closed.")
os._exit(0)
class ArduinoROS():
def __init__(self):
rospy.init_node('arduino', log_level=rospy.INFO)
# Get the actual node name in case it is set in the launch file
self.name = rospy.get_name()
# Cleanup when termniating the node
rospy.on_shutdown(self.shutdown)
if rospy.get_param("~override_arduino_port") not in ['false', False]:
self.port = rospy.get_param("~override_arduino_port")
else:
self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 57600))
self.timeout = rospy.get_param("~timeout", 0.5)
self.robot_id = rospy.get_namespace().replace('/', '')
self.base_frame = rospy.get_param("~base_frame", 'base_link')
self.base_frame = '{}_{}'.format(self.robot_id, self.base_frame)
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.
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
)
# 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.motors_reversed
)
# Make the connection
self.controller.connect()
rospy.loginfo(
"Connected to Arduino on port {} at {} baud".format(
self.port, self.baud
)
)
# Reserve a thread lock
mutex = thread.allocate_lock()
# Initialize any sensors
self.mySensors = list()
sensor_params = rospy.get_param("~sensors", dict({}))
sensor_funcs = {
'Ping': Ping,
'GP2D12': GP2D12,
'Digital': DigitalSensor,
'Analog': AnalogSensor,
'PololuMotorCurrent': PololuMotorCurrent,
'PhidgetsVoltage': PhidgetsVoltage,
'PhidgetsCurrent': PhidgetsCurrent,
}
for name, params in sensor_params.iteritems():
# Set the direction to input if not specified
try:
params['direction']
except:
params['direction'] = 'input'
sensor = sensor_funcs[params['type']](
self.controller, name, params['pin'], params['rate'],
self.base_frame, direction=params['direction']
)
try:
self.mySensors.append(sensor)
rospy.loginfo(
"Sensor {}[{}] publishing on topic {}/sensor/{}".format(
name, params["type"], rospy.get_name(), name
)
)
except:
rospy.logerr(
"Sensor type {} not recognized.".format(params['type'])
)
# Initialize the base controller if used
if self.use_base_controller:
self.myBaseController = BaseController(
self.controller, self.base_frame,
"{}_base_controller".format(self.name)
)
# 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
msg.header.stamp = now
for i in range(len(self.mySensors)):
msg.name.append(self.mySensors[i].name)
msg.value.append(self.mySensors[i].value)
try:
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...")
# Stop the robot
try:
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.Publish(Twist())
rospy.sleep(2)
except:
pass
# Close the serial port
try:
self.controller.close()
except:
pass
finally:
rospy.loginfo("Serial port closed.")
os._exit(0)