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 
17 
18 namespace drake {
19 namespace systems {
20 namespace lcm {
21 
22 /**
23  * Receives LCM messages from a given channel and outputs them to a
24  * System<double>'s port. This class stores the most recently processed LCM
25  * message in the State. When a LCM message arrives asynchronously, an update
26  * event is scheduled to process the message and store it in the State at the
27  * earliest possible simulation time. The output is always consistent with the
28  * State.
29  *
30  * To process a LCM message, CalcNextUpdateTime() needs to be called first to
31  * check for new messages and schedule a callback event if a new LCM message
32  * has arrived. The message is then processed and stored in the Context by
33  * CalcDiscreteVariableUpdates() or CalcUnrestrictedUpdate() depending on the
34  * output type. When this system is evaluated by the Simulator, all these
35  * operations are taken care of by the Simulator. On the other hand, the user
36  * needs to manually replicate this process without the Simulator.
37  *
38  * @ingroup message_passing
39  */
40 class LcmSubscriberSystem : public LeafSystem<double>,
42  public:
44 
45  /**
46  * Factory method that returns a subscriber System that provides
47  * Value<LcmMessage> message objects on its sole abstract-valued output port.
48  *
49  * @tparam LcmMessage message type to deserialize, e.g., lcmt_drake_signal.
50  *
51  * @param[in] channel The LCM channel on which to subscribe.
52  *
53  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
54  */
55  template <typename LcmMessage>
56  static std::unique_ptr<LcmSubscriberSystem> Make(
57  const std::string& channel, drake::lcm::DrakeLcmInterface* lcm) {
58  return std::make_unique<LcmSubscriberSystem>(
59  channel, std::make_unique<Serializer<LcmMessage>>(), lcm);
60  }
61 
62  /**
63  * Constructor that returns a subscriber System that provides message objects
64  * on its sole abstract-valued output port. The type of the message object is
65  * determined by the @p serializer.
66  *
67  * @param[in] channel The LCM channel on which to subscribe.
68  *
69  * @param[in] serializer The serializer that converts between byte vectors
70  * and LCM message objects.
71  *
72  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
73  */
74  LcmSubscriberSystem(const std::string& channel,
75  std::unique_ptr<SerializerInterface> serializer,
77 
78  /**
79  * Constructor that returns a subscriber System that provides vector data on
80  * its sole vector-valued output port. The message contents are mapped to
81  * vector data by the @p translator.
82  *
83  * @param[in] channel The LCM channel on which to subscribe.
84  *
85  * @param[in] translator A reference to the translator that converts between
86  * LCM message objects and `drake::systems::VectorBase` objects. The
87  * reference must remain valid for the lifetime of this `LcmSubscriberSystem`
88  * object.
89  *
90  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
91  */
92  LcmSubscriberSystem(const std::string& channel,
93  const LcmAndVectorBaseTranslator& translator,
95 
96  /**
97  * Constructor that returns a subscriber System that provides vector data on
98  * its sole vector-valued output port. The message contents are mapped to
99  * vector data by the a translator found in the @p translator_dictionary.
100  *
101  * @param[in] channel The LCM channel on which to subscribe.
102  *
103  * @param[in] translator_dictionary A dictionary for obtaining the appropriate
104  * translator for a particular LCM channel. The reference must remain valid
105  * for the lifetime of this `LcmSubscriberSystem` object.
106  *
107  * @param lcm A non-null pointer to the LCM subsystem to subscribe on.
108  */
109  LcmSubscriberSystem(const std::string& channel,
110  const LcmTranslatorDictionary& translator_dictionary,
112 
113  ~LcmSubscriberSystem() override;
114 
115  /// Returns the default name for a system that subscribes to @p channel.
116  static std::string make_name(const std::string& channel);
117 
118  const std::string& get_channel_name() const;
119 
120  /**
121  * Returns the translator used by this subscriber. This translator can be used
122  * to translate a BasicVector into a serialized LCM message, which is then
123  * passed to DrakeMockLcm::InduceSubscriberCallback(). This mimics a message
124  * reception by an LCM subscriber and is useful for unit testing.
125  * @pre this system is using a vector-valued port (not abstract-valued).
126  */
128 
129  /**
130  * Blocks the caller until @p old_message_count is different from the
131  * internal message counter, and the internal message counter is returned.
132  */
133  int WaitForMessage(int old_message_count) const;
134 
135  /**
136  * Returns the message counter stored in @p context.
137  */
138  int GetMessageCount(const Context<double>& context) const;
139 
140  protected:
141  void DoCalcNextUpdateTime(const Context<double>& context,
143  double* time) const override;
144 
146  const Context<double>&,
148  State<double>* state) const override {
149  ProcessMessageAndStoreToAbstractState(&state->get_mutable_abstract_state());
150  }
151 
152  std::unique_ptr<AbstractValues> AllocateAbstractState() const override;
153 
155  const Context<double>&,
157  DiscreteValues<double>* discrete_state) const override {
158  ProcessMessageAndStoreToDiscreteState(discrete_state);
159  }
160 
161  std::unique_ptr<DiscreteValues<double>> AllocateDiscreteState()
162  const override;
163 
164  void SetDefaultState(const Context<double>& context,
165  State<double>* state) const override;
166 
167  private:
168  // All constructors delegate to here.
169  LcmSubscriberSystem(const std::string& channel,
170  const LcmAndVectorBaseTranslator* translator,
171  std::unique_ptr<SerializerInterface> serializer,
173 
174  void ProcessMessageAndStoreToDiscreteState(
175  DiscreteValues<double>* discrete_state) const;
176 
177  void ProcessMessageAndStoreToAbstractState(
178  AbstractValues* abstract_state) const;
179 
180  // Callback entry point from LCM into this class. Also wakes up one thread
181  // block on notification_ if it's not nullptr.
182  void HandleMessage(const std::string& channel, const void* message_buffer,
183  int message_size) override;
184 
185  // This pair of methods is used for the output port when we're using a
186  // translator.
187  std::unique_ptr<BasicVector<double>> AllocateTranslatorOutputValue() const;
188  void CalcTranslatorOutputValue(const Context<double>& context,
189  BasicVector<double>* output_vector) const;
190 
191  // This pair of methods is used for the output port when we're using a
192  // serializer.
193  std::unique_ptr<systems::AbstractValue> AllocateSerializerOutputValue() const;
194  void CalcSerializerOutputValue(const Context<double>& context,
195  AbstractValue* output_value) const;
196 
197  // The channel on which to receive LCM messages.
198  const std::string channel_;
199 
200  // Converts LCM message bytes to VectorBase objects.
201  // Will be non-null iff our output port is vector-valued.
202  const LcmAndVectorBaseTranslator* const translator_{};
203 
204  // Converts LCM message bytes to Value<LcmMessage> objects.
205  // Will be non-null iff our output port is abstract-valued.
206  const std::unique_ptr<SerializerInterface> serializer_;
207 
208  // The mutex that guards received_message_ and received_message_count_.
209  mutable std::mutex received_message_mutex_;
210 
211  // A condition variable that's signaled every time the handler is called.
212  mutable std::condition_variable received_message_condition_variable_;
213 
214  // The bytes of the most recently received LCM message.
215  std::vector<uint8_t> received_message_;
216 
217  // A message counter that's incremented every time the handler is called.
218  int received_message_count_{0};
219 
220  drake::lcm::DrakeLcmInterface* lcm_interface_;
221 };
222 
223 } // namespace lcm
224 } // namespace systems
225 } // 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
Definition: automotive_demo.cc:88
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:145
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:154
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:40
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:56
A fully type-erased container class.
Definition: value.h:99
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
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 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
#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
Provides careful macros to selectively enable or disable the special member functions for copy-constr...