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
|
#! /usr/bin/env python
"""
This script tests the SICK laser range sensor in 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 Sick_Test(MorseTestCase):
def setUpEnv(self):
""" Defines the test scenario, using the Builder API.
"""
robot = ATRV()
robot.rotate(z = math.pi)
robot.translate(x = -4.5)
sick = Sick('Sick')
sick.translate(z=0.9)
sick.properties(laser_range = 10.0, Visible_arc = False)
sick.create_laser_arc()
robot.append(sick)
sick.add_stream('socket')
env = Environment('indoors-1/boxes', fastmode = True)
env.add_service('socket')
def test_sick(self):
""" This test is guaranteed to be started only when the simulator
is ready.
"""
with Morse() as morse:
# Read the data from the sick sensor
self.sick_stream = morse.robot.Sick
sick = self.sick_stream.get()
# On the right of the sensor, nothing to hit. So position is
# (0.0, 0.0, 0.0) and distance == laser_range
for index in range(105, 180):
ray = sick['point_list'][index]
self.assertAlmostEqual(ray[0], 0.0)
self.assertAlmostEqual(ray[1], 0.0)
self.assertAlmostEqual(ray[2], 0.0)
length = sick['range_list'][index]
self.assertAlmostEqual(length, 10.0)
# In the center of the sensor, we hit the red block (situed
# near (-7, 0)).
# Distance to hit is near 2.5
for index in range(80, 100):
length = sick['range_list'][index]
self.assertAlmostEqual(length, 2.5, delta=0.05)
# Check some specific point. Hit 90 is the ray corresponding
# to angle math.pi/2 (in front of the robot). y is computed
# by trigonometry.
ray = sick['point_list'][90]
self.assertAlmostEqual(ray[0], 2.5, delta=0.05)
self.assertAlmostEqual(ray[1], 0.0, delta=0.05)
self.assertAlmostEqual(ray[2], 0.0, delta=0.05)
ray = sick['point_list'][85]
self.assertAlmostEqual(ray[0], 2.5, delta=0.05)
self.assertAlmostEqual(ray[1], -2.5 * math.tan(math.radians(5)),
delta=0.05)
self.assertAlmostEqual(ray[2], 0.0, delta=0.05)
ray = sick['point_list'][95]
self.assertAlmostEqual(ray[0], 2.5, delta=0.05)
self.assertAlmostEqual(ray[1], -2.5 * math.tan(math.radians(-5)),
delta=0.05)
self.assertAlmostEqual(ray[2], 0.0, delta=0.05)
# Then, there is a full empty sector
for index in range(105, 150):
ray = sick['point_list'][index]
self.assertAlmostEqual(ray[0], 0.0)
self.assertAlmostEqual(ray[1], 0.0)
self.assertAlmostEqual(ray[2], 0.0)
length = sick['range_list'][index]
self.assertAlmostEqual(length, 10.0)
# The last ray hit the green block
# Distance and real position are a bit complicated to
# compute manually, so don't check real precision. Just
# verify that the distance is near 5.8
#print([i for i,r in enumerate(sick['range_list']) if 5<r<6])
for index in range(16, 22):
length = sick['range_list'][index]
self.assertAlmostEqual(length, 5.8, delta=0.15)
########################## Run these tests ##########################
if __name__ == "__main__":
from morse.testing.testing import main
main(Sick_Test)
|
