def __init__(self):
rospy.init_node('arduino_servo_node', log_level=rospy.INFO)
self.dynamic_client = DynamicReconfig.Client("arduino_dynamic_reconfig",timeout=10,config_callback=self.dynamic_callBack)
# Get the actual node name in case it is set in the launch file
self.name = rospy.get_name()
rospy.on_shutdown(self.shutdown)
# Get all param
self.getAllParams()
#subscrib topic to move servo
rospy.Subscriber(self.moveServoTopic, MoveServo, self.moveServoCB)
#define publisher to pub arduino firmware version
self.versionPub = rospy.Publisher(self.firmwareVerTopic, UInt16, queue_size=1)
#define all services
self.defineAllServices()
# Initialize the controlller
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")
# Enable servo x and servo y
self.controller.servo_enable(self.servo_x_id, 1)
self.controller.servo_enable(self.servo_y_id, 1)
# while loop
while not rospy.is_shutdown():
rate = rospy.Rate(self.pubRate)
firmwareVersion = self.controller.get_firmware_version()
self.versionPub.publish(firmwareVersion)
rate.sleep()
def __init__(self):
rospy.init_node('Arduino', log_level=rospy.DEBUG)
# Cleanup when terminating the node
rospy.on_shutdown(self.shutdown)
# 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)
# loop rate
self.node_rate = int(rospy.get_param("~node_rate", 50))
r = rospy.Rate(self.node_rate)
# create instance of the Arduino driver
self.port = rospy.get_param("~port", "/dev/ttyACM0")
self.baud = int(rospy.get_param("~baud", 115200))
self.com_timeout = rospy.get_param("~com_timeout", 0.5)
rospy.loginfo("Requesting Arduino on port " + self.port + " at " +
str(self.baud) + " baud with communication timeout " +
str(self.com_timeout) + " s")
self.controller = Arduino(self.port, self.baud, self.com_timeout)
self.controller.connect()
self.controller.reset_odometry()
rospy.loginfo("Connected to Arduino on port " + self.port + " at " +
str(self.baud) + " baud")
# inform Arduino about IP address
ip = socket.gethostbyname(socket.gethostname())
rospy.loginfo("send IP to Arduino: " + ip)
self.controller.set_ip(ip)
# Initialize the base controller
self.base_frame = rospy.get_param("~base_frame", 'base_link')
self.myBaseController = BaseController(self.controller,
self.base_frame)
# Reserve a thread lock
mutex = thread.allocate_lock()
# Start polling the base controller
while not rospy.is_shutdown():
mutex.acquire()
self.myBaseController.poll()
mutex.release()
r.sleep()
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, rospy.loginfo)
# 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'])
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))
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()
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", 115200))
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()
class ArduinoROS():
def __init__(self):
rospy.init_node('arduino_servo_node', log_level=rospy.INFO)
self.dynamic_client = DynamicReconfig.Client("arduino_dynamic_reconfig",timeout=10,config_callback=self.dynamic_callBack)
# Get the actual node name in case it is set in the launch file
self.name = rospy.get_name()
rospy.on_shutdown(self.shutdown)
# Get all param
self.getAllParams()
#subscrib topic to move servo
rospy.Subscriber(self.moveServoTopic, MoveServo, self.moveServoCB)
#define publisher to pub arduino firmware version
self.versionPub = rospy.Publisher(self.firmwareVerTopic, UInt16, queue_size=1)
#define all services
self.defineAllServices()
# Initialize the controlller
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")
# Enable servo x and servo y
self.controller.servo_enable(self.servo_x_id, 1)
self.controller.servo_enable(self.servo_y_id, 1)
# while loop
while not rospy.is_shutdown():
rate = rospy.Rate(self.pubRate)
firmwareVersion = self.controller.get_firmware_version()
self.versionPub.publish(firmwareVersion)
rate.sleep()
def dynamicReconfig(self, paramList):
self.pubRate = paramList['firmware_pub_rate']
self.default_x_deg = paramList['default_x_deg']
self.default_y_deg = paramList['default_y_deg']
self.move_delay = paramList['servo_move_delay']
#rospy.loginfo("(%f,%d,%d,%d)" %(self.pubRate,self.default_x_deg,self.default_y_deg,self.move_delay))
def dynamic_callBack(self, config):
dynamic_param = self.dynamic_client.get_configuration(timeout=3)
self.dynamicReconfig(dynamic_param)
def defineAllServices(self):
# Two services to position/read postion a PWM servo
rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
# Service to enable a servo
rospy.Service('~servo_enable', ServoEnable, self.ServoEnableHandler)
# Two services to get all servos position and enabled status
rospy.Service('~get_all_servos_pos', GetAllServoPos, self.GetAllServoPosHandler)
rospy.Service('~get_all_servos_enable', GetAllServoEnable, self.GetAllServoEnableHandler)
def moveServoCB(self, msgData):
self.moveServo(msgData.servoId, msgData.targetPos, msgData.delay)
def getAllParams(self):
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.servo_x_id = rospy.get_param("~servo_x_id", 3)
self.servo_y_id = rospy.get_param("~servo_y_id", 4)
self.pubRate = rospy.get_param("~version_pub_rate", 0.1)
self.move_delay = rospy.get_param("~servo_move_delay", 50)
self.moveServoTopic = rospy.get_param("~move_servo_topic", "/arduino_servo_node/moveServoTopic")
self.firmwareVerTopic = rospy.get_param("~firmware_version_topic", "/arduino_servo_node/firmwareVersionTopic")
self.default_x_deg = rospy.get_param("~default_x_deg", 90)
self.default_y_deg = rospy.get_param("~default_y_deg", 0)
# Service callback functions
def ServoWriteHandler(self, req):
self.controller.servo_write(req.id, req.value, req.delay)
return ServoWriteResponse()
def ServoReadHandler(self, req):
pos = self.controller.servo_read(req.id)
return ServoReadResponse(pos)
def ServoEnableHandler(self, req):
self.controller.servo_enable(req.id, req.flag)
return ServoEnableResponse()
def GetAllServoPosHandler(self, req):
positions = []
positions = self.controller.getAllServosPos()
return GetAllServoPosResponse(positions)
def GetAllServoEnableHandler(self, req):
enabled = []
enabled = self.controller.getAllServosEnabled()
return GetAllServoEnableResponse(enabled)
def moveServo(self, servo_id, move_angular, move_delay):
self.controller.servo_write(servo_id, move_angular, move_delay)
def enableServoByID(self, servo_id, flag):
self.controller.servo_enable(servo_id, flag)
def shutdown(self):
rospy.logwarn("Shutting down arduino_servo_node ...")
# init servo position to origin
self.moveServo(self.servo_x_id, self.default_x_deg, self.move_delay)
self.moveServo(self.servo_y_id, self.default_y_deg, self.move_delay)
# Close the serial port
try:
self.controller.close()
except:
pass
finally:
rospy.logwarn("Arduino serial port closed.")
os._exit(0)
(inches). The sensor distance resolution is integer based. Also, the
maxsonar trigger pin is RX, and the echo pin is PW.
'''
return self.execute('z %d %d' % (triggerPin, outputPin))
""" Basic test for connectivity """
if __name__ == "__main__":
if os.name == "posix":
portName = "/dev/ttyACM0"
else:
portName = "COM43" # Windows style COM port.
baudRate = 57600
myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
myArduino.connect()
print "Sleeping for 1 second..."
time.sleep(1)
print "Reading on analog port 0", myArduino.analog_read(0)
print "Reading on digital port 0", myArduino.digital_read(0)
print "Blinking the LED 3 times"
for i in range(3):
myArduino.digital_write(13, 1)
time.sleep(1.0)
#print "Current encoder counts", myArduino.encoders()
print "Connection test successful.",
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'] == "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()
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)