Drake
LcmDrivenLoop Class Reference

This class implements a loop that's driven by a Lcm message. More...

#include <systems/lcm/lcm_driven_loop.h>

Public Member Functions

 LcmDrivenLoop (const System< double > &system, const LcmSubscriberSystem &driving_subscriber, std::unique_ptr< Context< double >> context, drake::lcm::DrakeLcm *lcm, std::unique_ptr< LcmMessageToTimeInterface > time_converter)
 Constructor. More...
 
const AbstractValueWaitForMessage ()
 Blocks the caller until a driving Lcm message is received, then returns a const reference of AbstractValue to that message. More...
 
void RunToSecondsAssumingInitialized (double stop_time=std::numeric_limits< double >::infinity())
 Starts the message handling loop assuming the context (e.g. More...
 
void set_publish_on_every_received_message (bool flag)
 Sets a flag that forces Publish() at the very beginning of the message handling loop as well as inside the loop. More...
 
Context< double > & get_mutable_context ()
 Returns a mutable reference to the context. More...
 
const LcmMessageToTimeInterfaceget_message_to_time_converter () const
 Returns a const reference to the message to seconds converter. More...
 
Does not allow copy, move, or assignment
 LcmDrivenLoop (const LcmDrivenLoop &)=delete
 
LcmDrivenLoopoperator= (const LcmDrivenLoop &)=delete
 
 LcmDrivenLoop (LcmDrivenLoop &&)=delete
 
LcmDrivenLoopoperator= (LcmDrivenLoop &&)=delete
 

Detailed Description

This class implements a loop that's driven by a Lcm message.

The context time is explicitly slaved to the time in the received Lcm message. This class is intended to provide a generalized way to implement a message handling loop: an input message arrives, from which a response is computed and published. A common use case is to implement a distributed controller for a physical robot, where low level communication with the hardware is handled in the device driver (a separate process than the controller). The device driver sends a message containing the estimated state. The controller processes that message and sends back a command in response. The device driver finally receives and executes the command.

This class is designed to be agnostic to different types of the driving Lcm message to provide a generic API. The Lcm message is internally encapsulated in AbstractValue, which erases its type. In addition, the message time stamp is the only required information by this class, which can be extracted by an instance of LcmMessageToTimeInterface. It is assumed that the caller knows the concrete type of the message, and is able to supply a time converter.

This class uses the Simulator class internally for event handling (kPublishAction, kDiscreteUpdateAction, kUnrestrictedUpdateAction) and continuous state integration (e.g. the I term in a PID). The main message handling loop conceptually is:

while(context.time < stop_time) {
  msg = wait_for_message("channel");
  simulator.StepTo(msg.time);
  if (publish) {
    system.Publish(simulator.context);
  }
}

Since time is slaved to some outside source, the user needs to be mindful of system's configuration especially about event timing. For example, let us assume that system is configured to perform a discrete time action every 5ms (at 200Hz), and the necessary computation for system to step forward in time is very small. Now, suppose the driving message arrives at 1 Hz in real time. One would observe 200 such actions occur in rapid succession followed by nearly one second of silence. This is because msg = wait_for_message("channel") takes about one second in real time, and simulator.StepTo(msg.time), which forwards the simulator's clock by one second and performs 200 actions takes about 0 seconds in real time. The root cause is that the 200Hz rate of the handler system is tied to the internal virtual clock rather than real time. This problem is less significant when the computation time for handling one message is roughly the same as the interval between consecutive driving messages.

This implementation relies on several assumptions:

  1. The loop is blocked only on one Lcm message.
  2. It's pointless to for the handler system to perform any computation without a new Lcm message, thus the handler loop is blocking.
  3. The computation for the given system should be faster than the incoming message rate.

Constructor & Destructor Documentation

LcmDrivenLoop ( const LcmDrivenLoop )
delete
LcmDrivenLoop ( LcmDrivenLoop &&  )
delete
LcmDrivenLoop ( const System< double > &  system,
const LcmSubscriberSystem driving_subscriber,
std::unique_ptr< Context< double >>  context,
drake::lcm::DrakeLcm lcm,
std::unique_ptr< LcmMessageToTimeInterface time_converter 
)

Constructor.

Parameters
systemConst reference to the handler system. Its life span must be longer than this.
driving_subscriberConst reference to the driving subscriber. Its life span must be longer than this.
contextUnique pointer to a context allocated for system. Can be nullptr, in which case a context will be allocated internally.
lcmPointer to Lcm interface. Its life span must be longer than this. lcm cannot be nullptr, otherwise aborts.
time_converterUnique pointer to a converter that extracts time in seconds from the driving message time. Cannot be nullptr, otherwise this aborts. time_converter is necessary because of two reasons. 1: The Lcm message type agnostic design of this class. 2: Lcm messages lack a uniform time stamp field that has consistent units. So extracting the time stamp depends on the concrete message content.

Here is the call graph for this function:

Member Function Documentation

const LcmMessageToTimeInterface& get_message_to_time_converter ( ) const
inline

Returns a const reference to the message to seconds converter.

Here is the caller graph for this function:

Context<double>& get_mutable_context ( )
inline

Returns a mutable reference to the context.

Here is the caller graph for this function:

LcmDrivenLoop& operator= ( LcmDrivenLoop &&  )
delete
LcmDrivenLoop& operator= ( const LcmDrivenLoop )
delete
void RunToSecondsAssumingInitialized ( double  stop_time = std::numeric_limits<double>::infinity())

Starts the message handling loop assuming the context (e.g.

state and time) has already been properly initialized by the caller if necessary.

Parameters
stop_timeEnd time in seconds relative to the time stamp in the driving Lcm message.

Here is the call graph for this function:

Here is the caller graph for this function:

void set_publish_on_every_received_message ( bool  flag)
inline

Sets a flag that forces Publish() at the very beginning of the message handling loop as well as inside the loop.

To achieve "publish whenever a new message has been handled", the user needs to make sure that no subsystems have declared period publish, and flag is true.

const AbstractValue & WaitForMessage ( )

Blocks the caller until a driving Lcm message is received, then returns a const reference of AbstractValue to that message.

The call is assumed to know the type of the actual message and have means to inspect the message.

Here is the call graph for this function:

Here is the caller graph for this function:


The documentation for this class was generated from the following files: