beluga_amcl::NdtAmclNode3D Class Reference

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

#include <ndt_amcl_node_3d.hpp>

Inheritance diagram for beluga_amcl::NdtAmclNode3D:

Collaboration diagram for beluga_amcl::NdtAmclNode3D:

Public Member Functions
	NdtAmclNode3D (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::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.
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.
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. 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
std::unique_ptr< message_filters::Subscriber< sensor_msgs::msg::PointCloud2, rclcpp_lifecycle::LifecycleNode > >	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.
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.
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

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

Member Function Documentation

initialize_from_estimate()

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

---

The documentation for this class was generated from the following file:

• /home/runner/work/beluga/beluga/beluga_amcl/include/beluga_amcl/ndt_amcl_node_3d.hpp