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)