def test_lcm(self): dut = DrakeLcm() self.assertIsInstance(dut, DrakeLcmInterface) dut.get_lcm_url() DrakeLcm(lcm_url="") # Test virtual function names. dut.Publish dut.HandleSubscriptions
def test_params(self): dut = DrakeLcmParams() dut.lcm_url = "memq://123" dut.channel_suffix = "_foo" dut.defer_initialization = True instance = DrakeLcm(params=dut) self.assertTrue(instance.get_lcm_url(), "memq://123") self.assertIn("lcm_url", repr(dut)) copy.copy(dut)
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)
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)