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_diagram_publisher(self):
"""Acceptance tests that a Python LeafSystem is able to output LCM
messages for LcmPublisherSystem to transmit.
"""
lcm = DrakeLcm()
builder = DiagramBuilder()
source = builder.AddSystem(TestSystemsLcm.PythonMessageSource())
publisher = builder.AddSystem(
mut.LcmPublisherSystem.Make(channel="LCMT_HEADER",
lcm_type=lcmt_header,
lcm=lcm,
publish_period=0.05))
builder.Connect(source.get_output_port(), publisher.get_input_port())
diagram = builder.Build()
diagram.Publish(diagram.CreateDefaultContext())
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
test_prefix = "TEST_PREFIX_"
def normal(builder, scene_graph):
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):
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 __init__(self, tree):
lcm = DrakeLcm()
self.tree = tree
self.visualizer = DrakeVisualizer(tree=self.tree,
lcm=lcm,
enable_playback=True)
self.x = np.concatenate([
robot.getZeroConfiguration(),
np.zeros(tree.get_num_velocities())
])
# Workaround for the fact that PublishLoadRobot is not public.
# Ultimately that should be fixed.
sim = Simulator(self.visualizer)
context = sim.get_mutable_context()
context.FixInputPort(0, BasicVector(self.x))
sim.Initialize()
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_apply_visualization_config(self):
"""Exercises VisualizationConfig and ApplyVisualizationConfig.
"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
plant.Finalize()
lcm_buses = LcmBuses()
lcm_buses.Add("default", DrakeLcm())
config = mut.VisualizationConfig(publish_period=0.25)
self.assertIn("publish_period", repr(config))
copy.copy(config)
mut.ApplyVisualizationConfig(config=config,
plant=plant,
scene_graph=scene_graph,
lcm_buses=lcm_buses,
builder=builder)
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)
def test_drake_visualizer_api(self):
tree = self._make_tree()
lcm = DrakeLcm()
# Test for existence.
viz = mut.DrakeVisualizer(tree=tree, lcm=lcm, enable_playback=True)
viz.set_publish_period(period=0.01)
# - Force publish to ensure we have enough knot points.
context = viz.CreateDefaultContext()
x0 = np.zeros(tree.get_num_positions() + tree.get_num_velocities())
viz.get_input_port(0).FixValue(context, x0)
context.SetTime(0.)
viz.Publish(context)
# Use a small time period, since it uses realtime playback.
context.SetTime(0.01)
viz.Publish(context)
viz.ReplayCachedSimulation()
# - Check that PublishLoadRobot can be called.
viz.PublishLoadRobot()
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 test_subscriber(self):
lcm = DrakeLcm()
dut = mut.LcmSubscriberSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
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)
# Test LcmInterfaceSystem overloads
lcm_system = mut.LcmInterfaceSystem(lcm=lcm)
dut = mut.LcmSubscriberSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
lcm=lcm_system)
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)
import numpy as np
rtree = getCassieTree()
plant = RigidBodyPlant(rtree, 0.0005)
builder = DiagramBuilder()
cassie = builder.AddSystem(plant)
setDefaultContactParams(cassie)
# precomputed standing fixed point state
# q = getNominalStandingConfiguration()
# qd = [0. for i in xrange(rtree.get_num_velocities())]
x = CassieFixedPointState()
# Setup visualizer
lcm = DrakeLcm()
visualizer = builder.AddSystem(
DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True))
builder.Connect(cassie.get_output_port(0), visualizer.get_input_port(0))
# Zero inputs -- passive forward simulation
u0 = ConstantVectorSource(np.zeros(rtree.get_num_actuators()))
null_controller = builder.AddSystem(u0)
builder.Connect(null_controller.get_output_port(0), cassie.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
def test_lcm(self):
self._test_lcm_interface(DrakeLcm())
def test_lcm(self):
dut = DrakeLcm()
self.assertIsInstance(dut, DrakeLcmInterface)
# Test virtual function names.
dut.Publish
dut.HandleSubscriptions
from pydrake.systems.framework import (
DiagramBuilder, )
from pydrake.systems.primitives import (
ConstantVectorSource, )
from pydrake.lcm import DrakeLcm
from pydrake.geometry import (Box, DrakeVisualizer)
from pydrake.math import (RigidTransform, RollPitchYaw)
from pydrake.multibody.plant import (AddMultibodyPlantSceneGraph,
CoulombFriction, Propeller, PropellerInfo)
from pydrake.multibody.tree import (SpatialInertia, RotationalInertia,
PlanarJoint)
from pydrake.systems.analysis import Simulator
global_lcm_ftw = DrakeLcm()
def make_box(mbp, name):
inertia = SpatialInertia.MakeFromCentralInertia(
1, [0, 0, 0], RotationalInertia(1 / 600, 1 / 120, 1 / 120))
body = mbp.AddRigidBody(name, inertia)
shape = Box(1, 0.1, 0.1)
mbp.RegisterVisualGeometry(body=body,
X_BG=RigidTransform(),
shape=shape,
name=f"{name}_visual",
diffuse_color=[1., 0.64, 0.0, 0.5])
body_friction = CoulombFriction(static_friction=0.6, dynamic_friction=0.5)
mbp.RegisterCollisionGeometry(body=body,
X_BG=RigidTransform(),
def __init__(self, *, meshcat_host=None, 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._poll_handlers = []
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=meshcat_host or "localhost",
port=meshcat_port,
show_stats_plot=False)
self.meshcat = Meshcat(params=params)
default_viewer = _ViewerApplet(meshcat=self.meshcat,
path="/DRAKE_VIEWER",
alpha_slider_name="Viewer α")
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT",
message_type=lcmt_viewer_load_robot,
handler=default_viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW",
message_type=lcmt_viewer_draw,
handler=default_viewer.on_viewer_draw)
self._poll(handler=default_viewer.on_poll)
illustration_viewer = _ViewerApplet(meshcat=self.meshcat,
path="/Visual Geometry",
alpha_slider_name="Visual α")
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT_ILLUSTRATION",
message_type=lcmt_viewer_load_robot,
handler=illustration_viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW_ILLUSTRATION",
message_type=lcmt_viewer_draw,
handler=illustration_viewer.on_viewer_draw)
self._poll(handler=illustration_viewer.on_poll)
proximity_viewer = _ViewerApplet(meshcat=self.meshcat,
path="/Collision Geometry",
alpha_slider_name="Collision α",
start_visible=False)
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT_PROXIMITY",
message_type=lcmt_viewer_load_robot,
handler=proximity_viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW_PROXIMITY",
message_type=lcmt_viewer_draw,
handler=proximity_viewer.on_viewer_draw)
self._poll(handler=proximity_viewer.on_poll)
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 test_lcm_driven_loop(self):
"""Duplicates the test logic in `lcm_driven_loop_test.cc`."""
lcm_url = "udpm://239.255.76.67:7669"
t_start = 3.
t_end = 10.
def publish_loop():
# Publishes a set of messages for the driven loop. This should be
# run from a separate process.
# N.B. Because of this, care should be taken not to share C++
# objects between process boundaries.
t = t_start
while t <= t_end:
message = header_t()
message.utime = int(1e6 * t)
lcm.Publish("TEST_LOOP", message.encode())
time.sleep(0.1)
t += 1
class DummySys(LeafSystem):
# Converts message to time in seconds.
def __init__(self):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort("header_t",
AbstractValue.Make(header_t))
self.DeclareVectorOutputPort(BasicVector(1), self._calc_output)
def _calc_output(self, context, output):
message = self.EvalAbstractInput(context, 0).get_value()
y = output.get_mutable_value()
y[:] = message.utime / 1e6
# Construct diagram for LcmDrivenLoop.
lcm = DrakeLcm(lcm_url)
utime = mut.PyUtimeMessageToSeconds(header_t)
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
builder = DiagramBuilder()
builder.AddSystem(sub)
dummy = builder.AddSystem(DummySys())
builder.Connect(sub.get_output_port(0), dummy.get_input_port(0))
logger = LogOutput(dummy.get_output_port(0), builder)
logger.set_forced_publish_only()
diagram = builder.Build()
dut = mut.LcmDrivenLoop(diagram, sub, None, lcm, utime)
dut.set_publish_on_every_received_message(True)
# N.B. Use `multiprocessing` instead of `threading` so that we may
# avoid issues with GIL deadlocks.
publish_proc = Process(target=publish_loop)
publish_proc.start()
# Initialize to first message.
first_msg = dut.WaitForMessage()
dut.get_mutable_context().SetTime(utime.GetTimeInSeconds(first_msg))
# Run to desired amount. (Anything more will cause interpreter to
# "freeze".)
dut.RunToSecondsAssumingInitialized(t_end)
publish_proc.join()
# Check expected values.
log_t_expected = np.array([4, 5, 6, 7, 8, 9])
log_t = logger.sample_times()
self.assertTrue(np.allclose(log_t_expected, log_t))
log_y = logger.data()
self.assertTrue(np.allclose(log_t_expected, log_y))
def add_drake_visualizer(scene_graph, builder):
lcm = DrakeLcm()
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph, lcm=lcm)
DispatchLoadMessage(scene_graph,
lcm) # TODO: only update viewer after a plan is found
return lcm # Important that variable is saved
def test_buses(self):
dut = mut.LcmBuses()
dut.Add("default", DrakeLcm())
self.assertEqual(dut.size(), 1)
self.assertIsInstance(dut.Find("Basic test", "default"), DrakeLcm)
self.assertEqual(len(dut.GetAllBusNames()), 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()``).
"""
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)
def test_lcm(self):
dut = DrakeLcm()
self.assertIsInstance(dut, DrakeLcmInterface)
# Quickly start and stop the receiving thread.
dut.StartReceiveThread()
dut.StopReceiveThread()
def test_subscriber_wait_for_message_with_timeout(self):
"""Confirms that the subscriber times out."""
lcm = DrakeLcm("memq://")
lcm.StartReceiveThread()
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
sub.WaitForMessage(0, timeout=0.02)
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 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()
def test_deprecated(self):
with catch_drake_warnings(expected_count=1):
DrakeLcm(lcm_url="", defer_initialization=True)
