def test_lcm_interface_system_diagram(self):
# First, check the class doc.
self.assertIn("only inherits from LeafSystem",
mut.LcmInterfaceSystem.__doc__)
# Next, construct a diagram and add both the interface system and
# a subscriber.
builder = DiagramBuilder()
lcm = DrakeLcm()
lcm_system = builder.AddSystem(mut.LcmInterfaceSystem(lcm=lcm))
# Create subscriber in the diagram.
subscriber = builder.AddSystem(
mut.LcmSubscriberSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
lcm=lcm))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()
# Publish test message.
model_message = self._model_message()
lcm.Publish("TEST_CHANNEL", model_message.encode())
# Simulate to a non-zero time to ensure the subscriber picks up the
# message.
eps = np.finfo(float).eps
simulator.AdvanceTo(eps)
# Ensure that we have what we want.
context = subscriber.GetMyContextFromRoot(
simulator.get_mutable_context())
actual_message = subscriber.get_output_port(0).Eval(context)
self.assert_lcm_equal(actual_message, model_message)
def test_subscriber(self):
lcm = DrakeLcm()
dut = mut.LcmSubscriberSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm)
model_message = self._model_message()
lcm.Publish(channel="TEST_CHANNEL", buffer=model_message.encode())
lcm.HandleSubscriptions(0)
context = self._process_event(dut)
actual_message = dut.get_output_port(0).Eval(context)
self.assert_lcm_equal(actual_message, model_message)
def test_subscriber_cpp(self):
lcm = DrakeLcm()
dut = mut.LcmSubscriberSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm,
use_cpp_serializer=True)
model_message = self._model_message()
lcm.Publish(channel="TEST_CHANNEL", buffer=model_message.encode())
lcm.HandleSubscriptions(0)
context = self._process_event(dut)
abstract = dut.get_output_port(0).EvalAbstract(context)
actual_message = self._cpp_value_to_py_message(abstract)
self.assert_lcm_equal(actual_message, model_message)
def test_subscriber_wait_for_message(self):
"""Ensures that `WaitForMessage` threading works in a Python workflow
that is not threaded."""
# N.B. This will fail with `threading`. See below for using
# `multithreading`.
lcm = DrakeLcm("memq://")
lcm.StartReceiveThread()
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
value = AbstractValue.Make(header_t())
for i in range(3):
message = header_t()
message.utime = i
lcm.Publish("TEST_LOOP", message.encode())
sub.WaitForMessage(i, value)
self.assertEqual(value.get_value().utime, i)
def test_subscriber_wait_for_message(self):
"""Checks how `WaitForMessage` works without Python threads."""
lcm = DrakeLcm()
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
value = AbstractValue.Make(header_t())
for old_message_count in range(3):
message = header_t()
message.utime = old_message_count + 1
lcm.Publish("TEST_LOOP", message.encode())
for attempt in range(10):
new_count = sub.WaitForMessage(
old_message_count, value, timeout=0.02)
if new_count > old_message_count:
break
lcm.HandleSubscriptions(0)
self.assertEqual(value.get_value().utime, old_message_count + 1)
def test_subscriber(self):
lcm = DrakeLcm()
dut = Subscriber(lcm=lcm, channel="CHANNEL", lcm_type=lcmt_quaternion)
lcm.Publish(channel="CHANNEL", buffer=self.quat.encode())
self.assertEqual(dut.count, 0)
self.assertEqual(len(dut.raw), 0)
self.assertEqual(dut.message.w, 0)
self.assertEqual(dut.message.x, 0)
self.assertEqual(dut.message.y, 0)
self.assertEqual(dut.message.z, 0)
lcm.HandleSubscriptions(0)
self.assertEqual(dut.count, 1)
self.assertEqual(dut.raw, self.quat.encode())
self.assertEqual(dut.message.w, self.quat.w)
self.assertEqual(dut.message.x, self.quat.x)
self.assertEqual(dut.message.y, self.quat.y)
self.assertEqual(dut.message.z, self.quat.z)
