Class Unicycle2D
Defined in File unicycle_2d.hpp
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
-
virtual 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
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_stampis guaranteed to be less thanending_stamp.ending_stamp – [in] The ending timestamp of the motion model constraints to be generated.
ending_stampis guaranteed to be greater thanbeginning_stamp.constraints – [out] One or more motion model constraints between the requested timestamps.
variables – [out] One or more variables at both the
beginning_stampandending_stamp. The variables should include initial values for the optimizer.
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.
-
virtual void onInit() override
Perform any required initialization for the kinematic model.
-
virtual 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::TimestampManager timestamp_manager_
Tracks timestamps and previously created motion model segments
-
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.
-
void print(std::ostream &stream = std::cout) const