Source code for base.odometry_testing

#! /usr/bin/env python
"""
This script tests some of the base functionalities of MORSE.
"""

import sys
import math
from morse.testing.testing import MorseTestCase
from pymorse import Morse

# Include this import to be able to use your test file as a regular 
# builder script, ie, usable with: 'morse [run|exec] base_testing.py
try:
    from morse.builder import *
except ImportError:
    pass

[docs]class OdometryTest(MorseTestCase):
[docs] def setUpEnv(self): """ Defines the test scenario, using the Builder API. """ robot = ATRV() robot.translate(x = 5.0, y = 2.0) robot.rotate(z = math.pi / 2) pose = Pose() robot.append(pose) pose.add_stream('socket') motion = MotionVW('motion') robot.append(motion) motion.add_stream('socket') odo = Odometry() robot.append(odo) odo.level('differential') odo.add_stream('socket') raw_odo = Odometry() raw_odo.level("raw") robot.append(raw_odo) raw_odo.add_stream('socket') integ_odo = Odometry() robot.append(integ_odo) integ_odo.add_stream("socket") env = Environment('empty', fastmode = True) env.add_service('socket')
[docs] def clear_datas(self, x, y, yaw): self.x = x self.y = y self.yaw = yaw
[docs] def record_datas(self, record): dx = record['dx'] dy = record['dy'] self.yaw += record['dyaw'] # normalise angle while (self.yaw > math.pi): self.yaw+= -2 * math.pi while (self.yaw < -math.pi): self.yaw+= 2 * math.pi self.x += dx self.y += dy
[docs] def odometry_test_helper(self, morse, v, w, t): self.odo_stream.subscribe(self.record_datas) self.motion.publish({'v':v, 'w':w}) morse.sleep(t + 0.1) self.odo_stream.unsubscribe(self.record_datas)
[docs] def verify(self, expected_x, expected_y, expected_yaw): # Numerical integration is maybe not really good, so test with a # precision of 0.16 precision = 0.16 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)
[docs] def test_odometry(self): """ This test is guaranteed to be started only when the simulator is ready. """ with Morse() as morse: # Read the start position, it must be (0.0, 0.0, 0.0) self.pose_stream = morse.robot.pose self.odo_stream = morse.robot.odo self.integ_odo_stream = morse.robot.integ_odo self.motion = morse.robot.motion self.clear_datas(0.0, 0.0, 0.0) self.odometry_test_helper(morse, 1.0, 0.0, 2.0) self.verify(2.0, 0.0, 0.0) self.odometry_test_helper(morse, -1.0, 0.0, 2.0) self.verify(0.0, 0.0, 0.0) self.odometry_test_helper(morse, 1.0, -math.pi/4.0, 2.0) self.verify(4.0 / math.pi, -4.0/math.pi, -math.pi/2.0) self.odometry_test_helper(morse, 0.5, -math.pi/8.0, 12.0) self.verify(0.0, 0.0, 0.0) # XXX fail Y with 0.11 delta #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) ########################## Run these tests ##########################
if __name__ == "__main__": from morse.testing.testing import main main(OdometryTest)