def test_publisher(self):
lcm = DrakeLcm()
dut = mut.LcmPublisherSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
lcm=lcm,
publish_period=0.1)
subscriber = Subscriber(lcm, "TEST_CHANNEL", lcmt_quaternion)
model_message = self._model_message()
self._fix_and_publish(dut, AbstractValue.Make(model_message))
lcm.HandleSubscriptions(0)
self.assert_lcm_equal(subscriber.message, model_message)
# Test `publish_triggers` overload.
mut.LcmPublisherSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
lcm=lcm,
publish_period=0.1,
publish_triggers={TriggerType.kPeriodic})
# Test LcmInterfaceSystem overloads
lcm_system = mut.LcmInterfaceSystem(lcm=lcm)
dut = mut.LcmPublisherSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
lcm=lcm_system,
publish_period=0.1)
self._fix_and_publish(dut, AbstractValue.Make(model_message))
lcm.HandleSubscriptions(0)
self.assert_lcm_equal(subscriber.message, model_message)
# Test `publish_triggers` overload.
mut.LcmPublisherSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
lcm=lcm_system,
publish_period=0.1,
publish_triggers={TriggerType.kPeriodic})
def test_connect_drake_visualizer(self):
# Test visualization API.
T = float
SceneGraph = mut.SceneGraph_[T]
DiagramBuilder = DiagramBuilder_[T]
Simulator = Simulator_[T]
lcm = DrakeLcm()
role = mut.Role.kIllustration
def normal(builder, scene_graph):
with catch_drake_warnings(expected_count=2):
mut.ConnectDrakeVisualizer(builder=builder,
scene_graph=scene_graph,
lcm=lcm,
role=role)
mut.DispatchLoadMessage(scene_graph=scene_graph,
lcm=lcm,
role=role)
def port(builder, scene_graph):
with catch_drake_warnings(expected_count=2):
mut.ConnectDrakeVisualizer(
builder=builder,
scene_graph=scene_graph,
pose_bundle_output_port=(
scene_graph.get_pose_bundle_output_port()),
lcm=lcm,
role=role)
mut.DispatchLoadMessage(scene_graph=scene_graph,
lcm=lcm,
role=role)
for func in [normal, port]:
# Create subscribers.
load_channel = "DRAKE_VIEWER_LOAD_ROBOT"
draw_channel = "DRAKE_VIEWER_DRAW"
load_subscriber = Subscriber(lcm, load_channel,
lcmt_viewer_load_robot)
draw_subscriber = Subscriber(lcm, draw_channel, lcmt_viewer_draw)
# Test sequence.
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Only load will be published by `DispatchLoadMessage`.
func(builder, scene_graph)
lcm.HandleSubscriptions(0)
self.assertEqual(load_subscriber.count, 1)
self.assertEqual(draw_subscriber.count, 0)
diagram = builder.Build()
# Load and draw will be published.
Simulator(diagram).Initialize()
lcm.HandleSubscriptions(0)
self.assertEqual(load_subscriber.count, 2)
self.assertEqual(draw_subscriber.count, 1)
def load(self):
"""
Loads ``meshcat`` visualization elements.
Precondition:
The ``scene_graph`` used to construct this object must be part of a
fully constructed diagram (e.g. via ``DiagramBuilder.Build()``).
"""
self.vis[self.prefix].delete()
# Intercept load message via memq LCM.
memq_lcm = DrakeLcm("memq://")
memq_lcm_subscriber = Subscriber(
lcm=memq_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, memq_lcm)
memq_lcm.HandleSubscriptions(0)
assert memq_lcm_subscriber.count > 0
load_robot_msg = memq_lcm_subscriber.message
# Translate elements to `meshcat`.
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[source_name, frame_name] = self._parse_name(link.name)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
meshcat_geom, material, element_local_tf = _convert_mesh(geom)
if meshcat_geom is not None:
cur_vis = (
self.vis[self.prefix][source_name][str(link.robot_num)]
[frame_name][str(j)])
cur_vis.set_object(meshcat_geom, material)
cur_vis.set_transform(element_local_tf)
# Draw the frames in self.frames_to_draw.
if "::" in frame_name:
robot_name, link_name = self._parse_name(frame_name)
else:
robot_name = "world"
link_name = frame_name
if (robot_name in self.frames_to_draw.keys()
and link_name in self.frames_to_draw[robot_name]):
prefix = (self.prefix + '/' + source_name + '/'
+ str(link.robot_num) + '/' + frame_name)
AddTriad(
self.vis,
name="frame",
prefix=prefix,
length=self.axis_length,
radius=self.axis_radius,
opacity=self.frames_opacity)
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_publisher(self):
lcm = DrakeLcm()
dut = mut.LcmPublisherSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm,
publish_period=0.1)
subscriber = Subscriber(lcm, "TEST_CHANNEL", quaternion_t)
model_message = self._model_message()
self._fix_and_publish(dut, AbstractValue.Make(model_message))
lcm.HandleSubscriptions(0)
self.assert_lcm_equal(subscriber.message, model_message)
def test_publisher_cpp(self):
lcm = DrakeLcm()
dut = mut.LcmPublisherSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm,
use_cpp_serializer=True)
subscriber = Subscriber(lcm, "TEST_CHANNEL", quaternion_t)
model_message = self._model_message()
model_value = self._model_value_cpp()
self._fix_and_publish(dut, model_value)
lcm.HandleSubscriptions(0)
self.assert_lcm_equal(subscriber.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):
"""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)
class Meldis:
"""
MeshCat LCM Display Server (MeLDiS)
Offers a MeshCat vizualization server that listens for and draws Drake's
legacy LCM vizualization messages.
Refer to the pydrake.visualization.meldis module docs for details.
"""
def __init__(self, *, meshcat_port=None):
self._lcm = DrakeLcm()
lcm_url = self._lcm.get_lcm_url()
logging.info(f"Meldis is listening for LCM messages at {lcm_url}")
self.meshcat = Meshcat(port=meshcat_port)
viewer = _ViewerApplet(meshcat=self.meshcat)
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT",
message_type=lcmt_viewer_load_robot,
handler=viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW",
message_type=lcmt_viewer_draw,
handler=viewer.on_viewer_draw)
contact = _ContactApplet(meshcat=self.meshcat)
self._subscribe(channel="CONTACT_RESULTS",
message_type=lcmt_contact_results_for_viz,
handler=contact.on_contact_results)
def _subscribe(self, channel, message_type, handler):
def _parse_and_handle(data):
handler(message=message_type.decode(data))
self._lcm.Subscribe(channel=channel, handler=_parse_and_handle)
def serve_forever(self):
# TODO(jwnimmer-tri) If there are no browser connections open for some
# period of time, we should probably give up and quit, rather than
# leave a zombie meldis running forever.
while True:
self._lcm.HandleSubscriptions(timeout_millis=1000)
def test_drake_visualizer(self, T):
# Test visualization API.
SceneGraph = mut.SceneGraph_[T]
DiagramBuilder = DiagramBuilder_[T]
Simulator = Simulator_[T]
lcm = DrakeLcm()
role = mut.Role.kIllustration
params = mut.DrakeVisualizerParams(
publish_period=0.1, role=mut.Role.kIllustration,
default_color=mut.Rgba(0.1, 0.2, 0.3, 0.4))
self.assertEqual(repr(params), "".join([
"DrakeVisualizerParams("
"publish_period=0.1, "
"role=Role.kIllustration, "
"default_color=Rgba(r=0.1, g=0.2, b=0.3, a=0.4))"]))
# Add some subscribers to detect message broadcast.
load_channel = "DRAKE_VIEWER_LOAD_ROBOT"
draw_channel = "DRAKE_VIEWER_DRAW"
load_subscriber = Subscriber(
lcm, load_channel, lcmt_viewer_load_robot)
draw_subscriber = Subscriber(
lcm, draw_channel, lcmt_viewer_draw)
# There are three ways to configure DrakeVisualizer.
def by_hand(builder, scene_graph, params):
visualizer = builder.AddSystem(
mut.DrakeVisualizer_[T](lcm=lcm, params=params))
builder.Connect(scene_graph.get_query_output_port(),
visualizer.query_object_input_port())
def auto_connect_to_system(builder, scene_graph, params):
mut.DrakeVisualizer_[T].AddToBuilder(builder=builder,
scene_graph=scene_graph,
lcm=lcm, params=params)
def auto_connect_to_port(builder, scene_graph, params):
mut.DrakeVisualizer_[T].AddToBuilder(
builder=builder,
query_object_port=scene_graph.get_query_output_port(),
lcm=lcm, params=params)
for func in [by_hand, auto_connect_to_system, auto_connect_to_port]:
# Build the diagram.
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
func(builder, scene_graph, params)
# Simulate to t = 0 to send initial load and draw messages.
diagram = builder.Build()
Simulator(diagram).AdvanceTo(0)
lcm.HandleSubscriptions(0)
self.assertEqual(load_subscriber.count, 1)
self.assertEqual(draw_subscriber.count, 1)
load_subscriber.clear()
draw_subscriber.clear()
# Ad hoc broadcasting.
scene_graph = SceneGraph()
mut.DrakeVisualizer_[T].DispatchLoadMessage(
scene_graph, lcm, params)
lcm.HandleSubscriptions(0)
self.assertEqual(load_subscriber.count, 1)
self.assertEqual(draw_subscriber.count, 0)
load_subscriber.clear()
draw_subscriber.clear()
class Meldis:
"""
MeshCat LCM Display Server (MeLDiS)
Offers a MeshCat vizualization server that listens for and draws Drake's
legacy LCM vizualization messages.
Refer to the pydrake.visualization.meldis module docs for details.
"""
def __init__(self, *, meshcat_port=None):
# Bookkeeping for update throtting.
self._last_update_time = time.time()
# Bookkeeping for subscriptions, keyed by LCM channel name.
self._message_types = {}
self._message_handlers = {}
self._message_pending_data = {}
self._lcm = DrakeLcm()
lcm_url = self._lcm.get_lcm_url()
_logger.info(f"Meldis is listening for LCM messages at {lcm_url}")
params = MeshcatParams(host="localhost", port=meshcat_port)
self.meshcat = Meshcat(params=params)
viewer = _ViewerApplet(meshcat=self.meshcat)
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT",
message_type=lcmt_viewer_load_robot,
handler=viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW",
message_type=lcmt_viewer_draw,
handler=viewer.on_viewer_draw)
contact = _ContactApplet(meshcat=self.meshcat)
self._subscribe(channel="CONTACT_RESULTS",
message_type=lcmt_contact_results_for_viz,
handler=contact.on_contact_results)
# Bookkeeping for automatic shutdown.
self._last_poll = None
self._last_active = None
def _subscribe(self, channel, message_type, handler):
"""Subscribes the handler to the given channel, using message_type to
pass in a decoded message object (not the raw bytes). The handler will
only be called at some maximum frequency. Messages on the same channel
that arrive too quickly will be discarded.
"""
# Record this channel's type and handler.
assert self._message_types.get(channel, message_type) == message_type
self._message_types[channel] = message_type
self._message_handlers.setdefault(channel, []).append(handler)
# Subscribe using an internal function that implements "last one wins".
# It's important to service the LCM queue as frequently as possible:
# https://github.com/RobotLocomotion/drake/issues/15234
# https://github.com/lcm-proj/lcm/issues/345
# However, if the sender is transmitting visualization messages at
# a high rate (e.g., if a sim is running much faster than realtime),
# then we should only pass some of them along to MeshCat to avoid
# flooding it. The hander merely records the message data; we'll
# pass it along to MeshCat using our `self._should_update()` timer.
def _on_message(data):
self._message_pending_data[channel] = data
self._lcm.Subscribe(channel=channel, handler=_on_message)
def _invoke_subscriptions(self):
"""Posts any unhandled messages to their handlers and clears the
collection of unhandled messages.
"""
for channel, data in self._message_pending_data.items():
message = self._message_types[channel].decode(data)
for function in self._message_handlers[channel]:
function(message=message)
self._message_pending_data.clear()
def serve_forever(self, *, idle_timeout=None):
"""Run indefinitely, forwarding LCM => MeshCat messages.
If provided, the optional idle_timeout must be strictly positive and
this loop will sys.exit after that many seconds without any websocket
connections.
"""
while True:
self._lcm.HandleSubscriptions(timeout_millis=1000)
if not self._should_update():
continue
self._invoke_subscriptions()
self.meshcat.Flush()
self._check_for_shutdown(idle_timeout=idle_timeout)
def _should_update(self):
"""Post LCM-driven updates to MeshCat no faster than 40 Hz."""
now = time.time()
update_period = 0.025 # 40 Hz
remaining = update_period - (now - self._last_update_time)
if remaining > 0.0:
return False
else:
self._last_update_time = now
return True
def _check_for_shutdown(self, *, idle_timeout):
# Allow the user to opt-out of the timeout feature.
if idle_timeout is None:
return
assert idle_timeout > 0.0
# One-time initialization.
now = time.time()
if self._last_active is None:
self._last_active = now
return
# Only check once every 5 seconds.
if (self._last_poll is not None) and (now < self._last_poll + 5.0):
return
self._last_poll = now
# Check to see if any browser client(s) are connected.
if self.meshcat.GetNumActiveConnections() > 0:
self._last_active = now
return
# In case we are idle for too long, exit automatically.
if now > self._last_active + idle_timeout:
_logger.info("Meldis is exiting now; no browser was connected for"
f" >{idle_timeout} seconds")
sys.exit(1)
def _build_body_patches(self, use_random_colors,
substitute_collocated_mesh_files):
"""
Generates body patches. self._patch_Blist stores a list of patches for
each body (starting at body id 1). A body patch is a list of all 3D
vertices of a piece of visual geometry.
"""
self._patch_Blist = {}
self._patch_Blist_colors = {}
memq_lcm = DrakeLcm("memq://")
memq_lcm_subscriber = Subscriber(lcm=memq_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
# TODO(SeanCurtis-TRI): Use SceneGraph inspection instead of mocking
# LCM and inspecting the generated message.
DispatchLoadMessage(self._scene_graph, memq_lcm)
memq_lcm.HandleSubscriptions(0)
assert memq_lcm_subscriber.count > 0
load_robot_msg = memq_lcm_subscriber.message
# Spawn a random color generator, in case we need to pick random colors
# for some bodies. Each body will be given a unique color when using
# this random generator, with each visual element of the body colored
# the same.
color = iter(
plt.cm.rainbow(np.linspace(0, 1, load_robot_msg.num_links)))
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
this_body_patches = []
this_body_colors = []
this_color = next(color)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
X_BG = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)), geom.position)
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Draw a bounding box.
patch_G = np.vstack(
(geom.float_data[0] / 2. *
np.array([-1, -1, 1, 1, -1, -1, 1, 1]),
geom.float_data[1] / 2. *
np.array([-1, 1, -1, 1, -1, 1, -1, 1]),
geom.float_data[2] / 2. *
np.array([-1, -1, -1, -1, 1, 1, 1, 1])))
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
radius = geom.float_data[0]
lati, longi = np.meshgrid(np.arange(0., 2. * math.pi, 0.5),
np.arange(0., 2. * math.pi, 0.5))
lati = lati.ravel()
longi = longi.ravel()
patch_G = np.vstack([
np.sin(lati) * np.cos(longi),
np.sin(lati) * np.sin(longi),
np.cos(lati)
])
patch_G *= radius
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
radius = geom.float_data[0]
length = geom.float_data[1]
# In the lcm geometry, cylinders are along +z
# https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m
# Two circles: one at bottom, one at top.
sample_pts = np.arange(0., 2. * math.pi, 0.25)
patch_G = np.hstack([
np.array([[
radius * math.cos(pt), radius * math.sin(pt),
-length / 2.
],
[
radius * math.cos(pt),
radius * math.sin(pt), length / 2.
]]).T for pt in sample_pts
])
elif geom.type == geom.MESH:
filename = geom.string_data
base, ext = os.path.splitext(filename)
if (ext.lower() != ".obj"
and substitute_collocated_mesh_files):
# Check for a co-located .obj file (case insensitive).
for f in glob.glob(base + '.*'):
if f[-4:].lower() == '.obj':
filename = f
break
if filename[-4:].lower() != '.obj':
raise RuntimeError("The given file " + filename +
" is not "
"supported and no alternate " +
base + ".obj could be found.")
if not os.path.exists(filename):
raise FileNotFoundError(errno.ENOENT,
os.strerror(errno.ENOENT),
filename)
mesh = ReadObjToSurfaceMesh(filename)
patch_G = np.vstack([v.r_MV() for v in mesh.vertices()]).T
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
# Compute pose in body.
patch_B = X_BG @ patch_G
# Close path if not closed.
if (patch_B[:, -1] != patch_B[:, 0]).any():
patch_B = np.hstack((patch_B, patch_B[:, 0][np.newaxis].T))
this_body_patches.append(patch_B)
if use_random_colors:
this_body_colors.append(this_color)
else:
this_body_colors.append(geom.color)
self._patch_Blist[link.name] = this_body_patches
self._patch_Blist_colors[link.name] = this_body_colors
def load(self):
"""
Loads ``meshcat`` visualization elements.
Precondition:
The ``scene_graph`` used to construct this object must be part of a
fully constructed diagram (e.g. via ``DiagramBuilder.Build()``).
"""
if self._delete_prefix_on_load:
self.vis[self.prefix].delete()
# Intercept load message via memq LCM.
memq_lcm = DrakeLcm("memq://")
memq_lcm_subscriber = Subscriber(lcm=memq_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, memq_lcm)
memq_lcm.HandleSubscriptions(0)
assert memq_lcm_subscriber.count > 0
load_robot_msg = memq_lcm_subscriber.message
# Translate elements to `meshcat`.
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[_, frame_name] = self._parse_name(link.name)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
meshcat_geom, material, element_local_tf = _convert_mesh(geom)
if meshcat_geom is not None:
cur_vis = (self.vis[self.prefix][str(
link.robot_num)][frame_name][str(j)])
# Make the uuid's deterministic for mesh geometry, to
# support caching at the zmqserver. This means that
# multiple (identical) geometries may have the same UUID,
# but testing suggests that meshcat + three.js are ok with
# it.
if isinstance(meshcat_geom, meshcat.geometry.MeshGeometry):
meshcat_geom.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
meshcat_geom.contents))
material.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
meshcat_geom.contents + "material"))
mesh = meshcat.geometry.Mesh(meshcat_geom, material)
mesh.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
meshcat_geom.contents + "mesh"))
cur_vis.set_object(mesh)
else:
cur_vis.set_object(meshcat_geom, material)
cur_vis.set_transform(element_local_tf)
# Draw the frames in self.frames_to_draw.
if "::" in frame_name:
robot_name, link_name = self._parse_name(frame_name)
else:
robot_name = "world"
link_name = frame_name
if (robot_name in self.frames_to_draw.keys()
and link_name in self.frames_to_draw[robot_name]):
prefix = (self.prefix + '/' + str(link.robot_num) + '/' +
frame_name)
AddTriad(self.vis,
name="frame",
prefix=prefix,
length=self.axis_length,
radius=self.axis_radius,
opacity=self.frames_opacity)
