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_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_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_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 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_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_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):
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_two_modules(self):
logger = MagicMock()
bus = Bus(logger)
handler2 = bus.handle('2')
handler1 = bus.handle('1')
handler1.register('raw')
bus.connect('1.raw', '2.42')
logger.write = MagicMock(return_value=123)
handler1.publish('raw', b"Hello!")
self.assertEqual(handler2.listen(), (123, '42', b"Hello!"))
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)
class Recorder:
def __init__(self, config, logger):
self.stop_requested = threading.Event()
self.sigint_received = False
self.modules = {}
self.bus = Bus(logger)
for module_name, module_config in config['modules'].items():
klass = get_class_by_name(module_config['driver'])
self.modules[module_name] = klass(module_config.get('init', {}),
bus=self.bus.handle(module_name))
for sender, receiver in config['links']:
self.bus.connect(sender, receiver, self.modules)
signal.signal(signal.SIGINT, self.request_stop)
g_logger.info("SIGINT handler installed")
def __enter__(self):
for module in self.modules.values():
module.start()
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self.request_stop()
for module in self.modules.values():
module.join(1)
for name, max_delay, timestamp in self.bus.delays():
g_logger.error(
f"{name:>12}: maximum delay of {max_delay} at {timestamp}")
for t in threading.enumerate():
if t != threading.current_thread():
g_logger.error(f'thread {repr(t.name)} still running!')
g_logger.error(
f' class: {t.__class__.__module__}.{t.__class__.__name__}'
)
g_logger.error(f' target: {t._target}')
if self.sigint_received:
g_logger.info("committing suicide by SIGINT")
signal.signal(signal.SIGINT, signal.SIG_DFL)
os.kill(os.getpid(), signal.SIGINT)
def request_stop(self, signum=None, frame=None): # pylint: disable=unused-argument
if signum == signal.SIGINT:
self.sigint_received = True
if self.stop_requested.is_set():
return
for module in self.modules.values():
module.request_stop()
self.stop_requested.set()
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_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_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_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_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):
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_buttons(self):
logger = MagicMock()
logger.write = MagicMock(return_value=timedelta(42))
bus = Bus(logger)
eduro = Eduro(config={}, bus=bus.handle('eduro'))
tester = bus.handle('tester')
tester.register('can')
bus.connect('eduro.buttons', 'tester.buttons')
bus.connect('tester.can', 'eduro.can')
tester.publish('can', CAN_triplet(0x28A, [0, 0]))
eduro.request_stop()
eduro.run()
tester.shutdown()
self.assertEqual(tester.listen(), (timedelta(42), 'buttons', {
'blue_selected': True,
'cable_in': False
}))
def test_control_center(self):
logger = MagicMock()
bus = Bus(logger)
logger.write = MagicMock(return_value=datetime.timedelta(seconds=97))
c = LoRa(bus=bus.handle('lora'), config={'device_id': 1})
app = bus.handle('app')
app.register('cmd')
tester = bus.handle('serial')
tester.register('raw')
bus.connect('app.cmd', 'lora.cmd')
bus.connect('lora.raw', 'serial.raw')
c.start()
app.publish('cmd', [3, b'Pause'])
c.request_stop()
c.join()
self.assertEqual(tester.listen()[2], b'alive\n')
self.assertEqual(tester.listen()[2], b'3:Pause:97\n')
def test_tcp_send(self):
with patch('osgar.drivers.logsocket.socket.socket') as mock:
instance = mock.return_value
logger = MagicMock()
logger.register = MagicMock(return_value=1)
bus = Bus(logger)
config = {'host': '192.168.2.23', 'port':2111}
device = LogTCP(config=config, bus=bus.handle('tcp'))
tester = bus.handle('tester')
tester.register('raw')
bus.connect('tester.raw', 'tcp.raw')
tester.publish('raw', b'bin data')
device.start()
device.request_stop()
device.join()
instance.send.assert_called_once_with(b'bin data')
def test_usage(self):
logger = MagicMock()
bus = Bus(logger)
logger.write = MagicMock(return_value=datetime.timedelta(
microseconds=9721))
c = TwistWrap(bus=bus.handle('twister'), config={})
tester = bus.handle('tester')
tester.register('desired_speed')
bus.connect('tester.desired_speed', 'twister.desired_speed')
bus.connect('twister.cmd_vel', 'tester.cmd_vel')
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, math.radians(90)]])
def test_processing(self):
config = {}
logger = MagicMock()
logger.write = MagicMock(return_value=timedelta(135))
bus = Bus(logger)
#robot_bus = BusHandler(logger, out={}, name='robot')
#bus = BusHandler(logger,
# out={'orientation':[(robot_bus.queue, 'orientation')], 'rotation':[]},
# name='imu')
imu = IMU(config, bus=bus.handle('imu'))
tester = bus.handle('tester')
tester.register('raw')
bus.connect('tester.raw', 'imu.raw')
bus.connect('imu.orientation', 'tester.orientation')
imu.start()
tester.publish('raw', self.nmea_line)
imu.request_stop()
imu.join()
tester.shutdown()
self.assertEqual(tester.listen(), (timedelta(135), 'orientation', self.orientation))
def test_publish_status(self):
logger = MagicMock()
logger.write = MagicMock(return_value=timedelta(seconds=135))
bus = Bus(logger=logger)
tester = bus.handle('tester')
tester.register('raw')
spider = Spider(config={}, bus=bus.handle('spider'))
bus.connect('tester.raw', 'spider.raw')
bus.connect('spider.status', 'tester.status')
spider.can_bridge_initialized = True # skip initialization
self.assertEqual(CAN_packet(0x200, [0, 0x80]), b'@\x02\x00\x80')
tester.publish('raw', b'@\x02\x00\x80')
spider.start()
dt, stream, data = tester.listen()
spider.request_stop()
spider.join()
self.assertEqual(dt, timedelta(seconds=135))
self.assertEqual(stream, 'status')
self.assertEqual(data, ([0x8000, None]))
def test_orientation(self):
logger = MagicMock()
logger.write = MagicMock(return_value=datetime.timedelta(
microseconds=9721))
bus = Bus(logger)
moves = [
# heading zero
[Rotation(0, 0, 0, 1), [0, 0, 0, 1]],
# looking left 90 degrees
# t265 turns around y axis +90
# osgar turns round z axis +90
[
Rotation(0, 0.7071068, 0, 0.7071068),
[0, 0, 0.7071068, 0.7071068]
],
# looking up 90 degrees
# t265 turns around x axis +90
# osgar turns round y axis -90
[
Rotation(0.7071068, 0, 0, 0.7071068),
[0, -0.7071068, 0, 0.7071068]
]
]
c = RealSense(bus=bus.handle('rs'), config={"device": "T200"})
tester = bus.handle('tester')
bus.connect('rs.orientation', 'tester.orientation')
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, orientation = tester.listen()
self.assertEqual(channel, 'orientation')
self.assertEqual(orientation, output)
def test_dynamic_tcp_called_twice(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])
tester.publish('addr', ['192.168.1.31', 8010])
device.start()
tester.sleep(0.1)
device.request_stop()
device.join()
self.assertEqual(instance.connect.call_args_list, [
call(('10.1.10.1', 8000)),
call(('192.168.1.31', 8010))
])
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={})
tester = bus.handle('tester')
bus.connect('rs.depth', 'tester.depth')
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]
c.depth_callback(frameset)
dt, channel, depth = tester.listen()
depth_expected = np.asanyarray([1,2])
self.assertEqual(channel, 'depth')
self.assertEqual(depth.shape, depth_expected.shape)
self.assertEqual(depth.dtype, depth_expected.dtype)
self.assertTrue(np.array_equal(depth, depth_expected))
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_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()