Class AmclNode

Inheritance Relationships

Base Type

Class Documentation

class AmclNode : public beluga_amcl::BaseAMCLNode

2D AMCL as a ROS 2 composable lifecycle node.

Public Functions

explicit AmclNode(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

Constructor.

~AmclNode() override

Protected Functions

void do_activate(const rclcpp_lifecycle::State&) override

Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state.

void do_deactivate(const rclcpp_lifecycle::State&) override

Callback for lifecycle transitions from the ACTIVE state to the INACTIVE state.

void do_cleanup(const rclcpp_lifecycle::State&) override

Callback for lifecycle transitions from the INACTIVE state to the UNCONFIGURED state.

auto get_initial_estimate() const -> std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>>

Get initial pose estimate from parameters if set.

auto get_motion_model(std::string_view) const -> beluga_ros::Amcl::motion_model_variant

Get motion model as per current parametrization.

auto get_sensor_model(std::string_view, nav_msgs::msg::OccupancyGrid::SharedPtr) const -> beluga_ros::Amcl::sensor_model_variant

Get sensor model as per current parametrization.

auto make_particle_filter(nav_msgs::msg::OccupancyGrid::SharedPtr) const -> std::unique_ptr<beluga_ros::Amcl>

Instantiate particle filter given an initial occupancy grid map and the current parametrization.

void map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr)

Callback for occupancy grid map updates.

virtual void do_periodic_timer_callback() override

Callback for periodic particle cloud updates.

template<typename TransformT>
std::optional<TransformT> lookup_transform(const std::string &target_frame_id, const std::string &source_frame_id, const tf2::TimePoint &stamp)

Try to look up a tf transform immediately.

std::optional<beluga_ros::LaserScan> wrap_sensor_data(const sensor_msgs::msg::LaserScan::ConstSharedPtr &sensor_msg)

Try to wrap a laser scan message.

std::optional<beluga_ros::SparsePointCloud3f> wrap_sensor_data(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &sensor_msg)

Try to wrap a pointcloud message.

template<typename MessageT>
void sensor_callback(const std::shared_ptr<const MessageT> &sensor_msg)

Callback for sensor updates.

void do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) override

Callback for pose (re)initialization.

void global_localization_callback(std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response)

Callback for the global relocalization service.

void nomotion_update_callback(std::shared_ptr<rmw_request_id_t> request_header, std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res)

Callback for the no motion update service.

bool initialize_from_estimate(const std::pair<Sophus::SE2d, Eigen::Matrix3d> &estimate)

Initialize particles from an estimated pose and covariance.

If an exception occurs during the initialization, an error message is logged, and the initialization process is also aborted, returning false. If the initialization is successful, the TF broadcast is enabled.

Parameters:

estimate – A pair representing the estimated pose and covariance for initialization.

Returns:

True if the initialization is successful, false otherwise.

bool initialize_from_map()

Initialize particles from map.

If an exception occurs during the initialization, an error message is logged, and the initialization process is also aborted, returning false. If the initialization is successful, the TF broadcast is enabled.

Returns:

True if the initialization is successful, false otherwise.

Protected Attributes

rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_

Occupancy grid map updates subscription.

std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> laser_scan_sub_

Laser scan updates subscription.

std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>> point_cloud_sub_

Point cloud updates subscription.

rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr likelihood_field_pub_

Likelihood field publisher.

rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_localization_server_

Global relocalization service server.

rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_server_

No motion update service server.

std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_

Transform synchronization filter for laser scan updates.

::message_filters::Connection laser_scan_connection_

Connection for laser scan updates filter and callback.

std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>> point_cloud_filter_

Transform synchronization filter for laser scan updates.

::message_filters::Connection point_cloud_connection_

Connection for point cloud updates filter and callback.

std::unique_ptr<beluga_ros::Amcl> particle_filter_

Particle filter instance.

std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> last_known_estimate_

Last known pose estimate, if any.

std::optional<Sophus::SE2d> last_known_odom_transform_in_map_

Last known map to odom correction estimate, if any.

bool enable_tf_broadcast_ = {false}

Whether to broadcast transforms or not.