AmclNode Class Reference#
Beluga AMCL
|
beluga_amcl::AmclNode Class Reference
2D AMCL as a ROS 2 composable lifecycle node. More...
#include <amcl_node.hpp>
Inheritance diagram for beluga_amcl::AmclNode:
Collaboration diagram for beluga_amcl::AmclNode:
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::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. | |
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()
|
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
-
estimate A pair representing the estimated pose and covariance for initialization.
- Returns
- True if the initialization is successful, false otherwise.
◆ 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
Generated by 1.9.1