Class Optimizer

Nested Relationships

Nested Types

Inheritance Relationships

Derived Types

Class Documentation

class Optimizer

A base class that can be used to build fuse optimizer nodes.

An optimizer implements the basic fuse information flow contract:

  • Sensors push information into the optimizer using the transaction callback

  • The optimizer requests motion models be created between each sensor timestamp

  • The optimizer computes the optimal variable values

  • The optimizer provides access to the optimal variable values to the publishers

Optimizer implementations are not required to use this base class; it is simply provided as a convenience class that implements the mechanics of the information flow contract. Derived classes can then concentrate on the details of when and what to optimize.

This base class provides functions for:

  • Loading the set of motion model plugins as configured on the parameter server

  • Loading the set of publisher plugins as configured on the parameter server

  • Loading the set of sensor plugins as configured on the parameter server

  • Generating the correct motion model constraints for each received sensor transaction

  • Sending updated variable information to the sensors, motion models, and publishers

Parameter Server format: 

motion_models:
 - name: string
   type: string
 - ...
sensor_models:
 - name: string
   type: string
   motion_models: [name1, name2, ...]
 - ...
publishers:
 - name: string
   type: string
 - ...

Subclassed by fuse_optimizers::BatchOptimizer, fuse_optimizers::FixedLagSmoother

Public Functions

Optimizer(fuse_core::node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces, fuse_core::Graph::UniquePtr graph = nullptr)

Constructor.

Parameters:

  • interfaces – [in] The node interfaces for the node driving the optimizer

  • graph – [in] The graph used with the optimizer

virtual ~Optimizer()

Destructor.

Protected Types

using MotionModelUniquePtr = class_loader::ClassLoader::UniquePtr<fuse_core::MotionModel>
using MotionModels = std::unordered_map<std::string, MotionModelUniquePtr>
using PublisherUniquePtr = class_loader::ClassLoader::UniquePtr<fuse_core::Publisher>
using Publishers = std::unordered_map<std::string, PublisherUniquePtr>
using SensorModelUniquePtr = class_loader::ClassLoader::UniquePtr<fuse_core::SensorModel>
using SensorModels = std::unordered_map<std::string, SensorModelInfo>
using MotionModelGroup = std::vector<std::string>

sensor -> motion models group

using AssociatedMotionModels = std::unordered_map<std::string, MotionModelGroup>

Protected Functions

virtual void transactionCallback(const std::string &sensor_name, fuse_core::Transaction::SharedPtr transaction) = 0

Callback fired every time a SensorModel plugin creates a new transaction.

Parameters:

  • sensor_name – [in] The name of the sensor that produced the Transaction

  • stamps – [in] Any timestamps associated with the added variables. These are sent to the motion models to generate connected constraints.

  • transaction – [in] The populated Transaction object created by the loaded SensorModel plugin

void loadMotionModels()

Configure the motion model plugins specified on the parameter server.

Will throw if the parameter server configuration is invalid.

void loadPublishers()

Configure the publisher plugins specified on the parameter server.

Will throw if the parameter server configuration is invalid.

void loadSensorModels()

Configure the sensor model plugins specified on the parameter server.

Will throw if the parameter server configuration is invalid.

bool applyMotionModels(const std::string &sensor_name, fuse_core::Transaction &transaction) const

Given a transaction and some timestamps, augment the transaction with constraints from all associated motion models.

If no timestamps are provided, or no motion models are associated with this sensor, the transaction is left unmodified. If an associated motion model is unavailable, this will throw an exception.

Parameters:

  • name – [in] The name of the sensor that produced the Transaction

  • timestamps – [in] Any timestamps associated with the added variables. These are sent to the motion models to generate connected constraints.

  • transaction – [out] The Transaction object will be augmented with constraints and variables from the motion models

Returns:

Flag indicating if all motion model constraints were successfully generated

void notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph)

Send the sensors, motion models, and publishers updated graph information.

Parameters:

  • transaction – [in] A read-only pointer to a transaction containing all recent additions and removals

  • graph – [in] A read-only pointer to the graph object

void injectCallback(const std::string &sensor_name, fuse_core::Transaction::SharedPtr transaction)

Inject a transaction callback function into the global callback queue.

Parameters:

  • sensor_name – [in] The name of the sensor that produced the Transaction

  • transaction – [in] The populated Transaction object created by the loaded SensorModel plugin

void clearCallbacks()

Clear all of the callbacks inserted into the callback queue by the injectCallback() method.

void startPlugins()

Start all configured plugins (motion models, publishers, and sensor models)

void stopPlugins()

Stop all configured plugins (motion models, publishers, and sensor models)

virtual void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &status)

Update and publish diagnotics.

Parameters:

status – [in] The diagnostic status

Protected Attributes

fuse_core::node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces_
rclcpp::Clock::SharedPtr clock_
rclcpp::Logger logger_
AssociatedMotionModels associated_motion_models_

Tracks what motion models should be used for each sensor

fuse_core::Graph::UniquePtr graph_

The graph object that holds all variables and constraints

pluginlib::ClassLoader<fuse_core::MotionModel> motion_model_loader_

Pluginlib class loader for MotionModels

MotionModels motion_models_

The set of motion models, addressable by name.

pluginlib::ClassLoader<fuse_core::Publisher> publisher_loader_

Pluginlib class loader for Publishers

Publishers publishers_

The set of publishers to execute after every graph optimization.

pluginlib::ClassLoader<fuse_core::SensorModel> sensor_model_loader_

Pluginlib class loader for SensorModels

SensorModels sensor_models_

The set of sensor models, addressable by name.

diagnostic_updater::Updater diagnostic_updater_

Diagnostic updater.

std::shared_ptr<fuse_core::CallbackAdapter> callback_queue_
struct SensorModelInfo

A struct to hold the sensor model and whether it is an ignition one or not.

Public Functions

inline SensorModelInfo(SensorModelUniquePtr model, const bool ignition)

Constructor.

Parameters:

  • model – [in] The sensor model

  • ignition – [in] Whether this sensor model is an ignition one or not

Public Members

SensorModelUniquePtr model

The sensor model.

bool ignition

Whether this sensor model is an ignition one or not.