1#! /usr/bin/env python
2"""
3This script tests the 'data stream' oriented feature of the socket interface.
4"""
5
6from morse.testing.testing import MorseTestCase
7
8try:
9    # Include this import to be able to use your test file as a regular
10    # builder script, ie, usable with: 'morse [run|exec] <your test>.py
11    from morse.builder import *
12except ImportError:
13    pass
14
15import math
16from pymorse import Morse
17
18class RadarAltimeterTest(MorseTestCase):
19
20    def setUpEnv(self):
21
22        robot = RMax('robot')
23        robot.translate(0.0, 0.0, 20.0)
24
25        teleport = Teleport()
26        teleport.add_stream('socket')
27        robot.append(teleport)
28
29        altimeter = RadarAltimeter()
30        robot.append(altimeter)
31        altimeter.add_stream('socket')
32        altimeter.properties(MaxRange = 25.0)
33
34        env = Environment('indoors-1/boxes', fastmode = True)
35        env.add_service('socket')
36
37        ground = bpymorse.get_object('Ground')
38        ground.game.use_actor = True
39
40    def test_teleport(self):
41        """ Test if we can connect to the pose data stream, and read from it.
42        """
43
44        with Morse() as morse:
45            teleport_stream = morse.robot.teleport
46            altimeter_stream = morse.robot.altimeter
47
48            precision = 0.01
49
50            altitude = altimeter_stream.get()
51            self.assertAlmostEquals(altitude['z'], 20.0, delta = precision)
52
53            teleport_stream.publish({'x': 0.0, 'y': 0.0, 'z': 15.0,
54                'yaw': 0.0, 'pitch': 0.0, 'roll': 0.0})
55            morse.sleep(0.1)
56
57            altitude = altimeter_stream.get()
58            self.assertAlmostEquals(altitude['z'], 15.0, delta = precision)
59
60            teleport_stream.publish({'x': 0.0, 'y': 0.0, 'z': 30.0,
61                'yaw': 0.0, 'pitch': 0.0, 'roll': 0.0})
62            morse.sleep(0.1)
63
64            altitude = altimeter_stream.get()
65            self.assertEqual(altitude['z'], float('inf'))
66
67            teleport_stream.publish({'x': 0.0, 'y': 0.0, 'z': 12.0,
68                'yaw': 0.0, 'pitch': math.pi/2, 'roll': math.pi/2})
69            morse.sleep(0.1)
70
71            altitude = altimeter_stream.get()
72            self.assertAlmostEquals(altitude['z'], 12.0, delta = precision)
73
74            # Goes on top on the red box, size ~1.8m
75            teleport_stream.publish({'x': -7.5, 'y': 0.0, 'z': 15.0,
76                'yaw': 0.0, 'pitch': 0.0, 'roll': 0.0})
77            morse.sleep(0.1)
78
79            altitude = altimeter_stream.get()
80            self.assertAlmostEquals(altitude['z'], 13.14, delta = precision)
81
82
83########################## Run these tests ##########################
84if __name__ == "__main__":
85    from morse.testing.testing import main
86    main(RadarAltimeterTest)
87