Drake
lcm_subscriber_system.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <condition_variable>
4 #include <memory>
5 #include <mutex>
6 #include <string>
7 #include <vector>
8 
19 
20 namespace drake {
21 namespace systems {
22 namespace lcm {
23 
24 /**
25  * Receives LCM messages from a given channel and outputs them to a
26  * System<double>'s port. This class stores the most recently processed LCM
27  * message in the State. When a LCM message arrives asynchronously, an update
28  * event is scheduled to process the message and store it in the State at the
29  * earliest possible simulation time. The output is always consistent with the
30  * State.
31  *
32  * To process a LCM message, CalcNextUpdateTime() needs to be called first to
33  * check for new messages and schedule a callback event if a new LCM message
34  * has arrived. The message is then processed and stored in the Context by
35  * CalcDiscreteVariableUpdates() or CalcUnrestrictedUpdate() depending on the
36  * output type. When this system is evaluated by the Simulator, all these
37  * operations are taken care of by the Simulator. On the other hand, the user
38  * needs to manually replicate this process without the Simulator.
39  *
40  * If LCM service in use is a drake::lcm::DrakeLcmLog (not live operation),
41  * then see drake::systems::lcm::LcmLogPlaybackSystem for a helper to advance
42  * the log cursor in concert with the simulation.
43  *
44  * @ingroup message_passing
45  */
46 class LcmSubscriberSystem : public LeafSystem<double> {
47  public:
49 
50  /**
51  * Factory method that returns a subscriber System that provides
52  * Value<LcmMessage> message objects on its sole abstract-valued output port.
53  *
54  * @tparam LcmMessage message type to deserialize, e.g., lcmt_drake_signal.
55  *
56  * @param[in] channel The LCM channel on which to subscribe.
57  *
58  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
59  */
60  template <typename LcmMessage>
61  static std::unique_ptr<LcmSubscriberSystem> Make(
62  const std::string& channel, drake::lcm::DrakeLcmInterface* lcm) {
63  return std::make_unique<LcmSubscriberSystem>(
64  channel, std::make_unique<Serializer<LcmMessage>>(), lcm);
65  }
66 
67  /**
68  * Constructor that returns a subscriber System that provides message objects
69  * on its sole abstract-valued output port. The type of the message object is
70  * determined by the @p serializer.
71  *
72  * @param[in] channel The LCM channel on which to subscribe.
73  *
74  * @param[in] serializer The serializer that converts between byte vectors
75  * and LCM message objects.
76  *
77  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
78  */
79  LcmSubscriberSystem(const std::string& channel,
80  std::unique_ptr<SerializerInterface> serializer,
82 
83  /**
84  * Constructor that returns a subscriber System that provides vector data on
85  * its sole vector-valued output port. The message contents are mapped to
86  * vector data by the @p translator.
87  *
88  * @param[in] channel The LCM channel on which to subscribe.
89  *
90  * @param[in] translator A reference to the translator that converts between
91  * LCM message objects and `drake::systems::VectorBase` objects. The
92  * reference must remain valid for the lifetime of this `LcmSubscriberSystem`
93  * object.
94  *
95  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
96  */
97  LcmSubscriberSystem(const std::string& channel,
98  const LcmAndVectorBaseTranslator& translator,
100 
101  /**
102  * Constructor that returns a subscriber System that provides vector data on
103  * its sole vector-valued output port. The message contents are mapped to
104  * vector data by the a translator found in the @p translator_dictionary.
105  *
106  * @param[in] channel The LCM channel on which to subscribe.
107  *
108  * @param[in] translator_dictionary A dictionary for obtaining the appropriate
109  * translator for a particular LCM channel. The reference must remain valid
110  * for the lifetime of this `LcmSubscriberSystem` object.
111  *
112  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
113  */
114  LcmSubscriberSystem(const std::string& channel,
115  const LcmTranslatorDictionary& translator_dictionary,
117 
118  ~LcmSubscriberSystem() override;
119 
120  /// Returns the default name for a system that subscribes to @p channel.
121  static std::string make_name(const std::string& channel);
122 
123  const std::string& get_channel_name() const;
124 
125  /**
126  * Returns the translator used by this subscriber. This translator can be used
127  * to translate a BasicVector into a serialized LCM message, which is then
128  * passed to DrakeMockLcm::InduceSubscriberCallback(). This mimics a message
129  * reception by an LCM subscriber and is useful for unit testing.
130  * @pre this system is using a vector-valued port (not abstract-valued).
131  */
133 
134  /// Returns the sole output port.
138  }
139 
140  DRAKE_DEPRECATED("Don't use the indexed overload; use the no-arg overload.")
141  const OutputPort<double>& get_output_port(int index) const {
142  DRAKE_THROW_UNLESS(index == 0);
143  return get_output_port();
144  }
145 
146  // This system has no input ports.
147  void get_input_port(int) = delete;
148 
149  /**
150  * Blocks the caller until its internal message count exceeds
151  * `old_message_count`.
152  * @param old_message_count Internal message counter.
153  * @param message If non-null, will return the received message.
154  * @pre If `message` is specified, this system must be abstract-valued.
155  */
156  int WaitForMessage(
157  int old_message_count, AbstractValue* message = nullptr) const;
158 
159  /**
160  * Returns the internal message counter. Meant to be used with
161  * `WaitForMessage`.
162  */
163  int GetInternalMessageCount() const;
164 
165  /**
166  * Returns the message counter stored in @p context.
167  */
168  int GetMessageCount(const Context<double>& context) const;
169 
170  protected:
171  void DoCalcNextUpdateTime(const Context<double>& context,
173  double* time) const override;
174 
176  const Context<double>&,
178  State<double>* state) const override {
179  ProcessMessageAndStoreToAbstractState(&state->get_mutable_abstract_state());
180  }
181 
182  std::unique_ptr<AbstractValues> AllocateAbstractState() const override;
183 
185  const Context<double>&,
187  DiscreteValues<double>* discrete_state) const override {
188  ProcessMessageAndStoreToDiscreteState(discrete_state);
189  }
190 
191  std::unique_ptr<DiscreteValues<double>> AllocateDiscreteState()
192  const override;
193 
194  void SetDefaultState(const Context<double>& context,
195  State<double>* state) const override;
196 
197  private:
198  // All constructors delegate to here.
199  LcmSubscriberSystem(const std::string& channel,
200  const LcmAndVectorBaseTranslator* translator,
201  std::unique_ptr<SerializerInterface> serializer,
203 
204  void ProcessMessageAndStoreToDiscreteState(
205  DiscreteValues<double>* discrete_state) const;
206 
207  void ProcessMessageAndStoreToAbstractState(
208  AbstractValues* abstract_state) const;
209 
210  // Callback entry point from LCM into this class.
211  void HandleMessage(const void*, int);
212 
213  // This pair of methods is used for the output port when we're using a
214  // translator.
215  std::unique_ptr<BasicVector<double>> AllocateTranslatorOutputValue() const;
216  void CalcTranslatorOutputValue(const Context<double>& context,
217  BasicVector<double>* output_vector) const;
218 
219  // This pair of methods is used for the output port when we're using a
220  // serializer.
221  std::unique_ptr<systems::AbstractValue> AllocateSerializerOutputValue() const;
222  void CalcSerializerOutputValue(const Context<double>& context,
223  AbstractValue* output_value) const;
224 
225  // The channel on which to receive LCM messages.
226  const std::string channel_;
227 
228  // Converts LCM message bytes to VectorBase objects.
229  // Will be non-null iff our output port is vector-valued.
230  const LcmAndVectorBaseTranslator* const translator_{};
231 
232  // Converts LCM message bytes to Value<LcmMessage> objects.
233  // Will be non-null iff our output port is abstract-valued.
234  const std::unique_ptr<SerializerInterface> serializer_;
235 
236  // The mutex that guards received_message_ and received_message_count_.
237  mutable std::mutex received_message_mutex_;
238 
239  // A condition variable that's signaled every time the handler is called.
240  mutable std::condition_variable received_message_condition_variable_;
241 
242  // The bytes of the most recently received LCM message.
243  std::vector<uint8_t> received_message_;
244 
245  // A message counter that's incremented every time the handler is called.
246  int received_message_count_{0};
247 };
248 
249 } // namespace lcm
250 } // namespace systems
251 } // namespace drake
AbstractValues is a container for non-numerical state and parameters.
Definition: abstract_values.h:18
std::unique_ptr< DiscreteValues< double > > AllocateDiscreteState() const override
Reserves the discrete state as required by CreateDefaultContext.
Definition: lcm_subscriber_system.cc:182
~LcmSubscriberSystem() override
Definition: lcm_subscriber_system.cc:84
const OutputPort< double > & get_output_port() const
Returns the sole output port.
Definition: lcm_subscriber_system.h:135
Provides a portable macro for use in generating compile-time warnings for use of code that is permitt...
Provides a convenient wrapper to throw an exception when a condition is unmet.
Definition: automotive_demo.cc:90
This class represents an unrestricted update event.
Definition: event.h:482
A dictionary that maps between LCM channel names and translators that convert between LCM message obj...
Definition: lcm_translator_dictionary.h:20
void DoCalcUnrestrictedUpdate(const Context< double > &, const std::vector< const systems::UnrestrictedUpdateEvent< double > * > &, State< double > *state) const override
Derived-class event handler for all simultaneous unrestricted update events.
Definition: lcm_subscriber_system.h:175
void DoCalcDiscreteVariableUpdates(const Context< double > &, const std::vector< const systems::DiscreteUpdateEvent< double > * > &, DiscreteValues< double > *discrete_state) const override
Derived-class event handler for all simultaneous discrete update events.
Definition: lcm_subscriber_system.h:184
#define DRAKE_THROW_UNLESS(condition)
Evaluates condition and iff the value is false will throw an exception with a message showing at leas...
Definition: drake_throw.h:23
std::vector< double > vector
Definition: translator_test.cc:20
LcmSubscriberSystem(const LcmSubscriberSystem &)=delete
A pure virtual interface that enables LCM to be mocked.
Definition: drake_lcm_interface.h:22
Receives LCM messages from a given channel and outputs them to a System<double>&#39;s port...
Definition: lcm_subscriber_system.h:46
std::unique_ptr< AbstractValues > AllocateAbstractState() const override
Reserves the abstract state as required by CreateDefaultContext.
Definition: lcm_subscriber_system.cc:198
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:83
const LcmAndVectorBaseTranslator & get_translator() const
Returns the translator used by this subscriber.
Definition: lcm_subscriber_system.cc:298
void SetDefaultState(const Context< double > &context, State< double > *state) const override
Assigns default values to all elements of the state.
Definition: lcm_subscriber_system.cc:86
static std::unique_ptr< LcmSubscriberSystem > Make(const std::string &channel, drake::lcm::DrakeLcmInterface *lcm)
Factory method that returns a subscriber System that provides Value<LcmMessage> message objects on it...
Definition: lcm_subscriber_system.h:61
A fully type-erased container class.
Definition: value.h:101
void DoCalcNextUpdateTime(const Context< double > &context, systems::CompositeEventCollection< double > *events, double *time) const override
Computes the next time at which this System must perform a discrete action.
Definition: lcm_subscriber_system.cc:146
#define DRAKE_DEPRECATED(message)
Use DRAKE_DEPRECATED("message") to discourage use of particular classes, typedefs, variables, non-static data members, functions, arguments, enumerations, and template specializations.
Definition: drake_deprecated.h:33
const double time
Definition: robot_plan_interpolator_test.cc:64
const std::string & get_channel_name() const
Definition: lcm_subscriber_system.cc:217
AbstractValues & get_mutable_abstract_state()
Definition: state.h:88
Defines an abstract parent class of all translators that convert between LCM message bytes and drake:...
Definition: lcm_and_vector_base_translator.h:18
int WaitForMessage(int old_message_count, AbstractValue *message=nullptr) const
Blocks the caller until its internal message count exceeds old_message_count.
Definition: lcm_subscriber_system.cc:266
int get_num_output_ports() const
Returns the number of output ports of the system.
Definition: system.h:966
int GetMessageCount(const Context< double > &context) const
Returns the message counter stored in context.
Definition: lcm_subscriber_system.cc:131
static std::string make_name(const std::string &channel)
Returns the default name for a system that subscribes to channel.
Definition: lcm_subscriber_system.cc:213
const OutputPort< T > & get_output_port(int port_index) const
Returns the typed output port at index port_index.
Definition: system.h:979
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
int GetInternalMessageCount() const
Returns the internal message counter.
Definition: lcm_subscriber_system.cc:293
This class represents a discrete update event.
Definition: event.h:404
Provides careful macros to selectively enable or disable the special member functions for copy-constr...