Class Unicycle2DIgnition
Defined in File unicycle_2d_ignition.hpp
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.
-
virtual 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.
-
virtual 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.
Triggers the publication of a new prior transaction at the supplied pose.
Triggers the publication of a new prior transaction at the supplied pose.
Protected Functions
-
virtual 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.
-
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_