Class FixedLagSmoother
Defined in File fixed_lag_smoother.hpp
Nested Relationships
Nested Types
Inheritance Relationships
Base Type
public fuse_optimizers::Optimizer(Class Optimizer)
Class Documentation
-
class FixedLagSmoother : public fuse_optimizers::Optimizer
A fixed-lag smoother implementation that marginalizes out variables that are older than a defined lag time.
This implementation assumes that all added variable types are either derived from the fuse_variables::Stamped class, or are directly connected to at least one fuse_variables::Stamped variable via a constraint. The current time of the fixed-lag smoother is determined by the newest stamp of all added fuse_variables::Stamped variables.
During optimization: (1) new variables and constraints are added to the graph (2) the augmented graph is optimized and the variable values are updated (3) all motion models, sensors, and publishers are notified of the updated graph (4) all variables older than “current time - lag duration” are marginalized out.
Optimization is performed at a fixed frequency, controlled by the
optimization_frequencyparameter. Received sensor transactions are queued while the optimization is processing, then applied to the graph at the start of the next optimization cycle. If the previous optimization is not yet complete when the optimization period elapses, then a warning will be logged but a new optimization will not be started. The previous optimization will run to completion, and the next optimization will not begin until the next scheduled optimization period.Parameters:
lag_duration (float, default: 5.0) The duration of the smoothing window in seconds
motion_models (struct array) The set of motion model plugins to load
- name: string (A unique name for this motion model) type: string (The plugin loader class string for the desired motion model type) - ...
optimization_frequency (float, default: 10.0) The target frequency for optimization cycles. If an optimization takes longer than expected, an optimization cycle may be skipped.
publishers (struct array) The set of publisher plugins to load
- name: string (A unique name for this publisher) type: string (The plugin loader class string for the desired publisher type) - ...
sensor_models (struct array) The set of sensor model plugins to load
- name: string (A unique name for this sensor model) type: string (The plugin loader class string for the desired sensor model type) motion_models: [name1, name2, ...] (An optional list of motion model names that should be applied) - ...
transaction_timeout (float, default: 0.10) The maximum time to wait for motion models to be generated for a received transactions. Transactions are processes sequentially, so no new transactions will be added to the graph while waiting for motion models to be generated. Once the timeout expires, that transaction will be deleted from the queue.
Public Types
-
using ParameterType = FixedLagSmootherParams
Public Functions
-
FixedLagSmoother(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 ~FixedLagSmoother()
Destructor.
Protected Types
-
using TransactionQueue = std::vector<TransactionQueueElement>
Queue of Transaction objects, sorted by timestamp.
Note: Because the queue size of the fixed-lag smoother is expected to be small, the sorted queue is implemented using a std::vector. The queue size must exceed several hundred entries before a std::set will outperform a sorted vector.
Also, we sort the queue with the smallest stamp last. This allows us to clear the queue using the more efficient pop_back() operation.
Protected Functions
-
void autostart()
Automatically start the smoother if no ignition sensors are specified.
-
void preprocessMarginalization(const fuse_core::Transaction &new_transaction)
Perform any required preprocessing steps before
computeVariablesToMarginalize()is called.All new transactions that will be applied to the graph are provided. This does not include the marginal transaction that is computed later.
This method will be called before the graph has been updated.
- Parameters:
new_transaction – [in] All new, non-marginal-related transactions that will be applied to the graph
-
rclcpp::Time computeLagExpirationTime() const
Compute the oldest timestamp that is part of the configured lag window.
-
std::vector<fuse_core::UUID> computeVariablesToMarginalize(const rclcpp::Time &lag_expiration)
Compute the set of variables that should be marginalized from the graph.
This will be called after
preprocessMarginalization()and after the graph has been updated with the any previous marginal transactions and new transactions.- Parameters:
lag_expiration – [in] The oldest timestamp that should remain in the graph
- Returns:
A container with the set of variables to marginalize out. Order of the variables is not specified.
-
void postprocessMarginalization(const fuse_core::Transaction &marginal_transaction)
Perform any required post-marginalization bookkeeping.
The transaction containing the actual changed to the graph is supplied. This will be called before the transaction is actually applied to the graph.
- Parameters:
marginal_transaction – [in] The actual changes to the graph caused my marginalizing out the requested variables.
-
void optimizationLoop()
Function that optimizes all constraints, designed to be run in a separate thread.
This function waits for an optimization or shutdown signal, then either calls optimize() or exits appropriately.
-
void optimizerTimerCallback()
Callback fired at a fixed frequency to trigger a new optimization cycle.
This callback checks if a current optimization cycle is still running. If not, a new optimization cycle is started. If so, we simply wait for the next timer event to start another optimization cycle.
-
void processQueue(fuse_core::Transaction &transaction, const rclcpp::Time &lag_expiration)
Generate motion model constraints for pending transactions and combine them into a single transaction.
Transactions are processed sequentially based on timestamp. If motion models are successfully generated for a pending transactions, that transaction is merged into the combined transaction and removed from the pending queue. If motion models fail to generate after the configured transaction_timeout_, the transaction will be deleted from the pending queue and a warning will be displayed.
- Parameters:
transaction – [out] The transaction object to be augmented with pending motion model and sensor transactions
lag_expiration – [in] The oldest timestamp that should remain in the graph
Service callback that resets the optimizer to its original state.
-
inline rclcpp::Time getStartTime() const
Thread-safe read-only access to the timestamp of the first transaction.
-
inline void setStartTime(const rclcpp::Time &start_time)
Thread-safe write access to the optimizer start time.
Callback fired every time the SensorModel plugin creates a new transaction.
This callback is responsible for ensuring all associated motion models are applied before any other processing takes place. See Optimizer::applyMotionModels() for a helper function that does just that.
This implementation shares ownership of the transaction object.
- Parameters:
name – [in] The name of the sensor that produced the Transaction
transaction – [in] The populated Transaction object created by the loaded SensorModel plugin
-
virtual void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &status) override
Update and publish diagnotics.
- Parameters:
status – [in] The diagnostic status
Protected Attributes
-
std::thread optimization_thread_
Thread used to run the optimizer as a background process.
-
ParameterType params_
Configuration settings for this fixed-lag smoother.
-
std::atomic<bool> ignited_
Flag indicating the optimizer has received a transaction from an ignition sensor and it is queued but not processed yet
-
std::atomic<bool> optimization_running_
Flag indicating the optimization thread should be running
-
std::atomic<bool> started_
Flag indicating the optimizer has received a transaction from an ignition sensor
-
std::mutex pending_transactions_mutex_
Synchronize modification of the pending_transactions_ container
-
TransactionQueue pending_transactions_
The set of received transactions that have not been added to the optimizer yet. Transactions are added by the main thread, and removed and processed by the optimization thread.
-
std::mutex optimization_mutex_
Mutex held while the graph is begin optimized.
-
rclcpp::Time lag_expiration_
The oldest stamp that is inside the fixed-lag smoother window.
-
fuse_core::Transaction marginal_transaction_
The marginals to add during the next optimization cycle
-
VariableStampIndex timestamp_tracking_
Object that tracks the timestamp associated with each variable
-
ceres::Solver::Summary summary_
Optimization summary, written by optimizationLoop and read by setDiagnostics
-
std::mutex optimization_requested_mutex_
Required condition variable mutex.
-
rclcpp::Time optimization_deadline_
The deadline for the optimization to complete. Triggers a warning if exceeded.
-
bool optimization_request_
Flag to trigger a new optimization.
-
std::condition_variable optimization_requested_
Condition variable used by the optimization thread to wait until a new optimization is requested by the main thread
-
mutable std::mutex start_time_mutex_
Synchronize modification to the start_time_ variable.
-
rclcpp::Time start_time_ = {0, 0, RCL_ROS_TIME}
The timestamp of the first ignition sensor transaction
-
rclcpp::TimerBase::SharedPtr optimize_timer_
Service that resets the optimizer to its initial state.
Trigger an optimization operation at a fixed frequency
-
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_service_server_