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')