Class Odometry2D

Inheritance Relationships

Base Type

  • public fuse_core::AsyncSensorModel

Class Documentation

class Odometry2D : public fuse_core::AsyncSensorModel

An adapter-type sensor that produces pose (relative or absolute) and velocity constraints from sensor data published by another node.

This sensor subscribes to a nav_msgs::msg::Odometry topic and:

  1. Creates relative or absolute pose variables and constraints. If the differential parameter is set to false (the default), the measurement will be treated as an absolute constraint. If it is set to true, consecutive measurements will be used to generate relative pose constraints.

  2. Creates 2D velocity variables and constraints.

This sensor really just separates out the pose and twist components of the message, and processes them just like the Pose2D and Twist2D classes.

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

  • queue_size (int, default: 10) The subscriber queue size for the pose messages

  • topic (string) The topic to which to subscribe for the pose messages

  • differential (bool, default: false) Whether we should fuse measurements absolutely, or to create relative pose constraints using consecutive measurements.

  • pose_target_frame (string) Pose data will be transformed into this frame before it is fused. This frame should be a world-fixed frame, typically ‘odom’ or ‘map’.

  • twist_target_frame (string) Twist/velocity data will be transformed into this frame before it is fused. This frame should be a body-relative frame, typically ‘base_link’.

Subscribes:

  • topic (nav_msgs::msg::Odometry) Odometry information at a given timestep

Public Types

using ParameterType = parameters::Odometry2DParams

Public Functions

Odometry2D()

Default constructor.

virtual ~Odometry2D() = default

Destructor.

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

Shadowing extension to the AsyncSensorModel::initialize call.

void process(const nav_msgs::msg::Odometry &msg)

Callback for pose messages.

Parameters:

msg – [in] - The pose message to process

Protected Types

using OdometryThrottledCallback = fuse_core::ThrottledMessageCallback<nav_msgs::msg::Odometry>

Protected Functions

void onInit() override

Perform any required initialization for the sensor model.

This could include things like reading from the parameter server or subscribing to topics. The class’s node handles will be properly initialized before SensorModel::onInit() is called. Spinning of the callback queue will not begin until after the call to SensorModel::onInit() completes.

void onStart() override

Subscribe to the input topic to start sending transactions to the optimizer.

void onStop() override

Unsubscribe from the input topic to stop sending transactions to the optimizer.

void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped &pose, const geometry_msgs::msg::TwistWithCovarianceStamped &twist, const bool validate, fuse_core::Transaction &transaction)

Process a pose message in differential mode.

Parameters:

  • pose – [in] - The pose message to process in differential mode

  • twist – [in] - The twist message used in case the twist covariance is used in differential mode

  • validate – [in] - Whether to validate the pose and twist coavriance or not

  • transaction – [out] - The generated variables and constraints are added to this transaction

Protected Attributes

fuse_core::UUID device_id_

The UUID of this device.

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.

ParameterType params_
geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_
std::unique_ptr<tf2_ros::Buffer> tf_buffer_
std::unique_ptr<tf2_ros::TransformListener> tf_listener_
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_
OdometryThrottledCallback throttled_callback_