From 2de2028192d95859d35085a73387940f280714d0 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Fri, 18 Dec 2015 15:25:13 -0800 Subject: [PATCH] Added ROS services for reading and writing from/to all named sensors --- ros_arduino_msgs/CMakeLists.txt | 8 ++ ros_arduino_msgs/msg/AnalogFloat.msg | 2 +- .../srv/AnalogFloatSensorRead.srv | 2 + .../srv/AnalogFloatSensorWrite.srv | 2 + ros_arduino_msgs/srv/AnalogSensorRead.srv | 2 + ros_arduino_msgs/srv/AnalogSensorWrite.srv | 2 + ros_arduino_msgs/srv/DigitalSensorRead.srv | 2 + .../srv/DigitalSensorSetDirection.srv | 2 + ros_arduino_msgs/srv/DigitalSensorWrite.srv | 2 + ros_arduino_python/nodes/arduino_node.py | 33 ++---- .../src/ros_arduino_python/arduino_sensors.py | 101 ++++++++++++------ 11 files changed, 105 insertions(+), 53 deletions(-) create mode 100755 ros_arduino_msgs/srv/AnalogFloatSensorRead.srv create mode 100644 ros_arduino_msgs/srv/AnalogFloatSensorWrite.srv create mode 100755 ros_arduino_msgs/srv/AnalogSensorRead.srv create mode 100644 ros_arduino_msgs/srv/AnalogSensorWrite.srv create mode 100755 ros_arduino_msgs/srv/DigitalSensorRead.srv create mode 100755 ros_arduino_msgs/srv/DigitalSensorSetDirection.srv create mode 100755 ros_arduino_msgs/srv/DigitalSensorWrite.srv diff --git a/ros_arduino_msgs/CMakeLists.txt b/ros_arduino_msgs/CMakeLists.txt index 91b7c09..0f66e15 100644 --- a/ros_arduino_msgs/CMakeLists.txt +++ b/ros_arduino_msgs/CMakeLists.txt @@ -13,12 +13,20 @@ add_message_files(FILES add_service_files(FILES AnalogWrite.srv + AnalogSensorWrite.srv + AnalogFloatSensorWrite.srv AnalogRead.srv + AnalogSensorRead.srv + AnalogFloatSensorRead.srv DigitalRead.srv + DigitalSensorRead.srv DigitalSetDirection.srv + DigitalSensorSetDirection.srv DigitalWrite.srv + DigitalSensorWrite.srv Enable.srv Relax.srv + AnalogSensorRead.srv ServoRead.srv ServoWrite.srv SetSpeed.srv diff --git a/ros_arduino_msgs/msg/AnalogFloat.msg b/ros_arduino_msgs/msg/AnalogFloat.msg index 1f78bb1..899697a 100644 --- a/ros_arduino_msgs/msg/AnalogFloat.msg +++ b/ros_arduino_msgs/msg/AnalogFloat.msg @@ -1,3 +1,3 @@ # Float message from a single analog IO pin. Header header -float64 value \ No newline at end of file +float32 value \ No newline at end of file diff --git a/ros_arduino_msgs/srv/AnalogFloatSensorRead.srv b/ros_arduino_msgs/srv/AnalogFloatSensorRead.srv new file mode 100755 index 0000000..be1ec07 --- /dev/null +++ b/ros_arduino_msgs/srv/AnalogFloatSensorRead.srv @@ -0,0 +1,2 @@ +--- +float32 value \ No newline at end of file diff --git a/ros_arduino_msgs/srv/AnalogFloatSensorWrite.srv b/ros_arduino_msgs/srv/AnalogFloatSensorWrite.srv new file mode 100644 index 0000000..effb167 --- /dev/null +++ b/ros_arduino_msgs/srv/AnalogFloatSensorWrite.srv @@ -0,0 +1,2 @@ +float32 value +--- diff --git a/ros_arduino_msgs/srv/AnalogSensorRead.srv b/ros_arduino_msgs/srv/AnalogSensorRead.srv new file mode 100755 index 0000000..ed4504b --- /dev/null +++ b/ros_arduino_msgs/srv/AnalogSensorRead.srv @@ -0,0 +1,2 @@ +--- +uint16 value \ No newline at end of file diff --git a/ros_arduino_msgs/srv/AnalogSensorWrite.srv b/ros_arduino_msgs/srv/AnalogSensorWrite.srv new file mode 100644 index 0000000..9ce3d3c --- /dev/null +++ b/ros_arduino_msgs/srv/AnalogSensorWrite.srv @@ -0,0 +1,2 @@ +uint16 value +--- diff --git a/ros_arduino_msgs/srv/DigitalSensorRead.srv b/ros_arduino_msgs/srv/DigitalSensorRead.srv new file mode 100755 index 0000000..e3e4ce7 --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalSensorRead.srv @@ -0,0 +1,2 @@ +--- +bool value diff --git a/ros_arduino_msgs/srv/DigitalSensorSetDirection.srv b/ros_arduino_msgs/srv/DigitalSensorSetDirection.srv new file mode 100755 index 0000000..2b76f95 --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalSensorSetDirection.srv @@ -0,0 +1,2 @@ +bool direction +--- diff --git a/ros_arduino_msgs/srv/DigitalSensorWrite.srv b/ros_arduino_msgs/srv/DigitalSensorWrite.srv new file mode 100755 index 0000000..662c8fe --- /dev/null +++ b/ros_arduino_msgs/srv/DigitalSensorWrite.srv @@ -0,0 +1,2 @@ +bool value +--- diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index 080778a..52e4dde 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -114,32 +114,20 @@ class ArduinoROS(): # Initialize individual sensors appropriately for name, params in sensor_params.iteritems(): - # Set the direction to input if not specified - try: - params['direction'] - except: - params['direction'] = 'input' - - # Set the frame_id to the base frame if not set - try: - params['frame_id'] - except: - params['frame_id'] = self.base_frame - if params['type'].lower() == 'Ping'.lower(): - sensor = Ping(self.device, name, params['pin'], params['rate'], params['frame_id']) + sensor = Ping(self.device, name, **params) elif params['type'].lower() == 'GP2D12'.lower(): - sensor = GP2D12(self.device, name, params['pin'], params['rate'], params['frame_id']) + sensor = GP2D12(self.device, name, **params) elif params['type'].lower() == 'Digital'.lower(): - sensor = DigitalSensor(self.device, name, params['pin'], params['rate'], params['frame_id'], direction=params['direction']) + sensor = DigitalSensor(self.device, name, **params) elif params['type'].lower() == 'Analog'.lower(): - sensor = AnalogSensor(self.device, name, params['pin'], params['rate'], params['frame_id'], direction=params['direction']) + sensor = AnalogSensor(self.device, name, **params) elif params['type'].lower() == 'PololuMotorCurrent'.lower(): - sensor = PololuMotorCurrent(self.device, name, params['pin'], params['rate'], self.base_frame) + sensor = PololuMotorCurrent(self.device, name, **params) elif params['type'].lower() == 'PhidgetsVoltage'.lower(): - sensor = PhidgetsVoltage(self.device, name, params['pin'], params['rate'], self.base_frame) + sensor = PhidgetsVoltage(self.device, name, **params) elif params['type'].lower() == 'PhidgetsCurrent'.lower(): - sensor = PhidgetsCurrent(self.device, name, params['pin'], params['rate'], self.base_frame) + sensor = PhidgetsCurrent(self.device, name, **params) # if params['type'].lower() == 'MaxEZ1'.lower(): # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] @@ -197,9 +185,10 @@ class ArduinoROS(): # Start polling the sensors, base controller, and servo controller while not rospy.is_shutdown(): for sensor in self.mySensors: - mutex.acquire() - sensor.poll() - mutex.release() + if sensor.rate != 0: + mutex.acquire() + sensor.poll() + mutex.release() if self.use_base_controller: mutex.acquire() diff --git a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py index d535f8d..e34e893 100755 --- a/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py +++ b/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py @@ -22,6 +22,7 @@ import rospy from sensor_msgs.msg import Range from ros_arduino_msgs.msg import * +from ros_arduino_msgs.srv import * from math import pow LOW = 0 @@ -29,7 +30,7 @@ HIGH = 1 INPUT = 0 OUTPUT = 1 - + class MessageType: ANALOG = 0 DIGITAL = 1 @@ -39,19 +40,21 @@ class MessageType: BOOL = 5 class Sensor(object): - def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs): - self.controller = controller + def __init__(self, device, name, pin=None, type=None, rate=0, direction="input", frame_id="base_link", **kwargs): + self.device = device self.name = name self.pin = pin + self.type = type self.rate = rate self.direction = direction - self.frame_id = frame_id + self.value = None - self.t_delta = rospy.Duration(1.0 / self.rate) - self.t_next = rospy.Time.now() + self.t_delta - + if self.rate != 0: + self.t_delta = rospy.Duration(1.0 / self.rate) + self.t_next = rospy.Time.now() + self.t_delta + def poll(self): now = rospy.Time.now() if now > self.t_next: @@ -87,20 +90,33 @@ class AnalogSensor(Sensor): self.msg = Analog() self.msg.header.frame_id = self.frame_id + # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5) if self.direction == "output": - self.controller.pin_mode(self.pin, OUTPUT) + self.device.pin_mode(self.pin, OUTPUT) + # Create the write service + rospy.Service('~' + self.name + '/write', AnalogSensorWrite, self.sensor_write_handler) else: - self.controller.pin_mode(self.pin, INPUT) + self.device.pin_mode(self.pin, INPUT) + # Create the read service + rospy.Service('~' + self.name + '/read', AnalogSensorRead, self.sensor_read_handler) self.value = LOW def read_value(self): - return self.controller.analog_read(self.pin) + return self.device.analog_read(self.pin) def write_value(self, value): - return self.controller.analog_write(self.pin, value) + return self.device.analog_write(self.pin, value) + + def sensor_read_handler(self, req=None): + value = self.read_value() + return AnalogSensorReadResponse(value) + + def sensor_write_handler(self, req): + self.write_value(req.value) + return AnalogSensorWriteResponse() class AnalogFloatSensor(Sensor): def __init__(self, *args, **kwargs): @@ -111,20 +127,33 @@ class AnalogFloatSensor(Sensor): self.msg = AnalogFloat() self.msg.header.frame_id = self.frame_id + # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5) if self.direction == "output": - self.controller.pin_mode(self.pin, OUTPUT) + self.device.pin_mode(self.pin, OUTPUT) + # Create the write service + rospy.Service('~' + self.name + '/write', AnalogFloatSensorWrite, self.sensor_write_handler) else: - self.controller.pin_mode(self.pin, INPUT) + self.device.pin_mode(self.pin, INPUT) + # Create the read service + rospy.Service('~' + self.name + '/read', AnalogFloatSensorRead, self.sensor_read_handler) self.value = LOW def read_value(self): - return self.controller.analog_read(self.pin) + return self.device.analog_read(self.pin) def write_value(self, value): - return self.controller.analog_write(self.pin, value) + return self.device.analog_write(self.pin, value) + + def sensor_read_handler(self, req=None): + value = self.read_value() + return AnalogFloatSensorReadResponse(value) + + def sensor_write_handler(self, req): + self.write_value(req.value) + return AnalogFloatSensorWriteResponse() class DigitalSensor(Sensor): @@ -136,33 +165,49 @@ class DigitalSensor(Sensor): self.msg = Digital() self.msg.header.frame_id = self.frame_id + # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5) if self.direction == "output": - self.controller.pin_mode(self.pin, OUTPUT) + self.device.pin_mode(self.pin, OUTPUT) + # Create the write service + rospy.Service('~' + self.name + '/write', DigitalSensorWrite, self.sensor_write_handler) else: - self.controller.pin_mode(self.pin, INPUT) + self.device.pin_mode(self.pin, INPUT) + # Create the read service + rospy.Service('~' + self.name + '/read', DigitalSensorRead, self.sensor_read_handler) self.value = LOW def read_value(self): - return self.controller.digital_read(self.pin) + return self.device.digital_read(self.pin) def write_value(self): # Alternate HIGH/LOW when writing at a fixed rate self.value = not self.value - return self.controller.digital_write(self.pin, self.value) + return self.device.digital_write(self.pin, self.value) + + def sensor_read_handler(self, req=None): + value = self.read_value() + return DigitalSensorReadResponse(value) + + def sensor_write_handler(self, req): + self.write_value(req.value) + return DigitalSensorWriteResponse() -class RangeSensor(Sensor): +class RangeSensor(AnalogFloatSensor): def __init__(self, *args, **kwargs): super(RangeSensor, self).__init__(*args, **kwargs) + self.direction = "input" + self.message_type = MessageType.RANGE self.msg = Range() self.msg.header.frame_id = self.frame_id + # Create the publisher self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5) def read_value(self): @@ -192,7 +237,7 @@ class Ping(SonarSensor): def read_value(self): # The Arduino Ping code returns the distance in centimeters - cm = self.controller.ping(self.pin) + cm = self.device.ping(self.pin) # Convert it to meters for ROS distance = cm / 100.0 @@ -209,7 +254,7 @@ class GP2D12(IRSensor): self.msg.max_range = 0.80 def read_value(self): - value = self.controller.analog_read(self.pin) + value = self.device.analog_read(self.pin) # The GP2D12 cannot provide a meaning result closer than 3 cm. @@ -237,7 +282,7 @@ class PololuMotorCurrent(AnalogFloatSensor): def read_value(self): # From the Pololu source code - milliamps = self.controller.analog_read(self.pin) * 34 + milliamps = self.device.analog_read(self.pin) * 34 return milliamps / 1000.0 class PhidgetsVoltage(AnalogFloatSensor): @@ -246,7 +291,7 @@ class PhidgetsVoltage(AnalogFloatSensor): def read_value(self): # From the Phidgets documentation - voltage = 0.06 * (self.controller.analog_read(self.pin) - 500.) + voltage = 0.06 * (self.device.analog_read(self.pin) - 500.) return voltage class PhidgetsCurrent(AnalogFloatSensor): @@ -255,7 +300,7 @@ class PhidgetsCurrent(AnalogFloatSensor): def read_value(self): # From the Phidgets documentation for the 20 amp DC sensor - current = 0.05 * (self.controller.analog_read(self.pin) - 500.) + current = 0.05 * (self.device.analog_read(self.pin) - 500.) return current class MaxEZ1Sensor(SonarSensor): @@ -270,10 +315,6 @@ class MaxEZ1Sensor(SonarSensor): self.msg.max_range = 3.0 def read_value(self): - return self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin) + return self.device.get_MaxEZ1(self.trigger_pin, self.output_pin) - -if __name__ == '__main__': - myController = Controller() - mySensor = SonarSensor(myController, "My Sonar", type=Type.PING, pin=0, rate=10) \ No newline at end of file