AmclNode Class Reference

AmclNode Class Reference#

Beluga AMCL: beluga_amcl::AmclNode Class Reference
Beluga AMCL

2D AMCL as a ROS 2 composable lifecycle node. More...

#include <amcl_node.hpp>

Inheritance diagram for beluga_amcl::AmclNode:
[legend]
Collaboration diagram for beluga_amcl::AmclNode:
[legend]

Public Member Functions

 AmclNode (const rclcpp::NodeOptions &options=rclcpp::NodeOptions())
 Constructor.
 
- Public Member Functions inherited from beluga_amcl::BaseAMCLNode
 BaseAMCLNode (const std::string &node_name="amcl", const std::string &node_namespace="", const rclcpp::NodeOptions &node_options=rclcpp::NodeOptions{})
 Constructor, following LifecycleNode signature.
 
 ~BaseAMCLNode () override
 Destructor for the base AMCL node.
 

Protected Member 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.
 
void do_periodic_timer_callback () override
 Callback for periodic particle cloud updates.
 
void laser_callback (sensor_msgs::msg::LaserScan::ConstSharedPtr)
 Callback for laser scan 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. More...
 
bool initialize_from_map ()
 Initialize particles from map. More...
 
- Protected Member Functions inherited from beluga_amcl::BaseAMCLNode
CallbackReturn on_configure (const rclcpp_lifecycle::State &) override
 Callback for lifecycle transitions from the UNCONFIGURED state to the INACTIVE state.
 
CallbackReturn on_activate (const rclcpp_lifecycle::State &) override
 Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state.
 
CallbackReturn on_deactivate (const rclcpp_lifecycle::State &) override
 Callback for lifecycle transitions from the ACTIVE state to the INACTIVE state.
 
CallbackReturn on_cleanup (const rclcpp_lifecycle::State &) override
 Callback for lifecycle transitions from the INACTIVE state to the UNCONFIGURED state.
 
CallbackReturn on_shutdown (const rclcpp_lifecycle::State &) override
 Callback for lifecycle transitions from most states to the FINALIZED state.
 
auto get_execution_policy () const -> ExecutionPolicyVariant
 Get execution policy from parameters.
 
void periodic_timer_callback ()
 Callback for the periodic particle updates.
 
void autostart_callback ()
 Callback for the autostart timer.
 
virtual void do_autostart_callback ()
 User provided extra steps for the autostart process.
 
virtual void do_configure ([[maybe_unused]] const rclcpp_lifecycle::State &state)
 Extra steps for the on_configure callback. Defaults to no-op.
 
virtual void do_deactivate ([[maybe_unused]] const rclcpp_lifecycle::State &state)
 Extra steps for the on_deactivate callback. Defaults to no-op.
 
virtual void do_shutdown ([[maybe_unused]] const rclcpp_lifecycle::State &state)
 Extra steps for the on_shutdown callback. Defaults to no-op.
 
virtual void do_cleanup ([[maybe_unused]] const rclcpp_lifecycle::State &state)
 Extra steps for the on_cleanup callback. Defaults to no-op.
 
virtual void do_activate ([[maybe_unused]] const rclcpp_lifecycle::State &state)
 Extra steps for the on_activate callback. Defaults to no-op.
 
virtual void do_initial_pose_callback ([[maybe_unused]] geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message)
 Extra steps for (re)initialization messages.
 
void initial_pose_callback (geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message)
 Callback for (re)initialization messages.
 

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, rclcpp_lifecycle::LifecycleNode > > laser_scan_sub_
 Laser scan updates subscription.
 
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< beluga_ros::Amclparticle_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.
 
- Protected Attributes inherited from beluga_amcl::BaseAMCLNode
rclcpp_lifecycle::LifecyclePublisher< geometry_msgs::msg::PoseArray >::SharedPtr particle_cloud_pub_
 Particle cloud publisher.
 
rclcpp_lifecycle::LifecyclePublisher< visualization_msgs::msg::MarkerArray >::SharedPtr particle_markers_pub_
 Particle markers publisher.
 
rclcpp_lifecycle::LifecyclePublisher< geometry_msgs::msg::PoseWithCovarianceStamped >::SharedPtr pose_pub_
 Estimated pose publisher.
 
std::unique_ptr< bond::Bond > bond_
 Node bond with the lifecycle manager.
 
rclcpp::TimerBase::SharedPtr timer_
 Timer for periodic particle cloud updates.
 
std::unique_ptr< tf2_ros::Buffer > tf_buffer_
 Transforms buffer.
 
std::unique_ptr< tf2_ros::TransformBroadcaster > tf_broadcaster_
 Transforms broadcaster.
 
std::unique_ptr< tf2_ros::TransformListener > tf_listener_
 Transforms listener.
 
rclcpp::TimerBase::SharedPtr autostart_timer_
 Timer for node to configure and activate itself.
 
rclcpp::CallbackGroup::SharedPtr common_callback_group_
 Common mutually exclusive callback group.
 
rclcpp::SubscriptionOptions common_subscription_options_
 Common subscription options.
 
rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped >::SharedPtr initial_pose_sub_
 Pose (re)initialization subscription.
 

Detailed Description

2D AMCL as a ROS 2 composable lifecycle node.

Member Function Documentation

◆ initialize_from_estimate()

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

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
estimateA pair representing the estimated pose and covariance for initialization.
Returns
True if the initialization is successful, false otherwise.

◆ initialize_from_map()

bool beluga_amcl::AmclNode::initialize_from_map ( )
protected

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.

The documentation for this class was generated from the following file:
  • /home/runner/work/beluga/beluga/beluga_amcl/include/beluga_amcl/amcl_node.hpp