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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
|
#! /usr/bin/env python
"""
This script tests some of the base functionalities of MORSE.
"""
import sys
import math
from morse.testing.testing import MorseMoveTestCase
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 Accelero_Test(MorseMoveTestCase):
def setUpEnv(self):
""" Defines the test scenario, using the Builder API.
"""
robot = ATRV()
motion = MotionXYW()
robot.append(motion)
motion.add_stream('socket')
motion.add_service('socket')
accel = Accelerometer()
robot.append(accel)
accel.add_stream('socket')
accel_pi = Accelerometer()
robot.append(accel_pi)
accel_pi.rotate(z = math.pi / 2)
accel_pi.add_stream('socket')
env = Environment('empty', fastmode = True)
env.add_service('socket')
# The Blender control is not 'perfect' and the speed is so not
# perfectly constant, leading to 'small' acceleration.
def assert_accel_almost_null(self, acc, idx):
self.assertGreater(acc['acceleration'][idx], -1.2)
self.assertLess(acc['acceleration'][idx], 1.2)
def test_accel_sensor(self):
with Morse() as morse:
delta = 0.06
xyw = morse.robot.motion
accel_stream = morse.robot.accel
accel_pi_stream = morse.robot.accel_pi
xyw.publish({'x' : 1.0, 'y': 0.0, 'w' : 0.0})
morse.sleep(0.01)
acc = accel_stream.get()
acc_pi = accel_pi_stream.last()
self.assertGreater(acc['acceleration'][0], 50)
self.assert_accel_almost_null(acc, 1)
self.assertAlmostEqual(acc['velocity'][0], 1.0, delta = delta)
self.assertAlmostEqual(acc['velocity'][1], 0.0, delta = delta)
# acceleration phase
self.assertLess(acc_pi['acceleration'][1], -50)
self.assert_accel_almost_null(acc_pi, 0)
self.assertAlmostEqual(acc_pi['velocity'][0], 0.0, delta = delta)
self.assertAlmostEqual(acc_pi['velocity'][1], -1.0, delta = delta)
morse.sleep(0.1)
acc = accel_stream.get()
acc_pi = accel_pi_stream.get()
self.assert_accel_almost_null(acc, 0)
self.assert_accel_almost_null(acc, 1)
self.assertAlmostEqual(acc['velocity'][0], 1.0, delta = delta)
self.assertAlmostEqual(acc['velocity'][1], 0.0, delta = delta)
self.assertAlmostEquals(acc['distance'], 1/60, delta = delta)
self.assert_accel_almost_null(acc_pi, 0)
self.assert_accel_almost_null(acc_pi, 1)
self.assertAlmostEqual(acc_pi['velocity'][1], -1.0, delta = delta)
self.assertAlmostEqual(acc_pi['velocity'][0], 0.0, delta = delta)
self.assertAlmostEquals(acc_pi['distance'], 1/60, delta = delta)
xyw.publish({'x' : 0.0, 'y': 0.0, 'w' : 0.0})
morse.sleep(0.01)
# decacceleration phase
acc = accel_stream.last()
acc_pi = accel_pi_stream.last()
self.assertLess(acc['acceleration'][0], -50)
self.assert_accel_almost_null(acc, 1)
self.assertAlmostEqual(acc['velocity'][0], 0.0, delta = delta)
self.assertAlmostEqual(acc['velocity'][1], 0.0, delta = delta)
self.assertAlmostEquals(acc['distance'], 0.0, delta = delta)
self.assert_accel_almost_null(acc_pi, 0)
self.assertGreater(acc_pi['acceleration'][1], 50)
self.assertAlmostEqual(acc_pi['velocity'][0], 0.0, delta = delta)
self.assertAlmostEqual(acc_pi['velocity'][1], 0.0, delta = delta)
self.assertAlmostEquals(acc_pi['distance'], 0.0, delta = delta)
morse.sleep(0.1)
acc = accel_stream.get()
acc_pi = accel_pi_stream.get()
self.assert_accel_almost_null(acc, 0)
self.assert_accel_almost_null(acc, 1)
self.assertAlmostEqual(acc['velocity'][0], 0.0, delta = delta)
self.assertAlmostEqual(acc['velocity'][1], 0.0, delta = delta)
self.assertAlmostEquals(acc['distance'], 0.0, delta = delta)
self.assert_accel_almost_null(acc_pi, 0)
self.assert_accel_almost_null(acc_pi, 1)
self.assertAlmostEqual(acc_pi['velocity'][0], 0.0, delta = delta)
self.assertAlmostEqual(acc_pi['velocity'][1], 0.0, delta = delta)
self.assertAlmostEquals(acc_pi['distance'], 0.0, delta = delta)
xyw.publish({'x' : 0.0, 'y': 1.0, 'w' : 0.0})
morse.sleep(0.1)
acc = accel_stream.get()
acc_pi = accel_pi_stream.get()
self.assert_accel_almost_null(acc, 0)
self.assert_accel_almost_null(acc, 1)
self.assertAlmostEqual(acc['velocity'][0], 0.0, delta = delta)
self.assertAlmostEqual(acc['velocity'][1], 1.0, delta = delta)
self.assertAlmostEquals(acc['distance'], 1/60, delta = delta)
self.assert_accel_almost_null(acc_pi, 0)
self.assert_accel_almost_null(acc_pi, 1)
self.assertAlmostEqual(acc_pi['velocity'][0], 1.0, delta = delta)
self.assertAlmostEqual(acc_pi['velocity'][1], 0.0, delta = delta)
self.assertAlmostEquals(acc_pi['distance'], 1/60, delta = delta)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(Accelero_Test)
|
