1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
|
#! /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
class OdometryTest(MorseTestCase):
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')
def clear_datas(self, x, y, yaw):
self.x = x
self.y = y
self.yaw = yaw
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
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)
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)
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)
|
