diff --git a/src/morse/builder/data.py b/src/morse/builder/data.py index 4d0494bca..aa6a0bb07 100644 --- a/src/morse/builder/data.py +++ b/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": { diff --git a/src/morse/sensors/odometry.py b/src/morse/sensors/odometry.py index d4ead75d4..a00d46564 100644 --- a/src/morse/sensors/odometry.py +++ b/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) + diff --git a/testing/base/odometry_testing.py b/testing/base/odometry_testing.py index 29bb843b4..67c1588fb 100755 --- a/testing/base/odometry_testing.py +++ b/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