def test_odom(self): origin = np.asarray([10.0, 0.0, 0.0]) inc = np.asarray([100, 0.0, 0.0]) bus = Bus(MagicMock(write=MagicMock(return_value=timedelta()))) loc = Localization({}, bus.handle('loc')) loc.xyz = origin tester = bus.handle('tester') tester.register('orientation', 'odom') bus.connect('loc.pose3d', 'tester.pose3d') bus.connect('tester.orientation', 'loc.orientation') bus.connect('tester.odom', 'loc.odom') loc.start() tester.publish('orientation', quaternion.identity()) tester.publish('odom', 0 * inc) tester.publish('odom', 1 * inc) tester.publish('odom', 2 * inc) dt, channel, pose3d = tester.listen() self.assertEqual(channel, 'pose3d') self.assertPose3dEqual(pose3d, [list(origin), quaternion.identity()]) dt, channel, pose3d = tester.listen() self.assertPose3dEqual( pose3d, [list(origin + inc / 1000), quaternion.identity()]) dt, channel, pose3d = tester.listen() self.assertPose3dEqual( pose3d, [list(origin + 2 * inc / 1000), quaternion.identity()]) loc.request_stop() loc.join()
def test_infinity(self): log = MagicMock() log.write.return_value = timedelta() bus = Bus(log) tester = bus.handle('tester') tester.register("scan", "rs_scan") mixer = ScanMixer(config={}, bus=bus.handle('mixer')) bus.connect("tester.scan", "mixer.scan") bus.connect("tester.rs_scan", "mixer.rs_scan") bus.connect("mixer.scan", "tester.mixed") mixer.start() scan = 3 * [10] + [0] rs_scan = 3 * [8] tester.publish("rs_scan", rs_scan) tester.publish("scan", scan) _, channel, data = tester.listen() self.assertEqual(data, 4 * [8]) scan = 4 * [10] rs_scan = 2 * [8] + [0] tester.publish("rs_scan", rs_scan) tester.publish("scan", scan) _, channel, data = tester.listen() self.assertEqual(data, 3 * [8] + [10]) mixer.request_stop() mixer.join()
def test_pose2d_moves(self): logger = MagicMock() logger.write = MagicMock(return_value=datetime.timedelta(microseconds=9721)) bus = Bus(logger) moves = [ [Translation(0, 0, -1), [1000, 0, 0]], # forward [Translation(-1, 0, 0), [0, 1000, 0]], # left ] c = RealSense(bus=bus.handle('rs'), config={}) tester = bus.handle('tester') bus.connect('rs.pose2d', 'tester.pose2d') for input, output in moves: frame = MagicMock() frame.get_frame_number.return_value = 1 frame.get_timestamp.return_value = 1 pose_frame = frame.as_pose_frame.return_value pose_frame.get_pose_data.return_value = Pose( Acceleration(0, 0, 0), AngularAcceleration(0, 0, 0), AngularVelocity(0, 0, 0), 0, Rotation(0, 0, 0, 1), 0, input, Velocity(0, 0, 0)) frame.get_timestamp.return_value = 0 frame.get_frame_number.return_value = 0 c.pose_callback(frame) dt, channel, pose2d = tester.listen() self.assertEqual(channel, 'pose2d') self.assertEqual(pose2d, output)
def test_http_sleep(self): # reported as bug for IP camera running at full speed with patch('osgar.drivers.logsocket.urllib.request.urlopen') as mock: instance = mock.return_value instance.__enter__.return_value.read = MagicMock(return_value=b'123') logger = MagicMock() logger.register = MagicMock(return_value=1) logger.write = MagicMock(return_value=123) bus = Bus(logger) config = { "url": "http://192.168.0.99/img.jpg", "sleep": 0.1, "timeout": 1.0 } device = LogHTTP(config=config, bus=bus.handle('http')) tester = bus.handle('tester') bus.connect('http.raw', 'tester.raw') device.start() data = tester.listen() self.assertEqual(data, (123, 'raw', b'123')) device.request_stop() device.join() self.assertEqual( len(instance.__enter__.return_value.read.call_args_list), 1) # it should be just one call and sleep
def test_pose2d_heading(self): logger = MagicMock() logger.write = MagicMock(return_value=datetime.timedelta(microseconds=9721)) bus = Bus(logger) moves = [ [Rotation(0, 0, 0, 1), [0, 0, 0]], # heading zero [Rotation(0, 0.7071068, 0, 0.7071068), [0, 0, 90*100]], # facing left [Rotation(0, -0.7071068, 0, 0.7071068), [0, 0, -90*100]], # facing right [Rotation(0, -0.9999619, 0, 0.0087265), [0, 0, -179*100]], # facing backwards ] c = RealSense(bus=bus.handle('rs'), config={}) tester = bus.handle('tester') bus.connect('rs.pose2d', 'tester.pose2d') for input, output in moves: frame = MagicMock() pose_frame = frame.as_pose_frame.return_value pose_frame.get_pose_data.return_value = Pose( Acceleration(0, 0, 0), AngularAcceleration(0, 0, 0), AngularVelocity(0, 0, 0), 0, input, 0, Translation(0, 0, 0), Velocity(0, 0, 0)) frame.get_timestamp.return_value = 0 frame.get_frame_number.return_value = 0 c.pose_callback(frame) dt, channel, pose2d = tester.listen() self.assertEqual(channel, 'pose2d') self.assertEqual(pose2d, output)
def test_callback(self): global echo_data def echo_callback(data): global echo_data echo_data = data config = {} bus = Bus(MagicMock(write=MagicMock(return_value=timedelta()))) echo = EchoController(config, bus=bus.handle('echo')) # initialize internal variables, so that wait_for_init() can be skipped echo.sim_time = timedelta() echo.last_position = [0, 0, 0] echo.yaw = 0.0 tester = bus.handle('tester') tester.register("response") bus.connect("tester.response", "echo.response") bus.connect("echo.request", "tester.request") echo.send_request('hello!', echo_callback) echo.start() _, channel, data = tester.listen() self.assertEqual(data, ['0xe3e70682c2094cac629f6fbed82c07cd', 'hello!']) tester.publish('response', data) echo.bus.sleep(0.1) self.assertEqual(echo_data, 'hello!') echo.request_stop() echo.join()
def Xtest_go_to_entrance(self): config = { 'virtual_world': True, 'max_speed': 1.0, 'gap_size': 0.8, 'wall_dist': 0.8, 'timeout': 600, 'symmetric': False, 'right_wall': 'auto' } bus = Bus(simulation.SimLogger()) app = SubTChallenge(config, bus.handle('app')) sim = simulation.Simulation(bus.handle('sim'), end_condition=entrance_reached) g_logger.info("connecting:") for o in bus.handle('sim').out: if o == 'pose2d': continue # connect 'pose3d' only g_logger.info(f' sim.{o} → app.{o}') bus.connect(f'sim.{o}', f'app.{o}') for o in bus.handle('app').out: g_logger.info(f' app.{o} → sim.{o}') bus.connect(f'app.{o}', f'sim.{o}') g_logger.info("done.") app.start() sim.start() sim.join() app.request_stop() app.join() self.assertTrue(entrance_reached(sim))
def test_orientation(self): origin = np.asarray([10.0, 0.0, 0.0]) inc = np.asarray([100, 0.0, 0.0]) inc_ori = np.asarray([0.0, 100, 0.0]) orientation = quaternion.from_axis_angle([0, 0, 1], math.radians(90)) bus = Bus(MagicMock(write=MagicMock(return_value=timedelta()))) loc = Localization({}, bus.handle('loc')) loc.xyz = origin tester = bus.handle('tester') tester.register('orientation', 'odom') bus.connect('loc.pose3d', 'tester.pose3d') bus.connect('tester.orientation', 'loc.orientation') bus.connect('tester.odom', 'loc.odom') loc.start() # tester.publish('origin', ['name'] + list(origin) + quaternion.identity()) tester.publish('orientation', orientation) tester.publish('odom', 0 * inc) tester.publish('odom', 1 * inc) tester.publish('odom', 2 * inc) dt, channel, pose3d = tester.listen() self.assertEqual(channel, 'pose3d') self.assertPose3dEqual(pose3d, [list(origin), orientation]) dt, channel, pose3d = tester.listen() self.assertPose3dEqual(pose3d, [list(origin + inc_ori / 1000), orientation]) dt, channel, pose3d = tester.listen() self.assertPose3dEqual( pose3d, [list(origin + 2 * inc_ori / 1000), orientation]) loc.request_stop() loc.join()
def test_sleep(self): config = {} logger = MagicMock() bus = Bus(logger) lidar = SICKLidar(config, bus=bus.handle('lidar')) self.assertIsNone(lidar.sleep) config = {"sleep": 0.1} lidar = SICKLidar(config, bus=bus.handle('lidar')) self.assertAlmostEqual(lidar.sleep, 0.1)
def test_depth(self): logger = MagicMock() logger.write = MagicMock(return_value=datetime.timedelta( microseconds=9721)) bus = Bus(logger) c = RealSense(bus=bus.handle('rs'), config={ "device": "D400", "depth_rgb": True, "depth_infra": True }) tester = bus.handle('tester') bus.connect('rs.depth', 'tester.depth') bus.connect('rs.color', 'tester.color') bus.connect('rs.infra', 'tester.infra') frameset = MagicMock() frameset.is_frameset.return_value = True frame = frameset.as_frameset.return_value.get_depth_frame.return_value frame.get_timestamp.return_value = 0 frame.get_frame_number.return_value = 0 frame.is_depth_frame.return_value = True frame.as_depth_frame.return_value.get_data.return_value = [1, 2] color_frame = frameset.as_frameset.return_value.get_color_frame.return_value color_frame.get_timestamp.return_value = 0 color_frame.get_frame_number.return_value = 0 color_frame.is_video_frame.return_value = True color_frame.as_video_frame.return_value.get_data.return_value = np.asarray( [[0, 100, 255]], dtype=np.uint8) infra_frame = frameset.as_frameset.return_value.get_infrared_frame.return_value infra_frame.get_timestamp.return_value = 0 infra_frame.get_frame_number.return_value = 0 infra_frame.is_video_frame.return_value = True infra_frame.as_video_frame.return_value.get_data.return_value = np.asarray( [[0, 100]], dtype=np.uint8) c.depth_callback(frameset) dt, channel_1, depth = tester.listen() dt, channel_2, color = tester.listen() dt, channel_3, infra = tester.listen() depth_expected = np.asanyarray([1, 2]) color_expected = np.asanyarray([[0, 100, 255]], dtype=np.uint8) infra_expected = np.asanyarray([[0, 100]], dtype=np.uint8) self.assertEqual(channel_1, 'depth') self.assertEqual(channel_2, 'color') self.assertEqual(channel_3, 'infra') color = cv2.imdecode(np.frombuffer(color, dtype=np.uint8), 0) infra = cv2.imdecode(np.frombuffer(infra, dtype=np.uint8), 0) self.assertEqual(depth.shape, depth_expected.shape) self.assertEqual(depth.dtype, depth_expected.dtype) self.assertTrue(np.array_equal(depth, depth_expected)) self.assertEqual(color.shape, color_expected.shape) self.assertEqual(infra.shape, infra_expected.shape)
def test_usage(self): bus = Bus(MagicMock()) app = FollowMe(config={}, bus=bus.handle('app')) tester = bus.handle('tester') tester.register('emergency_stop') bus.connect('tester.emergency_stop', 'app.emergency_stop') tester.publish('emergency_stop', True) app.raise_exception_on_stop = True with self.assertRaises(EmergencyStopException): app.followme()
def test_node(self): logger = MagicMock() bus = Bus(logger) tester = bus.handle('tester') tester.register('raw') imu = LordIMU(config={}, bus=bus.handle('lord')) bus.connect('tester.raw', 'lord.raw') imu.start() tester.publish('raw', SAMPLE_DATA) imu.request_stop() imu.join() self.assertEqual(imu.raw, SAMPLE_DATA)
def test_usage(self): logger = MagicMock() logger.write = MagicMock(return_value=timedelta(seconds=135)) bus = Bus(logger) handle = bus.handle('cortexpilot') tester = bus.handle('tester') robot = Cortexpilot(config={}, bus=handle) bus.connect('cortexpilot.raw', 'tester.raw') robot.start() robot.request_stop() robot.join() tester.shutdown() self.assertEqual(tester.listen(), (timedelta(seconds=135), 'raw', b'\x00\x00\x03\x01\x01\xfb'))
def test_autodetect(self): SAMPLE_DATA = b'1|cmd=home\n' logger = MagicMock(write=MagicMock(return_value=datetime.timedelta())) bus = Bus(logger) c = LoRa(bus=bus.handle('lora'), config={'device_id': 4}) tester = bus.handle('tester') tester.register('raw') bus.connect('tester.raw', 'lora.raw') c.start() tester.publish('raw', SAMPLE_DATA) c.request_stop() c.join() self.assertEqual(c.device_id, 4)
def test_update(self): empty_config = {} bus = Bus(logger=MagicMock()) node = Node(config=empty_config, bus=bus.handle('mynode')) tester = bus.handle('tester') tester.register('vel') bus.connect('tester.vel', 'mynode.vel') dt = tester.publish('vel', 3) node.update() self.assertEqual(node.time, dt) self.assertEqual(node.vel, 3) node2 = Node(config=empty_config, bus=bus.handle('mynode2')) self.assertNotIn('vel', dir(node2))
def test_usage(self): bus = Bus(MagicMock(write=MagicMock(return_value=timedelta()))) tester = bus.handle('tester') tester.register("scan", "rs_scan") mixer = ScanMixer(config={}, bus=bus.handle('mixer')) bus.connect("tester.scan", "mixer.scan") bus.connect("tester.rs_scan", "mixer.rs_scan") bus.connect("mixer.scan", "tester.mixed") mixer.start() scan = [10, 10, 10, 10] tester.publish("scan", scan) _, channel, data = tester.listen() self.assertEqual(data, scan) mixer.request_stop() mixer.join()
def test_publish(self): logger = MagicMock() bus = Bus(logger) handle = bus.handle('test') with self.assertRaises(KeyError): handle.publish('raw', b'some binary data') logger = MagicMock() logger.register = MagicMock(return_value=1) bus = Bus(logger) handle = bus.handle('test') handle.register('raw') handle.publish('raw', b'some binary data 2nd try') logger.write.assert_called_once_with( 1, b'\xc4\x18some binary data 2nd try')
def test_send_cmd(self): logger = MagicMock() bus = Bus(logger) logger.write = MagicMock(return_value=datetime.timedelta( microseconds=9721)) c = LoRa(bus=bus.handle('lora'), config={'device_id': 3}) tester = bus.handle('tester') tester.register('raw') bus.connect('lora.cmd', 'tester.cmd') bus.connect('tester.raw', 'lora.raw') c.start() tester.publish('raw', b'1|3:GoHome:1234\n') c.request_stop() c.join() self.assertEqual(tester.listen()[2], b'GoHome')
def test_sync(self): logger = MagicMock() logger.write = MagicMock(return_value=timedelta(135)) bus = Bus(logger) eduro = Eduro(config={}, bus=bus.handle('eduro')) tester = bus.handle('tester') tester.register('can') bus.connect('tester.can', 'eduro.can') bus.connect('eduro.pose2d', 'tester.pose2d') sync = CAN_triplet(0x80, []) tester.publish('can', sync) eduro.request_stop() eduro.run() tester.shutdown() self.assertEqual(tester.listen(), (timedelta(135), 'pose2d', [0, 0, 0]))
def test_2nd_loop(self): logger = MagicMock() logger.write = MagicMock(return_value=timedelta(seconds=135)) bus = Bus(logger) handle = bus.handle('cortexpilot') tester = bus.handle('tester') robot = Cortexpilot(config={}, bus=handle) bus.connect('cortexpilot.raw', 'tester.raw') handle.queue.put((timedelta(seconds=123), 'raw', b'\x00\x00\x10\x01\x01Robik V4.0.2\x00\x8f')) robot.start() robot.request_stop() robot.join() tester.shutdown() # so that listen() does not block self.assertEqual(tester.listen(), (timedelta(seconds=135), 'raw', b'\x00\x00\x03\x01\x01\xfb')) # request version self.assertEqual(tester.listen(), (timedelta(seconds=135), 'raw', bytes.fromhex('00000f010d000000000000000040010000a2'))) # cmd
def test_alive(self): logger = MagicMock() bus = Bus(logger) handle = bus.handle('test') self.assertTrue(handle.is_alive()) handle.shutdown() self.assertFalse(handle.is_alive())
def test_shutdown(self): logger = MagicMock() bus = Bus(logger) handler = bus.handle('test') handler.shutdown() with self.assertRaises(BusShutdownException): handler.listen()
def test_report_error(self): log = MagicMock() bus = Bus(log) handle = bus.handle('test') handle.report_error(error=KeyError(123)) expected = bytes(str(dict(name='test', error=KeyError(123))), 'ascii') log.write.assert_called_once_with(0, expected)
def test_usage(self): logger = MagicMock() bus = Bus(logger) logger.write = MagicMock(return_value=datetime.timedelta( microseconds=9721)) c = Drone(bus=bus.handle('drone'), config={}) tester = bus.handle('tester') tester.register('desired_speed') bus.connect('tester.desired_speed', 'drone.desired_speed') bus.connect('drone.desired_speed_3d', 'tester.desired_speed_3d') c.start() tester.publish('desired_speed', [1000, 9000]) c.request_stop() c.join() self.assertEqual(tester.listen()[2], [[1.0, 0.0, 0.0], [0.0, 0.0, MAX_ANGULAR]])
def test_autodetect_bug(self): logger = MagicMock() logger.write = MagicMock(return_value=datetime.timedelta( microseconds=9721)) bus = Bus(logger) c = LoRa(bus=bus.handle('lora'), config={}) # force autodetection tester = bus.handle('tester') tester.register('raw') bus.connect('tester.raw', 'lora.raw') c.start() tester.publish('raw', b'4|alive\n') tester.publish('raw', b'4|alive-97') tester.publish('raw', b'21\n') c.request_stop() c.join() self.assertEqual(c.device_id, 4)
def test_usage(self): config = {} bus = Bus(MagicMock(write=MagicMock(return_value=timedelta()))) r1 = SpaceRoboticsChallengeRound1(config, bus=bus.handle('app')) r1.start() r1.request_stop() r1.join()
def test_dynamic_tcp(self): with patch('osgar.drivers.logsocket.socket.socket') as mock: instance = mock.return_value logger = MagicMock() bus = Bus(logger) device = LogTCPDynamicIP(config={}, bus=bus.handle('tcpdyn')) tester = bus.handle('tester') tester.register('addr') bus.connect('tester.addr', 'tcpdyn.addr') tester.publish('addr', ['10.1.10.1', 8000]) device.start() device.request_stop() device.join() instance.connect.assert_called_once_with(('10.1.10.1', 8000))
def test_usage(self): empty_config = {} bus = Bus(logger=MagicMock()) node = Node(config=empty_config, bus=bus.handle('mynode')) node.start() node.request_stop() node.join()
def test_named_publish(self): logger = MagicMock() logger.register = MagicMock(return_value=1) bus = Bus(logger) handle = bus.handle('gps_serial') handle.register('raw') handle.publish('raw', b'bin data') logger.write.assert_called_once_with(1, b'\xc4\x08bin data')
def test_publish_serialization(self): logger = MagicMock() logger.register = MagicMock(return_value=1) bus = Bus(logger) handle = bus.handle('test') handle.register('position') handle.publish('position', (-123, 456)) logger.write.assert_called_once_with(1, b'\x92\xd0\x85\xcd\x01\xc8')