Added ROS services for reading and writing from/to all named sensors

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

ros_arduino_msgs/msg/AnalogFloat.msg

@@ -1,3 +1,3 @@
 # Float message from a single analog IO pin.
 Header header
-float64 value
+float32 value

ros_arduino_msgs/srv/AnalogFloatSensorRead.srv

@@ -0,0 +1,2 @@
+---
+float32 value

ros_arduino_msgs/srv/AnalogFloatSensorWrite.srv

@@ -0,0 +1,2 @@
+float32 value
+---

ros_arduino_msgs/srv/AnalogSensorRead.srv

@@ -0,0 +1,2 @@
+---
+uint16 value

ros_arduino_msgs/srv/AnalogSensorWrite.srv

@@ -0,0 +1,2 @@
+uint16 value
+---

ros_arduino_msgs/srv/DigitalSensorRead.srv

@@ -0,0 +1,2 @@
+---
+bool value

ros_arduino_msgs/srv/DigitalSensorSetDirection.srv

@@ -0,0 +1,2 @@
+bool direction
+---

ros_arduino_msgs/srv/DigitalSensorWrite.srv

@@ -0,0 +1,2 @@
+bool value
+---

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()
+                if sensor.rate != 0:
-                sensor.poll()
+                    mutex.acquire()
-                mutex.release()
+                    sensor.poll()
+                    mutex.release()
                     
             if self.use_base_controller:
                 mutex.acquire()

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):
+    def __init__(self, device, name, pin=None, type=None, rate=0, direction="input", frame_id="base_link", **kwargs):
-        self.controller = controller
+        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)
+        if self.rate != 0:
-        self.t_next = rospy.Time.now() + self.t_delta
+            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)