def test_not_started_dynamic_tcp(self):
with patch('osgar.drivers.logsocket.socket.socket') as mock:
instance = mock.return_value
logger = MagicMock()
bus = Bus(logger)
config = {}
device = LogTCPDynamicIP(config=config, bus=bus.handle('tcp'))
device.start()
device.request_stop()
device.join()
instance.connect.assert_not_called()
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()
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_bus_sleep(self):
logger = MagicMock()
bus = Bus(logger)
handle = bus.handle('test')
handle.sleep(0.1)
bus = LogBusHandler(logger, inputs={}, outputs={})
bus.sleep(0.1)
bus = LogBusHandlerInputsOnly(logger, inputs={})
bus.sleep(0.1)
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['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)
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_sleep(self):
bus = Bus(MagicMock())
handle = bus.handle('test')
interval = 0.5
def sleep():
handle.sleep(interval)
t = threading.Thread(target=sleep, daemon=True)
start = time.monotonic()
t.start()
t.join(10)
end = time.monotonic()
# add extra epsilon time to avoid rounding error:
# AssertionError: 255.85899999999998 not greater than or equal to 255.859
eps = 0.000001 # 1us
self.assertFalse(t.is_alive())
self.assertGreaterEqual(end - interval + eps, start)
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):
logger = MagicMock()
bus = Bus(logger)
config = {
'ros_master_uri': 'http://127.0.0.1:11311',
'ros_client_uri': 'http://127.0.0.1:8000',
'topic': '/hello',
'topic_type': 'std_msgs/String',
'subscribe': [['/imu/data', 'std_msgs/Imu', 'imu_data']]
}
master = DummyROSMaster(('127.0.0.1', 11311))
proxy = ROSProxy(config=config, bus=bus.handle('ros'))
with GlobalExceptionWatcher():
proxy.start()
proxy.request_stop()
proxy.join(timeout=1)
self.assertFalse(proxy.is_alive())
master.shutdown()
def test_mixing(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 = 4 * [10]
rs_scan = 3 * [8]
tester.publish("rs_scan", rs_scan)
tester.publish("scan", scan)
_, channel, data = tester.listen()
self.assertEqual(data, 4 * [8])
mixer.request_stop()
mixer.join()
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_find_next_volatile(self):
# from seed 1500
config = {'debug': False}
bus = Bus(MagicMock(write=MagicMock(return_value=timedelta())))
r2e = SpaceRoboticsChallengeExcavatorRound2(config,
bus=bus.handle('app'))
r2e.xyz = [10.394101, -8.620875, 0.786391]
r2e.yaw = 2.767378
r2e.vol_list = [[49.6034306594, -45.9196365544],
[-40.7186943541, 25.742583001],
[52.5927389894, 31.5772572951],
[-35.4015899876, 30.9052755259],
[28.626252316, -36.5104539519],
[50.846455282, 10.1442217972],
[49.3141912004, -41.6719396868],
[-33.8047498291, -45.7775507432],
[-32.6856318871, 30.7811787209],
[41.5629764612, 43.1950851664],
[-6.56325228377, 46.9468649025],
[-5.62467497268, -4.92013629455],
[14.3408662367, 17.4889646202],
[54.657022764, 54.5170630599],
[15.4820006992, 1.24266919623],
[1.23307211219, 52.6044006624],
[56.128567115, -6.59073868759],
[-1.08706993599, 51.8179897589],
[-39.812812773, -32.6237207556],
[-39.8509161911, -54.5155864259],
[15.8911609315, -3.84581195752],
[51.9294757594, 14.3942159102],
[-40.8507536421, -45.2608876114],
[-27.542088652, 57.0], [51.8354318585, 34.5281584002],
[57.2500220512, 45.3890817188],
[-37.3400408782, 47.8302730305],
[-14.2076440605, -29.3234846144],
[-56.6504633815, 46.898890127]]
v = r2e.get_next_volatile()
np.testing.assert_array_equal([-5.62467497268, -4.92013629455], v)
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_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_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_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_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_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_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_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_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_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_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_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_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_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_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)