Class NdtAmclNode3D

Defined in File ndt_amcl_node_3d.hpp

Inheritance Relationships

Base Type

public beluga_amcl::BaseAMCLNode (Class BaseAMCLNode)

Class Documentation

class NdtAmclNode3D : public beluga_amcl::BaseAMCLNode 

3D NDT AMCL as a ROS 2 composable lifecycle node.

Public Functions

explicit NdtAmclNode3D(const rclcpp::NodeOptions &options = rclcpp::NodeOptions{}): Constructor.

Protected Functions

virtual void do_activate(const rclcpp_lifecycle::State&) override: Callback for lifecycle transitions from the INACTIVE state to the ACTIVE state.

virtual void do_configure(const rclcpp_lifecycle::State&) override: Callback for lifecycle transitions from the UNCONFIGURED state to the INACTIVE state.

virtual void do_deactivate(const rclcpp_lifecycle::State&) override: Callback for lifecycle transitions from the ACTIVE state to the INACTIVE state.

virtual 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::SE3d, Sophus::Matrix6d>>: Get initial pose estimate from parameters if set.

auto get_motion_model() const -> MotionModelVariant : Get motion model as per current parametrization.

beluga::NDTSensorModel<NDTMapRepresentation> get_sensor_model() const: Get sensor model as per current parametrization.

auto make_particle_filter() const -> std::unique_ptr<NdtAmclVariant>: Instantiate particle filter given an initial occupancy grid map and the current parametrization.

virtual void do_periodic_timer_callback() override: Callback for periodic particle cloud updates.

void laser_callback(sensor_msgs::msg::PointCloud2::ConstSharedPtr): Callback for laser scan updates.

virtual void do_initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr message) override: Callback for pose (re) initialization.

bool initialize_from_estimate(const std::pair<Sophus::SE3d, Sophus::Matrix6d> &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.

Protected Attributes

std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2>> laser_scan_sub_: Laser scan updates subscription.

std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>> laser_scan_filter_: Transform synchronization filter for laser scan updates.

::message_filters::Connection laser_scan_connection_: Connection for laser scan updates filter and callback.

beluga_amcl::NDTMapRepresentation map_: NDT map representation.

std::unique_ptr<NdtAmclVariant> particle_filter_: Particle filter instance.

std::optional<std::pair<Sophus::SE3d, Sophus::Matrix6d>> last_known_estimate_: Last known pose estimate, if any.

std::optional<Sophus::SE3d> 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.

rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>::SharedPtr map_visualization_pub_: Map visualization publisher.