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 
9 #include "drake/common/drake_copyable.h"
10 #include "drake/common/drake_deprecated.h"
11 #include "drake/common/drake_throw.h"
12 #include "drake/lcm/drake_lcm_interface.h"
13 #include "drake/lcm/drake_lcm_message_handler_interface.h"
14 #include "drake/systems/framework/basic_vector.h"
15 #include "drake/systems/framework/leaf_system.h"
16 #include "drake/systems/lcm/lcm_and_vector_base_translator.h"
17 #include "drake/systems/lcm/lcm_translator_dictionary.h"
18 #include "drake/systems/lcm/serializer.h"
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  * @ingroup message_passing
41  */
42 class LcmSubscriberSystem : public LeafSystem<double>,
44  public:
46 
47  /**
48  * Factory method that returns a subscriber System that provides
49  * Value<LcmMessage> message objects on its sole abstract-valued output port.
50  *
51  * @tparam LcmMessage message type to deserialize, e.g., lcmt_drake_signal.
52  *
53  * @param[in] channel The LCM channel on which to subscribe.
54  *
55  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
56  */
57  template <typename LcmMessage>
58  static std::unique_ptr<LcmSubscriberSystem> Make(
59  const std::string& channel, drake::lcm::DrakeLcmInterface* lcm) {
60  return std::make_unique<LcmSubscriberSystem>(
61  channel, std::make_unique<Serializer<LcmMessage>>(), lcm);
62  }
63 
64  /**
65  * Constructor that returns a subscriber System that provides message objects
66  * on its sole abstract-valued output port. The type of the message object is
67  * determined by the @p serializer.
68  *
69  * @param[in] channel The LCM channel on which to subscribe.
70  *
71  * @param[in] serializer The serializer that converts between byte vectors
72  * and LCM message objects.
73  *
74  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
75  */
76  LcmSubscriberSystem(const std::string& channel,
77  std::unique_ptr<SerializerInterface> serializer,
79 
80  /**
81  * Constructor that returns a subscriber System that provides vector data on
82  * its sole vector-valued output port. The message contents are mapped to
83  * vector data by the @p translator.
84  *
85  * @param[in] channel The LCM channel on which to subscribe.
86  *
87  * @param[in] translator A reference to the translator that converts between
88  * LCM message objects and `drake::systems::VectorBase` objects. The
89  * reference must remain valid for the lifetime of this `LcmSubscriberSystem`
90  * object.
91  *
92  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
93  */
94  LcmSubscriberSystem(const std::string& channel,
95  const LcmAndVectorBaseTranslator& translator,
97 
98  /**
99  * Constructor that returns a subscriber System that provides vector data on
100  * its sole vector-valued output port. The message contents are mapped to
101  * vector data by the a translator found in the @p translator_dictionary.
102  *
103  * @param[in] channel The LCM channel on which to subscribe.
104  *
105  * @param[in] translator_dictionary A dictionary for obtaining the appropriate
106  * translator for a particular LCM channel. The reference must remain valid
107  * for the lifetime of this `LcmSubscriberSystem` object.
108  *
109  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
110  */
111  LcmSubscriberSystem(const std::string& channel,
112  const LcmTranslatorDictionary& translator_dictionary,
114 
115  ~LcmSubscriberSystem() override;
116 
117  /// Returns the default name for a system that subscribes to @p channel.
118  static std::string make_name(const std::string& channel);
119 
120  const std::string& get_channel_name() const;
121 
122  /**
123  * Returns the translator used by this subscriber. This translator can be used
124  * to translate a BasicVector into a serialized LCM message, which is then
125  * passed to DrakeMockLcm::InduceSubscriberCallback(). This mimics a message
126  * reception by an LCM subscriber and is useful for unit testing.
127  * @pre this system is using a vector-valued port (not abstract-valued).
128  */
130 
131  /// Returns the sole output port.
135  }
136 
137  DRAKE_DEPRECATED("Don't use the indexed overload; use the no-arg overload.")
138  const OutputPort<double>& get_output_port(int index) const {
139  DRAKE_THROW_UNLESS(index == 0);
140  return get_output_port();
141  }
142 
143  // This system has no input ports.
144  void get_input_port(int) = delete;
145 
146  /**
147  * Blocks the caller until @p old_message_count is different from the
148  * internal message counter, and the internal message counter is returned.
149  */
150  int WaitForMessage(int old_message_count) const;
151 
152  /**
153  * Returns the message counter stored in @p context.
154  */
155  int GetMessageCount(const Context<double>& context) const;
156 
157  protected:
158  void DoCalcNextUpdateTime(const Context<double>& context,
160  double* time) const override;
161 
163  const Context<double>&,
165  State<double>* state) const override {
166  ProcessMessageAndStoreToAbstractState(&state->get_mutable_abstract_state());
167  }
168 
169  std::unique_ptr<AbstractValues> AllocateAbstractState() const override;
170 
172  const Context<double>&,
174  DiscreteValues<double>* discrete_state) const override {
175  ProcessMessageAndStoreToDiscreteState(discrete_state);
176  }
177 
178  std::unique_ptr<DiscreteValues<double>> AllocateDiscreteState()
179  const override;
180 
181  void SetDefaultState(const Context<double>& context,
182  State<double>* state) const override;
183 
184  private:
185  // All constructors delegate to here.
186  LcmSubscriberSystem(const std::string& channel,
187  const LcmAndVectorBaseTranslator* translator,
188  std::unique_ptr<SerializerInterface> serializer,
190 
191  void ProcessMessageAndStoreToDiscreteState(
192  DiscreteValues<double>* discrete_state) const;
193 
194  void ProcessMessageAndStoreToAbstractState(
195  AbstractValues* abstract_state) const;
196 
197  // Callback entry point from LCM into this class. Also wakes up one thread
198  // block on notification_ if it's not nullptr.
199  void HandleMessage(const std::string& channel, const void* message_buffer,
200  int message_size) override;
201 
202  // This pair of methods is used for the output port when we're using a
203  // translator.
204  std::unique_ptr<BasicVector<double>> AllocateTranslatorOutputValue() const;
205  void CalcTranslatorOutputValue(const Context<double>& context,
206  BasicVector<double>* output_vector) const;
207 
208  // This pair of methods is used for the output port when we're using a
209  // serializer.
210  std::unique_ptr<systems::AbstractValue> AllocateSerializerOutputValue() const;
211  void CalcSerializerOutputValue(const Context<double>& context,
212  AbstractValue* output_value) const;
213 
214  // The channel on which to receive LCM messages.
215  const std::string channel_;
216 
217  // Converts LCM message bytes to VectorBase objects.
218  // Will be non-null iff our output port is vector-valued.
219  const LcmAndVectorBaseTranslator* const translator_{};
220 
221  // Converts LCM message bytes to Value<LcmMessage> objects.
222  // Will be non-null iff our output port is abstract-valued.
223  const std::unique_ptr<SerializerInterface> serializer_;
224 
225  // The mutex that guards received_message_ and received_message_count_.
226  mutable std::mutex received_message_mutex_;
227 
228  // A condition variable that's signaled every time the handler is called.
229  mutable std::condition_variable received_message_condition_variable_;
230 
231  // The bytes of the most recently received LCM message.
232  std::vector<uint8_t> received_message_;
233 
234  // A message counter that's incremented every time the handler is called.
235  int received_message_count_{0};
236 
237  drake::lcm::DrakeLcmInterface* lcm_interface_;
238 };
239 
240 } // namespace lcm
241 } // namespace systems
242 } // 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:209
~LcmSubscriberSystem() override
Definition: lcm_subscriber_system.cc:83
const OutputPort< double > & get_output_port() const
Returns the sole output port.
Definition: lcm_subscriber_system.h:132
Definition: automotive_demo.cc:89
This class represents an unrestricted update event.
Definition: event.h:349
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:162
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:171
#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:15
int WaitForMessage(int old_message_count) const
Blocks the caller until old_message_count is different from the internal message counter, and the internal message counter is returned.
Definition: lcm_subscriber_system.cc:304
Receives LCM messages from a given channel and outputs them to a System<double>&#39;s port...
Definition: lcm_subscriber_system.h:42
std::unique_ptr< AbstractValues > AllocateAbstractState() const override
Reserves the abstract state as required by CreateDefaultContext.
Definition: lcm_subscriber_system.cc:225
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
const LcmAndVectorBaseTranslator & get_translator() const
Returns the translator used by this subscriber.
Definition: lcm_subscriber_system.cc:323
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:85
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:58
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:145
#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:244
Defines a message handler interface that must be implemented by all LCM subscribers within Drake...
Definition: drake_lcm_message_handler_interface.h:17
AbstractValues & get_mutable_abstract_state()
Definition: state.h:78
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 get_num_output_ports() const
Returns the number of output ports of the system.
Definition: system.h:972
int GetMessageCount(const Context< double > &context) const
Returns the message counter stored in context.
Definition: lcm_subscriber_system.cc:130
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:240
const OutputPort< T > & get_output_port(int port_index) const
Returns the output port at index port_index.
Definition: system.h:988
#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
This class represents a discrete update event.
Definition: event.h:279