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