Class AmclNode

• Defined in File amcl_node.hpp

Nested Relationships

Nested Types

• Struct AmclNode::amcl_hyp_t

Inheritance Relationships

Base Type

• public nav2_util::LifecycleNode

Class Documentation

class AmclNode : public nav2_util::LifecycleNode

Public Functions

explicit AmclNode(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())

~AmclNode()

Protected Functions

nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override

nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override

nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override

nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override

nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override

rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)

Callback executed when a parameter change is detected.

Parameters:

event – ParameterEvent message

void mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)

void handleMapMessage(const nav_msgs::msg::OccupancyGrid &msg)

void createFreeSpaceVector()

void freeMapDependentMemory()

map_t *convertMap(const nav_msgs::msg::OccupancyGrid &map_msg)

void initTransforms()

void initMessageFilters()

void initPubSub()

void initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)

void laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)

void initServices()

void globalLocalizationCallback(const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response)

void initialPoseReceivedSrv(const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> request, std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response)

void nomotionUpdateCallback(const std::shared_ptr<rmw_request_id_t> request_header, const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response)

void initOdometry()

bool getOdomPose(geometry_msgs::msg::PoseStamped &pose, double &x, double &y, double &yaw, const rclcpp::Time &sensor_timestamp, const std::string &frame_id)

void initParticleFilter()

void initLaserScan()

nav2_amcl::Laser *createLaserObject()

bool checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time last_time)

bool addNewScanner(int &laser_index, const sensor_msgs::msg::LaserScan::ConstSharedPtr &laser_scan, const std::string &laser_scan_frame_id, geometry_msgs::msg::PoseStamped &laser_pose)

bool shouldUpdateFilter(const pf_vector_t pose, pf_vector_t &delta)

bool updateFilter(const int &laser_index, const sensor_msgs::msg::LaserScan::ConstSharedPtr &laser_scan, const pf_vector_t &pose)

void publishParticleCloud(const pf_sample_set_t *set)

bool getMaxWeightHyp(std::vector<amcl_hyp_t> &hyps, amcl_hyp_t &max_weight_hyps, int &max_weight_hyp)

void publishAmclPose(const sensor_msgs::msg::LaserScan::ConstSharedPtr &laser_scan, const std::vector<amcl_hyp_t> &hyps, const int &max_weight_hyp)

void calculateMaptoOdomTransform(const sensor_msgs::msg::LaserScan::ConstSharedPtr &laser_scan, const std::vector<amcl_hyp_t> &hyps, const int &max_weight_hyp)

void sendMapToOdomTransform(const tf2::TimePoint &transform_expiration)

void handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped &msg)

void initParameters()

Protected Attributes

rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_

std::atomic<bool> active_ = {false}

rclcpp::CallbackGroup::SharedPtr callback_group_

rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_

std::unique_ptr<nav2_util::NodeThread> executor_thread_

map_t *map_ = {nullptr}

bool first_map_only_ = {true}

std::atomic<bool> first_map_received_ = {false}

amcl_hyp_t *initial_pose_hyp_

std::recursive_mutex mutex_

rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_

std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_

std::shared_ptr<tf2_ros::TransformListener> tf_listener_

std::shared_ptr<tf2_ros::Buffer> tf_buffer_

bool sent_first_transform_ = {false}

bool latest_tf_valid_ = {false}

tf2::Transform latest_tf_

std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan, rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_

std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_

message_filters::Connection laser_scan_connection_

rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::ConstSharedPtr initial_pose_sub_

rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_

rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::ParticleCloud>::SharedPtr particle_cloud_pub_

nav2_util::ServiceServer<std_srvs::srv::Empty, std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr global_loc_srv_

nav2_util::ServiceServer<nav2_msgs::srv::SetInitialPose, std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr initial_guess_srv_

nav2_util::ServiceServer<std_srvs::srv::Empty, std::shared_ptr<nav2_util::LifecycleNode>>::SharedPtr nomotion_update_srv_

std::atomic<bool> force_update_ = {false}

std::shared_ptr<nav2_amcl::MotionModel> motion_model_

geometry_msgs::msg::PoseStamped latest_odom_pose_

geometry_msgs::msg::PoseWithCovarianceStamped last_published_pose_

double init_pose_[3]

double init_cov_[3]

pluginlib::ClassLoader<nav2_amcl::MotionModel> plugin_loader_{"nav2_amcl", "nav2_amcl::MotionModel"}

std::atomic<bool> first_pose_sent_

pf_t *pf_ = {nullptr}

bool pf_init_

pf_vector_t pf_odom_pose_

int resample_count_ = {0}

int scan_error_count_ = {0}

std::vector<nav2_amcl::Laser*> lasers_

std::vector<bool> lasers_update_

std::map<std::string, int> frame_to_laser_

rclcpp::Time last_laser_received_ts_

rclcpp::Time last_time_printed_msg_

bool init_pose_received_on_inactive = {false}

bool initial_pose_is_known_ = {false}

bool set_initial_pose_ = {false}

bool always_reset_initial_pose_

double initial_pose_x_

double initial_pose_y_

double initial_pose_z_

double initial_pose_yaw_

double alpha1_

double alpha2_

double alpha3_

double alpha4_

double alpha5_

std::string base_frame_id_

double beam_skip_distance_

double beam_skip_error_threshold_

double beam_skip_threshold_

bool do_beamskip_

std::string global_frame_id_

double lambda_short_

double laser_likelihood_max_dist_

double laser_max_range_

double laser_min_range_

std::string sensor_model_type_

int max_beams_

int max_particles_

int min_particles_

std::string odom_frame_id_

double pf_err_

double pf_z_

double alpha_fast_

double alpha_slow_

int resample_interval_

std::string robot_model_type_

tf2::Duration save_pose_period_

double sigma_hit_

bool tf_broadcast_

tf2::Duration transform_tolerance_

double a_thresh_

double d_thresh_

double z_hit_

double z_max_

double z_short_

double z_rand_

std::string scan_topic_ = {"scan"}

std::string map_topic_ = {"map"}

bool freespace_downsampling_ = false

Protected Static Functions

static pf_vector_t uniformPoseGenerator(void *arg)

struct amcl_hyp_t

Public Members

double weight

pf_vector_t pf_pose_mean

pf_matrix_t pf_pose_cov