Class AmclNodelet

Inheritance Relationships

Base Type

  • public nodelet::Nodelet

Class Documentation

class AmclNodelet : public nodelet::Nodelet

2D AMCL as a ROS (1) nodelet.

Public Functions

AmclNodelet() = default
~AmclNodelet() override = default

Protected Types

using AmclConfigServer = dynamic_reconfigure::Server<beluga_amcl::AmclConfig>

Type alias for a dynamic_reconfigure::Server bound to beluga_amcl configuration.

Protected Functions

void onInit() override

Callback for nodelet initialization.

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, const nav_msgs::OccupancyGrid::ConstPtr&) const -> beluga_ros::Amcl::sensor_model_variant

Get sensor model as per current parametrization.

auto make_particle_filter(const nav_msgs::OccupancyGrid::ConstPtr&) const -> std::unique_ptr<beluga_ros::Amcl>

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

void config_callback(beluga_amcl::AmclConfig &config, uint32_t level)

Callback for dynamic_reconfigure updates.

void map_callback(const nav_msgs::OccupancyGrid::ConstPtr &message)

Callback for occupancy grid map updates.

void map_timer_callback(const ros::TimerEvent &ev)

Callback for repeated map initialization requests.

bool set_map_callback(nav_msgs::SetMap::Request &request, nav_msgs::SetMap::Response &response)

Callback for the map update service.

void handle_map_with_default_initial_pose(const nav_msgs::OccupancyGrid::ConstPtr &map)

Particle filter (re)initialization helper method.

void particle_cloud_timer_callback(const ros::TimerEvent &ev)

Callback for periodic particle cloud updates.

void laser_callback(const sensor_msgs::LaserScan::ConstPtr &laser_scan)

Callback for laser scan updates.

void initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &message)

Callback for pose (re)initialization.

bool global_localization_callback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)

Callback for the global relocalization service.

bool nomotion_update_callback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)

Callback for the no motion update service.

void save_pose_timer_callback(const ros::TimerEvent &ev)

Callback for periodic pose saving.

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.

void update_covariance_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &status)

Callback for estimated pose covariance diagnostics.

Protected Attributes

mutable std::mutex mutex_

Mutex for particle filter access.

ros::Timer particle_cloud_timer_

Timer for periodic particle cloud updates.

ros::Publisher particle_cloud_pub_

Particle cloud publisher.

ros::Publisher pose_pub_

Estimated pose publisher.

ros::Timer save_pose_timer_

Timer for pose saving.

ros::Subscriber initial_pose_sub_

Pose (re)initialization subscriber.

ros::Subscriber map_sub_

Occupancy grid map updates subscriber.

ros::Timer map_timer_

Timer for repeated map initialization requests.

ros::ServiceServer set_map_server_

Map update service server.

ros::ServiceClient get_map_client_

Map initialization service client.

ros::ServiceServer global_localization_server_

Global relocalization service server.

ros::ServiceServer nomotion_update_server_

No motion update service server.

bool configured_ = {false}

Flag set on first configuration.

beluga_amcl::AmclConfig config_

Current beluga_amcl configuration.

beluga_amcl::AmclConfig default_config_

Default beluga_amcl configuration.

std::unique_ptr<AmclConfigServer> config_server_

beluga_amcl configuration server.

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.

diagnostic_updater::Updater diagnosic_updater_

Diagnostics updater.

message_filters::Subscriber<sensor_msgs::LaserScan> laser_scan_sub_

Laser scan updates subscriber.

std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::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.

nav_msgs::OccupancyGrid::ConstPtr last_known_map_

Last known occupancy grid map.

bool enable_tf_broadcast_ = {false}

Whether to broadcast transforms or not.

Protected Static Functions

static auto get_execution_policy(std::string_view) -> beluga_ros::Amcl::execution_policy_variant

Get execution policy given its name.