[sensors] Added an 'encoders' level to the velocity sensor

src/morse/builder/data.py

@@ -222,6 +222,11 @@
             "text": INTERFACE_DEFAULT_OUT,
             "pocolibs": ['morse.middleware.pocolibs.sensors.pom.PomSensorPoster',
                          'morse.middleware.pocolibs.sensors.pom.PomPoster']
+            },
+        "encoders": {
+            "socket": INTERFACE_DEFAULT_OUT,
+            "yarp": INTERFACE_DEFAULT_OUT,
+            "text": INTERFACE_DEFAULT_OUT
             }
         },
     "morse.sensors.pose.Pose": {

src/morse/sensors/odometry.py

@@ -1,8 +1,9 @@
 import logging; logger = logging.getLogger("morse." + __name__)
+import math
 from morse.helpers.morse_math import normalise_angle
 import morse.core.sensor
 import copy
-from morse.helpers.components import add_data, add_level
+from morse.helpers.components import add_data, add_level, add_property
 
 class Odometry(morse.core.sensor.Sensor):
     """
@@ -12,6 +13,16 @@ class Odometry(morse.core.sensor.Sensor):
 
     The angles for yaw, pitch and roll are given in radians.
 
+    It also supports a lower-level mode that returns encoder pulses
+    (`encoders` abstraction level). It assumes in that case a differential
+    drive robot whose forward direction is +X.
+
+
+    In the `encoders` mode, the properties `wheel_base` (distance between the
+    wheels), `wheel_radius` and `encoders_resolution` should be set according
+    to your robot (respective defaults are 0.3m, 0.1m and 32767).
+
+
     .. note::
       This sensor always provides perfect data.
       To obtain more realistic readings, it is recommended to add modifiers.
@@ -21,11 +32,13 @@ class Odometry(morse.core.sensor.Sensor):
     """
 
     _name = "Odometry"
-    _short_desc = "An odometry sensor that returns raw, partially integrated or fully integrated displacement information."
+    _short_desc = "An odometry sensor that returns encoder pulses, raw, partially " \
+                  "integrated or fully integrated displacement information."
 
     add_level("raw", "morse.sensors.odometry.RawOdometry", doc = "raw odometry: only dS is exported")
     add_level("differential", None, doc = "differential odometry, corresponding to standard 'robot level' odometry")
     add_level("integrated", "morse.sensors.odometry.IntegratedOdometry", doc = "integrated odometry: absolution position is exported", default=True)
+    add_level("encoders", "morse.sensors.odometry.Encoders", doc = "returns encoder ticks, assuming a differential drive model")
 
     add_data('dS', 0.0, "float","curvilinear distance since last tick", level = "raw")
     add_data('dx', 0.0, "float","delta of X coordinate of the sensor", level = "differential")
@@ -47,6 +60,20 @@ class Odometry(morse.core.sensor.Sensor):
     add_data('wy', 0.0, "float","angular velocity related to the Y coordinate of the sensor", level = "integrated")
     add_data('wx', 0.0, "float","angular velocity related to the X coordinate of the sensor", level = "integrated")
 
+    add_data('l_encoder', 0, "int", 'pulses of the left encoder', level = "encoders")
+    add_data('r_encoder', 0, "int", 'pulses of the right encoder', level = "encoders")
+
+
+    add_property("wheel_base", 0.3, "wheel_base", "int", 
+            "distance (in m) between the drive wheels (**note**: only useful "
+            "with the `encoders` mode)")
+    add_property("wheel_radius", 0.1, "wheel_radius", "int", "radius (in m)"
+            "of the wheels (**note**: only useful with the `encoders` mode)")
+    add_property("encoders_resolution", 32767, "encoders_resolution", "int", 
+            "number of encoder pulses for a 360° rotation (**note**: only "
+            "useful with the `encoders` mode)")
+
+
 
     def __init__(self, obj, parent=None):
         """ Constructor method.
@@ -137,3 +164,45 @@ def default_action(self):
         # Store the 'new' previous data
         self.previous_pos = current_pos
 
+class Encoders(Odometry):
+
+
+    def __init__(self, obj, parent=None):
+        # Call the constructor of the parent class
+        Odometry.__init__(self, obj, parent)
+
+        # Amount of pulses the encoders would generate if this robot drives
+        # 1 meter
+        self.pulses_per_meter = self.encoders_resolution / (2 * math.pi * self.wheel_radius)
+
+        # keeps those as float
+        self.l_encoder = 0.
+        self.r_encoder = 0.
+
+    def default_action(self):
+        """ Odometry calculation for the a differential drive robot, based on
+        ROS differential_drive package.
+
+        Note that this simple computation assumes that dtheta remains small
+        between two measurements.
+        """
+        # Compute the position of the sensor within the original frame
+        current_pos = self.original_pos.transformation3d_with(self.position_3d)
+
+        # Compute the difference in positions with the previous loop
+        delta_pos = self.previous_pos.transformation3d_with(current_pos)
+        dt = delta_pos.x
+        dw = delta_pos.yaw
+
+        # Store the 'new' previous data
+        self.previous_pos = current_pos
+
+        dl = dt - (self.wheel_base * dw) / 2.
+        dr = dt + (self.wheel_base * dw) / 2.
+
+        self.l_encoder += dl * self.pulses_per_meter
+        self.r_encoder += dr * self.pulses_per_meter
+
+        self.local_data['l_encoder'] = int(self.l_encoder)
+        self.local_data['r_encoder'] = int(self.r_encoder)
+

testing/base/odometry_testing.py

@@ -16,10 +16,22 @@
     pass
 
 class OdometryTest(MorseTestCase):
+
+    WHEEL_RADIUS = 1 / (2 * math.pi) # one full turn == 1 meter
+    WHEEL_BASE = 1 / math.pi # one 360° rotation of the robot leads the wheel to move 1m
+    ENCODERS_RESOLUTION = 10000
+
+
     def setUpEnv(self):
         """ Defines the test scenario, using the Builder API.
         """
-
+
+        # (constants need to appear here as well to be visible within MORSE)
+        import math
+        WHEEL_RADIUS = 1 / (2 * math.pi) # one full turn == 1 meter
+        WHEEL_BASE = 1 / math.pi # one 360° rotation of the robot leads the wheel to move 1m
+        ENCODERS_RESOLUTION = 10000
+
         robot = ATRV()
         robot.translate(x = 5.0, y = 2.0)
         robot.rotate(z = math.pi / 2)
@@ -46,6 +58,16 @@ def setUpEnv(self):
         robot.append(integ_odo)
         integ_odo.add_stream("socket")
 
+        encoders = Odometry()
+        encoders.level("encoders")
+        encoders.properties(wheel_radius = WHEEL_RADIUS,
+                            wheel_base = WHEEL_BASE,
+                            encoders_resolution = ENCODERS_RESOLUTION)
+
+        robot.append(encoders)
+        encoders.add_stream("socket")
+
+
         env = Environment('empty', fastmode = True)
         env.add_service('socket')
 
@@ -83,15 +105,19 @@ def verify(self, expected_x, expected_y, expected_yaw):
 
         pose = self.pose_stream.get()
         integ_odo = self.integ_odo_stream.get()
+
         self.assertAlmostEqual(self.x, expected_x, delta=precision)
         self.assertAlmostEqual(self.y, expected_y, delta=precision)
         self.assertAlmostEqual(self.yaw, expected_yaw, delta=precision)
+
         self.assertAlmostEqual(pose['x'], 5.0 - expected_y, delta=precision)
         self.assertAlmostEqual(pose['y'], 2.0 + expected_x, delta=precision)
         self.assertAlmostEqual(pose['yaw'], expected_yaw + math.pi/2, delta=precision)
+
         self.assertAlmostEqual(integ_odo['x'], expected_x, delta=precision)
         self.assertAlmostEqual(integ_odo['y'], expected_y, delta=precision)
         self.assertAlmostEqual(integ_odo['yaw'], expected_yaw, delta=precision)
+
         self.clear_datas(expected_x, expected_y, expected_yaw)
 
     def test_odometry(self):
@@ -124,6 +150,37 @@ def test_odometry(self):
             #self.odometry_test_helper(morse, -2.0, math.pi/2.0, 3.0)
             #self.verify(4.0 / math.pi, -4.0/math.pi, -math.pi/2.0)
 
+    def verify_encoders(self, left, right):
+        DELTA = 100
+        encoders = self.encoders_stream.get()
+
+        self.assertAlmostEqual(encoders["l_encoder"], left, delta = DELTA )
+        self.assertAlmostEqual(encoders["r_encoder"], right, delta = DELTA)
+
+    def test_encoders(self):
+
+
+        with Morse() as morse:
+            # Read the start position, it must be (0.0, 0.0, 0.0)
+            self.odo_stream = morse.robot.odo
+            self.encoders_stream = morse.robot.encoders
+            self.motion = morse.robot.motion
+
+            self.verify_encoders(0, 0)
+            self.odometry_test_helper(morse, 1.0, 0.0, 1.0)
+            self.verify_encoders(OdometryTest.ENCODERS_RESOLUTION, OdometryTest.ENCODERS_RESOLUTION)
+            self.odometry_test_helper(morse, -1.0, 0.0, 2.0)
+            self.verify_encoders(-OdometryTest.ENCODERS_RESOLUTION, -OdometryTest.ENCODERS_RESOLUTION)
+            self.odometry_test_helper(morse, 1.0, 0.0, 1.0)
+            self.verify_encoders(0, 0)
+            self.odometry_test_helper(morse, 0, math.pi, 2.0)
+            self.verify_encoders(OdometryTest.ENCODERS_RESOLUTION, -OdometryTest.ENCODERS_RESOLUTION)
+            self.odometry_test_helper(morse, 0, -math.pi, 2.0)
+            self.verify_encoders(0, 0)
+            self.odometry_test_helper(morse, 0, -math.pi, 2.0)
+            self.verify_encoders(-OdometryTest.ENCODERS_RESOLUTION, OdometryTest.ENCODERS_RESOLUTION)
+
+
 ########################## Run these tests ##########################
 if __name__ == "__main__":
     from morse.testing.testing import main