forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
37 lines
1.3 KiB
37 lines
1.3 KiB
# See `ExecuteExtraPythonCode` in `pydrake_pybind.h` for usage details and
|
|
# rationale.
|
|
|
|
|
|
# This is a python reimplementation of drake::lcm::Subscriber. We reimplement
|
|
# (rather than bind) the C++ class because we want message() to be a python LCM
|
|
# message, not a C++ object.
|
|
class Subscriber:
|
|
"""Subscribes to and stores a copy of the most recent message on a given
|
|
channel, for some message type. This class does NOT provide any mutex
|
|
behavior for multi-threaded locking; it should only be used in cases where
|
|
the governing DrakeLcmInterface.HandleSubscriptions is called from the same
|
|
thread that owns all copies of this object.
|
|
"""
|
|
|
|
def __init__(self, lcm, channel, lcm_type):
|
|
"""Creates a subscriber and subscribes to `channel` on `lcm`.
|
|
|
|
Args:
|
|
lcm: LCM service instance (a pydrake.lcm.DrakeLcmInterface).
|
|
channel: LCM channel name.
|
|
lcm_type: Python class generated by lcmgen.
|
|
"""
|
|
self.lcm_type = lcm_type
|
|
self.clear()
|
|
lcm.Subscribe(channel=channel, handler=self._handler)
|
|
|
|
def clear(self):
|
|
self.count = 0
|
|
self.raw = []
|
|
self.message = self.lcm_type()
|
|
|
|
def _handler(self, raw):
|
|
self.count += 1
|
|
self.raw = raw
|
|
self.message = self.lcm_type.decode(raw)
|