Class Unicycle2D

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

  • public fuse_core::AsyncMotionModel

Class Documentation

class Unicycle2D : public fuse_core::AsyncMotionModel

A fuse_models 2D kinematic model that generates kinematic constraints between provided time stamps, and adds those constraints to the fuse graph.

This class uses a unicycle kinematic model for the robot. It is equivalent to the motion model that one would have if setting all 3D variable values to 0 in the robot_localization state estimation nodes.

Parameters:

  • ~device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to publish

  • ~device_name (string) Used to generate the device/robot ID if the device_id is not provided

  • ~buffer_length (double) The length of the graph state buffer and state history, in seconds

  • ~process_noise_diagonal (vector of doubles) An 8-dimensional vector containing the diagonal values for the process noise covariance matrix. Variable order is (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc).

Public Functions

Unicycle2D()

Default constructor.

All plugins are required to have a constructor that accepts no arguments

~Unicycle2D() = default

Destructor.

void initialize(fuse_core::node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces, const std::string &name) override

Shadowing extension to the AsyncMotionModel::initialize call.

void print(std::ostream &stream = std::cout) const

Protected Types

using StateHistory = std::map<rclcpp::Time, StateHistoryElement>

Protected Functions

bool applyCallback(fuse_core::Transaction &transaction) override

Augment a transaction structure such that the provided timestamps are connected by motion model constraints.

Parameters:

  • stamps – [in] The set of timestamps that should be connected by motion model constraints

  • transaction – [out] The transaction object that should be augmented with motion model constraints

Returns:

True if the motion models were generated successfully, false otherwise

void generateMotionModel(const rclcpp::Time &beginning_stamp, const rclcpp::Time &ending_stamp, std::vector<fuse_core::Constraint::SharedPtr> &constraints, std::vector<fuse_core::Variable::SharedPtr> &variables)

Generate a single motion model segment between the specified timestamps.

This function is used by the timestamp manager to generate just the new motion model segments required to fulfill a query.

Parameters:

  • beginning_stamp – [in] The beginning timestamp of the motion model constraints to be generated. beginning_stamp is guaranteed to be less than ending_stamp.

  • ending_stamp – [in] The ending timestamp of the motion model constraints to be generated. ending_stamp is guaranteed to be greater than beginning_stamp.

  • constraints – [out] One or more motion model constraints between the requested timestamps.

  • variables – [out] One or more variables at both the beginning_stamp and ending_stamp. The variables should include initial values for the optimizer.

void onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) override

Callback fired in the local callback queue thread(s) whenever a new Graph is received from the optimizer.

Parameters:

graph – [in] A read-only pointer to the graph object, allowing queries to be performed whenever needed.

void onInit() override

Perform any required initialization for the kinematic model.

void onStart() override

Reset the internal state history before starting.

Protected Attributes

fuse_core::node_interfaces::NodeInterfaces<fuse_core::node_interfaces::Base, fuse_core::node_interfaces::Clock, fuse_core::node_interfaces::Logging, fuse_core::node_interfaces::Parameters, fuse_core::node_interfaces::Topics, fuse_core::node_interfaces::Waitables> interfaces_

Shadows AsyncSensorModel interfaces_.

rclcpp::Clock::SharedPtr clock_

The sensor model’s clock, for timestamping and logging.

rclcpp::Logger logger_

The sensor model’s logger.

rclcpp::Duration buffer_length_

The length of the state history.

fuse_core::UUID device_id_

The UUID of the device to be published.

fuse_core::TimestampManager timestamp_manager_

Tracks timestamps and previously created motion model segments

fuse_core::Matrix8d process_noise_covariance_

Process noise covariance matrix.

bool scale_process_noise_ = {false}

Whether to scale the process noise covariance pose by the norm of the current state twist

double velocity_norm_min_ = {1e-3}

The minimum velocity/twist norm allowed when scaling the process noise covariance

bool disable_checks_ = {false}

Whether to disable the validation checks for the current and predicted state, including the process noise covariance after it is scaled and multiplied by dt

StateHistory state_history_

History of optimized graph pose estimates.

Protected Static Functions

static void updateStateHistoryEstimates(const fuse_core::Graph &graph, StateHistory &state_history, const rclcpp::Duration &buffer_length)

Update all of the estimated states in the state history container using the optimized values from the graph.

Parameters:

  • graph – [in] The graph object containing updated variable values

  • state_history – [in] The state history object to be updated

  • buffer_length – [in] States older than this in the history will be pruned

static void validateMotionModel(const StateHistoryElement &state1, const StateHistoryElement &state2, const fuse_core::Matrix8d &process_noise_covariance)

Validate the motion model state #1, state #2 and process noise covariance.

This validates the motion model states and process noise covariance are valid. It throws an exception if any validation check fails.

Parameters:

  • state1 – [in] The first/oldest state

  • state2 – [in] The second/newest state

  • process_noise_covariance – [in] The process noise covariance, after it is scaled and multiplied by dt

struct StateHistoryElement

Structure used to maintain a history of “good” pose estimates.

Public Functions

void print(std::ostream &stream = std::cout) const
void validate() const

Validate the state components: pose, linear velocity, yaw velocity and linear acceleration.

This validates the state components are finite. It throws an exception if any validation check fails.

Public Members

fuse_core::UUID position_uuid

The uuid of the associated position variable.

fuse_core::UUID yaw_uuid

The uuid of the associated orientation variable.

fuse_core::UUID vel_linear_uuid

The uuid of the associated linear velocity variable.

fuse_core::UUID vel_yaw_uuid

The uuid of the associated angular velocity variable.

fuse_core::UUID acc_linear_uuid

The uuid of the associated linear acceleration variable

tf2_2d::Transform pose

Map-frame pose.

tf2_2d::Vector2 velocity_linear

Body-frame linear velocity.

double velocity_yaw = {0.0}

Body-frame yaw velocity.

tf2_2d::Vector2 acceleration_linear

Body-frame linear acceleration.