Class AsyncPublisher
Defined in File async_publisher.hpp
Inheritance Relationships
Base Type
public fuse_core::Publisher
(Class Publisher)
Class Documentation
-
class AsyncPublisher : public fuse_core::Publisher
A publisher base class that provides an internal callback queue attached to a ROS 2 Node serviced by a local executor.
This allows publishers derived from this base class to operate similarly to a stand alone node — any subscription or service callbacks will be executed within an independent thread. The notable differences include:
a default constructor is required (because this is a plugin)
subscriptions, services, parameter server interactions, etc. should be placed in the onInit() call instead of in the constructor
a node has already been created and attached to a local callback queue
a special callback, notifyCallback(), will be fired every time the optimizer completes an optimization cycle
Public Functions
-
virtual ~AsyncPublisher()
Destructor.
-
virtual void initialize(node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces, const std::string &name) override
Initialize the AsyncPublisher object.
This will store the provided name and graph object, and create an internal executor for this instance that will use an internal callback queue serviced by a local thread. The AsyncPublisher::onInit() method will be called from here, once the callback queue is properly configured.
- Parameters:
interfaces – [in] The node interfaces to be used with the publisher
name – [in] A unique name to give this plugin instance
- Throws:
runtime_error – if already initialized
-
inline virtual const std::string &name() const override
Get the unique name of this publisher.
Notify the publisher that an optimization cycle is complete, and about changes to the Graph.
This function is called by the optimizer (and in the optimizer’s thread) after every optimization cycle is complete. To minimize the time spent by the optimizer, this function merely injects a call to AsyncPublisher::notifyCallback() into the internal callback queue. This has the added benefit of simplifying threading, as all callbacks made by this publisher will be executed from the same thread (or group of threads if a
thread_count
1 is used in the constructor).
- Parameters:
transaction – [in] A Transaction object, describing the set of variables that have been added and/or removed
graph – [in] A read-only pointer to the graph object, allowing queries to be performed whenever needed
-
virtual void start() override
Function to be executed whenever the optimizer is ready to receive transactions.
This method will be called by the optimizer, in the optimizer’s thread, once the optimizer has been initialized and is ready to receive transactions. It may also be called as part of a stop-start cycle when the optimizer has been requested to reset itself. This allows the publisher to reset any internal state before the optimizer begins processing after a reset. No calls to notify() will happen before the optimizer calls start().
This implementation inserts a call to onStart() into this publisher’s callback queue. This method then blocks until the call to onStart() has completed. This is meant to simplify thread synchronization. If this publisher uses a single-threaded executor, then all callbacks will fire sequentially and no semaphores are needed. If this publisher uses a multithreaded executor, then normal multithreading rules apply and data accessed in more than one place should be guarded.
-
virtual void stop() override
Function to be executed whenever the optimizer is no longer ready to receive transactions.
This method will be called by the optimizer, in the optimizer’s thread, before the optimizer shutdowns. It may also be called as part of a stop-start cycle when the optimizer has been requested to reset itself. This allows the publisher to reset any internal state before the optimizer begins processing after a reset. No calls to notify() will happen until start() has been called again.
This implementation inserts a call to onStop() into this publisher’s callback queue. This method then blocks until the call to onStop() has completed. This is meant to simplify thread synchronization. If this publisher uses a single-threaded executor, then all callbacks will fire sequentially and no semaphores are needed. If this publisher uses a multithreaded spinner, then normal multithreading rules apply and data accessed in more than one place should be guarded.
Protected Functions
-
explicit AsyncPublisher(size_t thread_count = 1)
Constructor.
Constructs a new publisher with a local node, a local callback queue, and internal executor.
- Parameters:
thread_count – [in] The number of threads used to service the local callback queue
-
inline virtual void onInit()
Perform any required initialization for the publisher.
The node_ and member variables will be completely initialized before this call occurs. Spinning of the callback queue will not begin until after the call to AsyncPublisher::onInit() completes.
Derived classes should override this method to implement any additional initialization steps needed (access the parameter server, advertise, subscribe, etc.).
Callback method executed in response to the optimizer completing an optimization cycle. All variables will now have updated values.
This method is executed using the internal callback queue and local thread(s). Derived classes should override this method to implement the desired publisher behavior.
- Parameters:
transaction – [in] A Transaction object, describing the set of variables that have been added and/or removed
graph – [in] A read-only pointer to the graph object, allowing queries to be performed whenever needed
-
inline virtual void onStart()
Perform any required operations to prepare for servicing calls to notify()
This function will be called once after initialize() but before any calls to notify(). It may also be called at any time after a call to stop().
Protected Attributes
-
std::shared_ptr<fuse_core::CallbackAdapter> callback_queue_
The callback queue used for fuse internal callbacks.
-
std::string name_
The unique name for this publisher instance.
-
rclcpp::CallbackGroup::SharedPtr cb_group_
Internal re-entrant callback group.
-
node_interfaces::NodeInterfaces<node_interfaces::Base, node_interfaces::Waitables> interfaces_
The node interfaces.
-
rclcpp::Executor::SharedPtr executor_
A single/multi-threaded executor assigned to the local callback queue.
-
size_t executor_thread_count_ = {1}
-
std::thread spinner_
Internal thread for spinning the executor.
-
std::atomic<bool> initialized_ = false
True if instance has been fully initialized.