diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index dc86baaf6e1..6c3f436d2bd 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -52,19 +52,42 @@ namespace nav2_amcl { - +/* + * @class AmclNode + * @brief ROS wrapper for AMCL + */ class AmclNode : public nav2_util::LifecycleNode { public: + /* + * @brief AMCL constructor + */ AmclNode(); + /* + * @brief AMCL destructor + */ ~AmclNode(); protected: - // Implement the lifecycle interface + /* + * @brief Lifecycle configure + */ nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /* + * @brief Lifecycle activate + */ nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /* + * @brief Lifecycle deactivate + */ nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /* + * @brief Lifecycle cleanup + */ nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /* + * @brief Lifecycle shutdown + */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't @@ -80,11 +103,30 @@ class AmclNode : public nav2_util::LifecycleNode } amcl_hyp_t; // Map-related + /* + * @brief Get new map from ROS topic to localize in + * @param msg Map message + */ void mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + /* + * @brief Handle a new map message + * @param msg Map message + */ void handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg); + /* + * @brief Creates lookup table of free cells in map + */ void createFreeSpaceVector(); + /* + * @brief Frees allocated map related memory + */ void freeMapDependentMemory(); map_t * map_{nullptr}; + /* + * @brief Convert an occupancy grid map to an AMCL map + * @param map_msg Map message + * @return pointer to map for AMCL to use + */ map_t * convertMap(const nav_msgs::msg::OccupancyGrid & map_msg); bool first_map_only_{true}; std::atomic first_map_received_{false}; @@ -96,6 +138,9 @@ class AmclNode : public nav2_util::LifecycleNode #endif // Transforms + /* + * @brief Initialize required ROS transformations + */ void initTransforms(); std::shared_ptr tf_broadcaster_; std::shared_ptr tf_listener_; @@ -103,15 +148,24 @@ class AmclNode : public nav2_util::LifecycleNode bool sent_first_transform_{false}; bool latest_tf_valid_{false}; tf2::Transform latest_tf_; + /* + * @brief Wait for transformation required to operate (laser to base) + */ void waitForTransforms(); // Message filters + /* + * @brief Initialize incoming data message subscribers and filters + */ void initMessageFilters(); std::unique_ptr> laser_scan_sub_; std::unique_ptr> laser_scan_filter_; message_filters::Connection laser_scan_connection_; // Publishers and subscribers + /* + * @brief Initialize pub subs of AMCL + */ void initPubSub(); rclcpp::Subscription::ConstSharedPtr initial_pose_sub_; @@ -120,12 +174,24 @@ class AmclNode : public nav2_util::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr particlecloud_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr particle_cloud_pub_; + /* + * @brief Handle with an initial pose estimate is received + */ void initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + /* + * @brief Handle when a laser scan is received + */ void laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan); // Services and service callbacks + /* + * @brief Initialize state services + */ void initServices(); rclcpp::Service::SharedPtr global_loc_srv_; + /* + * @brief Service callback for a global relocalization request + */ void globalLocalizationCallback( const std::shared_ptr request_header, const std::shared_ptr request, @@ -133,6 +199,9 @@ class AmclNode : public nav2_util::LifecycleNode // Let amcl update samples without requiring motion rclcpp::Service::SharedPtr nomotion_update_srv_; + /* + * @brief Request an AMCL update even though the robot hasn't moved + */ void nomotionUpdateCallback( const std::shared_ptr request_header, const std::shared_ptr request, @@ -142,12 +211,18 @@ class AmclNode : public nav2_util::LifecycleNode std::atomic force_update_{false}; // Odometry + /* + * @brief Initialize odometry + */ void initOdometry(); std::unique_ptr motion_model_; geometry_msgs::msg::PoseStamped latest_odom_pose_; geometry_msgs::msg::PoseWithCovarianceStamped last_published_pose_; double init_pose_[3]; // Initial robot pose double init_cov_[3]; + /* + * @brief Get robot pose in odom frame using TF + */ bool getOdomPose( // Helper to get odometric pose from transform system geometry_msgs::msg::PoseStamped & pose, @@ -156,8 +231,13 @@ class AmclNode : public nav2_util::LifecycleNode std::atomic first_pose_sent_; // Particle filter + /* + * @brief Initialize particle filter + */ void initParticleFilter(); - // Pose-generating function used to uniformly distribute particles over the map + /* + * @brief Pose-generating function used to uniformly distribute particles over the map + */ static pf_vector_t uniformPoseGenerator(void * arg); pf_t * pf_{nullptr}; bool pf_init_; @@ -165,41 +245,78 @@ class AmclNode : public nav2_util::LifecycleNode int resample_count_{0}; // Laser scan related + /* + * @brief Initialize laser scan + */ void initLaserScan(); + /* + * @brief Create a laser object + */ nav2_amcl::Laser * createLaserObject(); int scan_error_count_{0}; std::vector lasers_; std::vector lasers_update_; std::map frame_to_laser_; rclcpp::Time last_laser_received_ts_; + /* + * @brief Check if a laser has been received + */ void checkLaserReceived(); std::chrono::seconds laser_check_interval_; // TODO(mjeronimo): not initialized - + /* + * @brief Check if sufficient time has elapsed to get an update + */ bool checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time last_time); rclcpp::Time last_time_printed_msg_; - + /* + * @brief Add a new laser scanner if a new one is received in the laser scallbacks + */ 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); + /* + * @brief Whether the pf needs to be updated + */ bool shouldUpdateFilter(const pf_vector_t pose, pf_vector_t & delta); + /* + * @brief Update the PF + */ bool updateFilter( const int & laser_index, const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan, const pf_vector_t & pose); + /* + * @brief Publish particle cloud + */ void publishParticleCloud(const pf_sample_set_t * set); + /* + * @brief Get the current state estimat hypothesis from the particle cloud + */ bool getMaxWeightHyp( std::vector & hyps, amcl_hyp_t & max_weight_hyps, int & max_weight_hyp); + /* + * @brief Publish robot pose in map frame from AMCL + */ void publishAmclPose( const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan, const std::vector & hyps, const int & max_weight_hyp); + /* + * @brief Determine TF transformation from map to odom + */ void calculateMaptoOdomTransform( const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan, const std::vector & hyps, const int & max_weight_hyp); + /* + * @brief Publish TF transformation from map to odom + */ void sendMapToOdomTransform(const tf2::TimePoint & transform_expiration); + /* + * @brief Handle a new pose estimate callback + */ void handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg); bool init_pose_received_on_inactive{false}; bool initial_pose_is_known_{false}; @@ -210,7 +327,9 @@ class AmclNode : public nav2_util::LifecycleNode double initial_pose_z_; double initial_pose_yaw_; - // Node parameters (initialized via initParameters) + /* + * @brief Get ROS parameters for node + */ void initParameters(); double alpha1_; double alpha2_; diff --git a/nav2_amcl/include/nav2_amcl/angleutils.hpp b/nav2_amcl/include/nav2_amcl/angleutils.hpp index d3f8332c5b3..84de30f78db 100644 --- a/nav2_amcl/include/nav2_amcl/angleutils.hpp +++ b/nav2_amcl/include/nav2_amcl/angleutils.hpp @@ -27,10 +27,26 @@ namespace nav2_amcl { +/* + * @class angleutils + * @brief Some utilities for working with angles + */ class angleutils { public: + /* + * @brief Normalize angles + * @brief z Angle to normalize + * @return normalized angle + */ static double normalize(double z); + + /* + * @brief Find minimum distance between 2 angles + * @brief a Angle 1 + * @brief b Angle 2 + * @return normalized angle difference + */ static double angle_diff(double a, double b); }; diff --git a/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp b/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp index 21e37199bd9..edc41241284 100644 --- a/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp +++ b/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp @@ -29,9 +29,9 @@ namespace nav2_amcl // Forward declarations class LaserData; -/** - * @class nav2_amcl::Laser - * @brief An abstract laser model class +/* + * @class Laser + * @brief Base class for laser sensor models */ class Laser { @@ -42,18 +42,23 @@ class Laser * @param map Map pointer to use */ Laser(size_t max_beams, map_t * map); + + /* + * @brief Laser destructor + */ virtual ~Laser(); - /** - * @brief update based on new sensor data - * @param data Laser data to update using - * @return bool If successful + /* + * @brief Run a sensor update on laser + * @param pf Particle filter to use + * @param data Laser data to use + * @return if it was succesful */ virtual bool sensorUpdate(pf_t * pf, LaserData * data) = 0; - /** - * @brief update laser pose - * @param laser_pose Laser pose + /* + * @brief Set the laser pose from an update + * @param laser_pose Pose of the laser */ void SetLaserPose(pf_vector_t & laser_pose); @@ -62,6 +67,11 @@ class Laser double z_rand_; double sigma_hit_; + /* + * @brief Reallocate weights + * @param max_samples Max number of samples + * @param max_obs number of observations + */ void reallocTempData(int max_samples, int max_obs); map_t * map_; pf_vector_t laser_pose_; @@ -71,11 +81,22 @@ class Laser double ** temp_obs_; }; +/* + * @class LaserData + * @brief Class of laser data to process + */ class LaserData { public: Laser * laser; + + /* + * @brief LaserData constructor + */ LaserData() {ranges = NULL;} + /* + * @brief LaserData destructor + */ virtual ~LaserData() {delete[] ranges;} public: @@ -84,13 +105,26 @@ class LaserData double(*ranges)[2]; }; - +/* + * @class BeamModel + * @brief Beam model laser sensor + */ class BeamModel : public Laser { public: + /* + * @brief BeamModel constructor + */ BeamModel( double z_hit, double z_short, double z_max, double z_rand, double sigma_hit, double lambda_short, double chi_outlier, size_t max_beams, map_t * map); + + /* + * @brief Run a sensor update on laser + * @param pf Particle filter to use + * @param data Laser data to use + * @return if it was succesful + */ bool sensorUpdate(pf_t * pf, LaserData * data); private: @@ -101,29 +135,69 @@ class BeamModel : public Laser double chi_outlier_; }; +/* + * @class LikelihoodFieldModel + * @brief likelihood field model laser sensor + */ class LikelihoodFieldModel : public Laser { public: + /* + * @brief BeamModel constructor + */ LikelihoodFieldModel( double z_hit, double z_rand, double sigma_hit, double max_occ_dist, size_t max_beams, map_t * map); + + /* + * @brief Run a sensor update on laser + * @param pf Particle filter to use + * @param data Laser data to use + * @return if it was succesful + */ bool sensorUpdate(pf_t * pf, LaserData * data); private: + /* + * @brief Perform the update function + * @param data Laser data to use + * @param pf Particle filter to use + * @return if it was succesful + */ static double sensorFunction(LaserData * data, pf_sample_set_t * set); }; +/* + * @class LikelihoodFieldModelProb + * @brief likelihood prob model laser sensor + */ class LikelihoodFieldModelProb : public Laser { public: + /* + * @brief BeamModel constructor + */ LikelihoodFieldModelProb( double z_hit, double z_rand, double sigma_hit, double max_occ_dist, bool do_beamskip, double beam_skip_distance, double beam_skip_threshold, double beam_skip_error_threshold, size_t max_beams, map_t * map); + + /* + * @brief Run a sensor update on laser + * @param pf Particle filter to use + * @param data Laser data to use + * @return if it was succesful + */ bool sensorUpdate(pf_t * pf, LaserData * data); private: + /* + * @brief Perform the update function + * @param data Laser data to use + * @param pf Particle filter to use + * @return if it was succesful + */ static double sensorFunction(LaserData * data, pf_sample_set_t * set); bool do_beamskip_; double beam_skip_distance_; diff --git a/nav2_amcl/src/map/map_cspace.cpp b/nav2_amcl/src/map/map_cspace.cpp index 6dca9e89645..ce66baf1a56 100644 --- a/nav2_amcl/src/map/map_cspace.cpp +++ b/nav2_amcl/src/map/map_cspace.cpp @@ -25,6 +25,10 @@ #include #include "nav2_amcl/map/map.hpp" +/* + * @class CellData + * @brief Data about map cells + */ class CellData { public: @@ -33,9 +37,16 @@ class CellData unsigned int src_i_, src_j_; }; +/* + * @class CachedDistanceMap + * @brief Cached map with distances + */ class CachedDistanceMap { public: + /* + * @brief CachedDistanceMap constructor + */ CachedDistanceMap(double scale, double max_dist) : distances_(NULL), scale_(scale), max_dist_(max_dist) { @@ -48,6 +59,10 @@ class CachedDistanceMap } } } + + /* + * @brief CachedDistanceMap destructor + */ ~CachedDistanceMap() { if (distances_) { @@ -63,7 +78,9 @@ class CachedDistanceMap int cell_radius_; }; - +/* + * @brief operator< + */ bool operator<(const CellData & a, const CellData & b) { return a.map_->cells[MAP_INDEX( @@ -71,6 +88,12 @@ bool operator<(const CellData & a, const CellData & b) a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist; } +/* + * @brief get_distance_map + * @param scale of cost information wrt distance + * @param max_dist Maximum distance to cache from occupied information + * @return Pointer to cached distance map + */ CachedDistanceMap * get_distance_map(double scale, double max_dist) { @@ -86,6 +109,9 @@ get_distance_map(double scale, double max_dist) return cdm; } +/* + * @brief enqueue cell data for caching + */ void enqueue( map_t * map, int i, int j, int src_i, int src_j, @@ -119,7 +145,11 @@ void enqueue( marked[MAP_INDEX(map, i, j)] = 1; } -// Update the cspace distance values +/* + * @brief Update the cspace distance values + * @param map Map to update + * @param max_occ_distance Maximum distance for occpuancy interest + */ void map_update_cspace(map_t * map, double max_occ_dist) { unsigned char * marked;