Class Unicycle2DIgnition

Inheritance Relationships

Base Type

  • public fuse_core::AsyncSensorModel

Class Documentation

class Unicycle2DIgnition : public fuse_core::AsyncSensorModel

A fuse_models ignition sensor designed to be used in conjunction with the unicycle 2D motion model.

This class publishes a transaction that contains a prior on each state subvariable used in the unicycle 2D motion model (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, and y_acc). When the sensor is first loaded, it publishes a single transaction with the configured initial state and covariance. Additionally, whenever a pose is received, either on the set_pose service or the topic, this ignition sensor resets the optimizer then publishes a new transaction with a prior at the specified pose. Priors on (x_vel, y_vel, yaw_vel, x_acc, and y_acc) continue to use the values configured on the parameter server.

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

  • ~initial_sigma (vector of doubles) An 8-dimensional vector containing the standard deviations for the initial state values. The covariance matrix is created placing the squared standard deviations along the diagonal of an 8x8 matrix. Variable order is (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc).

  • ~initial_state (vector of doubles) An 8-dimensional vector containing the initial values for the state. Variable order is (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc).

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

  • ~reset_service (string, default: “~/reset”) The name of the reset service to call before sending a transaction

  • ~set_pose_deprecated_service (string, default: “set_pose_deprecated”) The name of the set_pose_deprecated service

  • ~set_pose_service (string, default: “set_pose”) The name of the set_pose service to advertise

  • ~topic (string, default: “set_pose”) The topic name for received PoseWithCovarianceStamped messages

Public Types

using ParameterType = parameters::Unicycle2DIgnitionParams

Public Functions

Unicycle2DIgnition()

Default constructor.

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

~Unicycle2DIgnition() = 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 start() override

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

As a very special case, we are overriding the start() method instead of providing an onStart() implementation. This is because the Unicycle2DIgnition sensor calls reset() on the optimizer, which in turn calls stop() and start(). If we used the AsyncSensorModel implementations of start() and stop(), the system would hang inside of one callback function while waiting for another callback to complete.

void stop() override

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

As a very special case, we are overriding the stop() method instead of providing an onStop() implementation. This is because the Unicycle2DIgnition sensor calls reset() on the optimizer, which in turn calls stop() and start(). If we used the AsyncSensorModel implementations of start() and stop(), the system would hang inside of one callback function while waiting for another callback to complete.

void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped &msg)

Triggers the publication of a new prior transaction at the supplied pose.

bool setPoseServiceCallback(rclcpp::Service<fuse_msgs::srv::SetPose>::SharedPtr service, std::shared_ptr<rmw_request_id_t>, const fuse_msgs::srv::SetPose::Request::SharedPtr req)

Triggers the publication of a new prior transaction at the supplied pose.

bool setPoseDeprecatedServiceCallback(rclcpp::Service<fuse_msgs::srv::SetPoseDeprecated>::SharedPtr service, std::shared_ptr<rmw_request_id_t> request_id, const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req)

Triggers the publication of a new prior transaction at the supplied pose.

Protected Functions

void onInit() override

Perform any required initialization for the kinematic ignition sensor.

void process(const geometry_msgs::msg::PoseWithCovarianceStamped &pose, std::function<void()> post_process = nullptr)

Process a received pose from one of the ROS comm channels.

This method validates the input pose, resets the optimizer, then constructs and sends the initial state constraints (by calling sendPrior()).

Parameters:

pose[in] - The pose and covariance to use for the prior constraints on (x, y, yaw)

void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped &pose)

Create and send a prior transaction based on the supplied pose.

The unicycle 2d state members not included in the pose message (x_vel, y_vel, yaw_vel, x_acc, y_acc) will use the initial state values and standard deviations configured on the parameter server.

Parameters:

pose[in] - The pose and covariance to use for the prior constraints on (x, y, yaw)

Protected Attributes

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

Shadows AsyncSensorModel interfaces_.

std::atomic_bool started_

Flag indicating the sensor has been started.

bool initial_transaction_sent_

Flag indicating an initial transaction has been sent already.

fuse_core::UUID device_id_

The UUID of this device.

rclcpp::Clock::SharedPtr clock_

The sensor model’s clock, for timestamping.

rclcpp::Logger logger_

The sensor model’s logger.

ParameterType params_

Object containing all of the configuration parameters.

Service client used to call the “reset” service on the optimizer

rclcpp::Client<std_srvs::srv::Empty>::SharedPtr reset_client_
rclcpp::Service<fuse_msgs::srv::SetPose>::SharedPtr set_pose_service_
rclcpp::Service<fuse_msgs::srv::SetPoseDeprecated>::SharedPtr set_pose_deprecated_service_
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_