adding doxygen to costmap_2d (#2292)

* adding doxygen to costmap_2d part 1

* finishing doxygen for costmap

* linting

nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp

@@ -31,17 +31,31 @@ namespace nav2_costmap_2d
 
 class Costmap2DROS;
 
+/**
+ * @class ClearCostmapService
+ * @brief Exposes services to clear costmap objects in inclusive/exclusive regions or completely
+ */
 class ClearCostmapService
 {
 public:
+  /**
+   * @brief A constructor
+   */
   ClearCostmapService(const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap);
 
+  /**
+   * @brief A constructor
+   */
   ClearCostmapService() = delete;
 
-  // Clears the region outside of a user-specified area reverting to the static map
+  /**
+   * @brief Clears the region outside of a user-specified area reverting to the static map
+   */
   void clearRegion(double reset_distance, bool invert);
 
-  // Clears all layers
+  /**
+   * @brief Clears all layers
+   */
   void clearEntirely();
 
 private:
@@ -57,27 +71,42 @@ private:
 
   // Server for clearing the costmap
   rclcpp::Service<nav2_msgs::srv::ClearCostmapExceptRegion>::SharedPtr clear_except_service_;
+  /**
+   * @brief Callback to clear costmap except in a given region
+   */
   void clearExceptRegionCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Request> request,
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Response> response);
 
   rclcpp::Service<nav2_msgs::srv::ClearCostmapAroundRobot>::SharedPtr clear_around_service_;
+  /**
+   * @brief Callback to clear costmap in a given region
+   */
   void clearAroundRobotCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Request> request,
     const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Response> response);
 
   rclcpp::Service<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr clear_entire_service_;
+  /**
+   * @brief Callback to clear costmap
+   */
   void clearEntireCallback(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Request> request,
     const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Response> response);
 
+  /**
+   * @brief  Function used to clear a given costmap layer
+   */
   void clearLayerRegion(
     std::shared_ptr<CostmapLayer> & costmap, double pose_x, double pose_y, double reset_distance,
     bool invert);
 
+  /**
+   * @brief Get the robot's position in the costmap using the master costmap
+   */
   bool getPosition(double & x, double & y) const;
 };
 

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp

@@ -258,11 +258,19 @@ public:
    */
   double getResolution() const;
 
+  /**
+   * @brief Set the default background value of the costmap
+   * @param c default value
+   */
   void setDefaultValue(unsigned char c)
   {
     default_value_ = c;
   }
 
+  /**
+   * @brief Get the default background value of the costmap
+   * @return default value
+   */
   unsigned char getDefaultValue()
   {
     return default_value_;
@@ -309,12 +317,21 @@ public:
    */
   bool saveMap(std::string file_name);
 
+  /**
+   * @brief Resize the costmap
+   */
   void resizeMap(
     unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
     double origin_y);
 
+  /**
+   * @brief Reset the costmap in bounds
+   */
   void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn);
 
+  /**
+   * @brief Reset the costmap in bounds to a value
+   */
   void resetMapToValue(
     unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn, unsigned char value);
 
@@ -390,7 +407,8 @@ protected:
    * @param  y0 The starting y coordinate
    * @param  x1 The ending x coordinate
    * @param  y1 The ending y coordinate
-   * @param  max_length The maximum desired length of the segment... allows you to not go all the way to the endpoint
+   * @param  max_length The maximum desired length of the segment...
+   * allows you to not go all the way to the endpoint
    * @param  min_length The minimum desired length of the segment
    */
   template<class ActionType>
@@ -426,7 +444,7 @@ protected:
     if (abs_dx >= abs_dy) {
       int error_y = abs_dx / 2;
 
-      // Subtract min_length from total length since initial point (x0, y0) has been adjusted by min Z
+      // Subtract minlength from length since initial point (x0, y0)has been adjusted by min Z
       length = (unsigned int)(scale * abs_dx) - min_length;
 
       bresenham2D(
@@ -437,7 +455,7 @@ protected:
     // otherwise y is dominant
     int error_x = abs_dy / 2;
 
-    // Subtract min_length from total length since initial point (x0, y0) has been adjusted by min Z
+    // Subtract minlength from total length since initial point (x0, y0) has been adjusted by min Z
     length = (unsigned int)(scale * abs_dy) - min_length;
     bresenham2D(
       at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, length);
@@ -445,7 +463,8 @@ protected:
 
 private:
   /**
-   * @brief  A 2D implementation of Bresenham's raytracing algorithm... applies an action at each step
+   * @brief  A 2D implementation of Bresenham's raytracing algorithm...
+   * applies an action at each step
    */
   template<class ActionType>
   inline void bresenham2D(
@@ -467,6 +486,9 @@ private:
     at(offset);
   }
 
+  /**
+   * @brief get the sign of an int
+   */
   inline int sign(int x)
   {
     return x > 0 ? 1.0 : -1.0;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

@@ -78,19 +78,34 @@ public:
    */
   ~Costmap2DPublisher();
 
+  /**
+   * @brief Configure node
+   */
   void on_configure() {}
+
+  /**
+   * @brief Activate node
+   */
   void on_activate()
   {
     costmap_pub_->on_activate();
     costmap_update_pub_->on_activate();
     costmap_raw_pub_->on_activate();
   }
+
+  /**
+   * @brief deactivate node
+   */
   void on_deactivate()
   {
     costmap_pub_->on_deactivate();
     costmap_update_pub_->on_deactivate();
     costmap_raw_pub_->on_deactivate();
   }
+
+  /**
+   * @brief Cleanup node
+   */
   void on_cleanup() {}
 
   /** @brief Include the given bounds in the changed-rectangle. */

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -90,12 +90,34 @@ public:
     const std::string & parent_namespace,
     const std::string & local_namespace);
 
+  /**
+   * @brief A destructor
+   */
   ~Costmap2DROS();
 
+  /**
+   * @brief Configure node
+   */
   nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Activate node
+   */
   nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Deactivate node
+   */
   nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Cleanup node
+   */
   nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief shutdown node
+   */
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
   /**
@@ -120,6 +142,9 @@ public:
    */
   void resume();
 
+  /**
+   * @brief Update the map with the layered costmap / plugins
+   */
   void updateMap();
 
   /**
@@ -190,6 +215,9 @@ public:
     return robot_base_frame_;
   }
 
+  /**
+   * @brief Get the layered costmap object used in the node
+   */
   LayeredCostmap * getLayeredCostmap()
   {
     return layered_costmap_.get();
@@ -284,6 +312,10 @@ protected:
   std::unique_ptr<LayeredCostmap> layered_costmap_{nullptr};
   std::string name_;
   std::string parent_namespace_;
+
+  /**
+   * @brief Function on timer for costmap update
+   */
   void mapUpdateLoop(double frequency);
   bool map_update_thread_shutdown_{false};
   bool stop_updates_{false};
@@ -294,7 +326,9 @@ protected:
   rclcpp::Duration publish_cycle_{1, 0};
   pluginlib::ClassLoader<Layer> plugin_loader_{"nav2_costmap_2d", "nav2_costmap_2d::Layer"};
 
-  // Parameters
+  /**
+   * @brief Get parameters for node
+   */
   void getParameters();
   bool always_send_full_costmap_{false};
   std::string footprint_;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp

@@ -54,7 +54,13 @@ namespace nav2_costmap_2d
 class CostmapFilter : public Layer
 {
 public:
+  /**
+   * @brief A constructor
+   */
   CostmapFilter();
+  /**
+   * @brief A destructor
+   */
   ~CostmapFilter();
 
   /**
@@ -69,18 +75,53 @@ public:
     return access_;
   }
 
-  /** Layer API **/
+  /**
+   * @brief Initialization process of layer on startup
+   */
   virtual void onInitialize() final;
+
+  /**
+   * @brief Update the bounds of the master costmap by this layer's update dimensions
+   * @param robot_x X pose of robot
+   * @param robot_y Y pose of robot
+   * @param robot_yaw Robot orientation
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
   virtual void updateBounds(
     double robot_x, double robot_y, double robot_yaw,
     double * min_x, double * min_y, double * max_x, double * max_y) final;
+
+  /**
+   * @brief Update the costs in the master costmap in the window
+   * @param master_grid The master costmap grid to update
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
   virtual void updateCosts(
     nav2_costmap_2d::Costmap2D & master_grid,
     int min_i, int min_j, int max_i, int max_j) final;
 
+  /**
+   * @brief Activate the layer
+   */
   virtual void activate() final;
+  /**
+   * @brief Deactivate the layer
+   */
   virtual void deactivate() final;
+  /**
+   * @brief Reset the layer
+   */
   virtual void reset() final;
+
+  /**
+   * @brief If clearing operations should be processed on this layer or not
+   */
   virtual bool isClearable() {return false;}
 
   /** CostmapFilter API **/

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp

@@ -50,25 +50,51 @@
 namespace nav2_costmap_2d
 {
 
+/**
+ * @class KeepoutFilter
+ * @brief Reads in a keepout mask and marks keepout regions in the map
+ * to prevent planning or control in restricted areas
+ */
 class KeepoutFilter : public CostmapFilter
 {
 public:
+  /**
+   * @brief A constructor
+   */
   KeepoutFilter();
 
+  /**
+   * @brief Initialize the filter and subscribe to the info topic
+   */
   void initializeFilter(
     const std::string & filter_info_topic);
 
+  /**
+   * @brief Process the keepout layer at the current pose / bounds / grid
+   */
   void process(
     nav2_costmap_2d::Costmap2D & master_grid,
     int min_i, int min_j, int max_i, int max_j,
     const geometry_msgs::msg::Pose2D & pose);
 
+  /**
+   * @brief Reset the costmap filter / topic / info
+   */
   void resetFilter();
 
+  /**
+   * @brief If this filter is active
+   */
   bool isActive();
 
 private:
+  /**
+   * @brief Callback for the filter information
+   */
   void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg);
+  /**
+   * @brief Callback for the filter mask
+   */
   void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
 
   rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp

@@ -48,22 +48,42 @@
 
 namespace nav2_costmap_2d
 {
-
+/**
+ * @class SpeedFilter
+ * @brief Reads in a speed restriction mask and enables a robot to
+ * dynamically adjust speed based on pose in map to slow in dangerous
+ * areas. Done via absolute speed setting or percentage of maximum speed
+ */
 class SpeedFilter : public CostmapFilter
 {
 public:
+  /**
+   * @brief A constructor
+   */
   SpeedFilter();
 
+  /**
+   * @brief Initialize the filter and subscribe to the info topic
+   */
   void initializeFilter(
     const std::string & filter_info_topic);
 
+  /**
+   * @brief Process the keepout layer at the current pose / bounds / grid
+   */
   void process(
     nav2_costmap_2d::Costmap2D & master_grid,
     int min_i, int min_j, int max_i, int max_j,
     const geometry_msgs::msg::Pose2D & pose);
 
+  /**
+   * @brief Reset the costmap filter / topic / info
+   */
   void resetFilter();
 
+  /**
+   * @brief If this filter is active
+   */
   bool isActive();
 
 protected:
@@ -98,7 +118,13 @@ protected:
     const unsigned int mx, const unsigned int my) const;
 
 private:
+  /**
+   * @brief Callback for the filter information
+   */
   void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg);
+  /**
+   * @brief Callback for the filter mask
+   */
   void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
 
   rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp

@@ -45,21 +45,40 @@
 namespace nav2_costmap_2d
 {
 
+/**
+ * @class CostmapLayer
+ * @brief A costmap layer base class for costmap plugin layers.
+ * Rather than just a layer, this object also contains an internal
+ * costmap object to populate and maintain state.
+ */
 class CostmapLayer : public Layer, public Costmap2D
 {
 public:
+  /**
+   * @brief CostmapLayer constructor
+   */
   CostmapLayer()
   : has_extra_bounds_(false),
     extra_min_x_(1e6), extra_max_x_(-1e6),
     extra_min_y_(1e6), extra_max_y_(-1e6) {}
 
+  /**
+   * @brief If layer is discrete
+   */
   bool isDiscretized()
   {
     return true;
   }
 
+  /**
+   * @brief Match the size of the master costmap
+   */
   virtual void matchSize();
 
+  /**
+   * @brief Clear an are in the costmap with the given dimension
+   * if invert, then clear everything except these dimensions
+   */
   virtual void clearArea(int start_x, int start_y, int end_x, int end_y, bool invert);
 
   /**

nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp

@@ -56,11 +56,13 @@ inline double sign0(double x)
   return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0);
 }
 
+/** @brief Gets L2 norm distance */
 inline double distance(double x0, double y0, double x1, double y1)
 {
   return hypot(x1 - x0, y1 - y0);
 }
 
+/** @brief Gets point distance to a line */
 double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1);
 
 #endif  // NAV2_COSTMAP_2D__COSTMAP_MATH_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

@@ -25,24 +25,45 @@
 
 namespace nav2_costmap_2d
 {
-
+/**
+ * @class CostmapSubscriber
+ * @brief Subscribes to the costmap via a ros topic
+ */
 class CostmapSubscriber
 {
 public:
+  /**
+   * @brief A constructor
+   */
   CostmapSubscriber(
     const nav2_util::LifecycleNode::WeakPtr & parent,
     const std::string & topic_name);
 
+  /**
+   * @brief A constructor
+   */
   CostmapSubscriber(
     const rclcpp::Node::WeakPtr & parent,
     const std::string & topic_name);
 
+  /**
+   * @brief A destructor
+   */
   ~CostmapSubscriber() {}
 
+  /**
+   * @brief A Get the costmap from topic
+   */
   std::shared_ptr<Costmap2D> getCostmap();
 
 protected:
+  /**
+   * @brief Convert an occ grid message into a costmap object
+   */
   void toCostmap2D();
+  /**
+   * @brief Callback for the costmap topic
+   */
   void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg);
 
   std::shared_ptr<Costmap2D> costmap_;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp

@@ -37,10 +37,17 @@
 
 namespace nav2_costmap_2d
 {
-
+/**
+ * @class CostmapTopicCollisionChecker
+ * @brief Using a costmap via a ros topic, this object is used to
+ * find if robot poses are in collision with the costmap environment
+ */
 class CostmapTopicCollisionChecker
 {
 public:
+  /**
+   * @brief A constructor
+   */
   CostmapTopicCollisionChecker(
     CostmapSubscriber & costmap_sub,
     FootprintSubscriber & footprint_sub,
@@ -49,15 +56,28 @@ public:
     std::string global_frame = "map",
     std::string robot_base_frame = "base_link",
     double transform_tolerance = 0.1);
-
+  /**
+   * @brief A destructor
+   */
   ~CostmapTopicCollisionChecker() = default;
 
-  // Returns the obstacle footprint score for a particular pose
+  /**
+   * @brief Returns the obstacle footprint score for a particular pose
+   */
   double scorePose(const geometry_msgs::msg::Pose2D & pose);
+  /**
+   * @brief Returns if a pose is collision free
+   */
   bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose);
 
 protected:
+  /**
+   * @brief Set a new footprint
+   */
   void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint);
+  /**
+   * @brief Get a footprint at a set pose
+   */
   Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose);
 
   // Name used for logging

nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp

@@ -40,7 +40,11 @@
 
 namespace nav2_costmap_2d
 {
-
+/**
+ * @class CollisionCheckerException
+ * @brief Exceptions thrown if collision checker determines a pose is in
+ * collision with the environment costmap
+ */
 class CollisionCheckerException : public std::runtime_error
 {
 public:

nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp

@@ -32,17 +32,45 @@ namespace nav2_costmap_2d
 {
 typedef std::vector<geometry_msgs::msg::Point> Footprint;
 
+/**
+ * @class FootprintCollisionChecker
+ * @brief Checker for collision with a footprint on a costmap
+ */
 template<typename CostmapT>
 class FootprintCollisionChecker
 {
 public:
+  /**
+   * @brief A constructor.
+   */
   FootprintCollisionChecker();
+  /**
+   * @brief A constructor.
+   */
   explicit FootprintCollisionChecker(CostmapT costmap);
+  /**
+   * @brief Find the footprint cost in oriented footprint
+   */
   double footprintCost(const Footprint footprint);
+  /**
+   * @brief Find the footprint cost a a post with an unoriented footprint
+   */
   double footprintCostAtPose(double x, double y, double theta, const Footprint footprint);
+  /**
+   * @brief Get the cost for a line segment
+   */
   double lineCost(int x0, int x1, int y0, int y1) const;
+  /**
+   * @brief Get the map coordinates from a world point
+   */
   bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
+  /**
+   * @brief Get the cost of a point
+   */
   double pointCost(int x, int y) const;
+  /**
+  * @brief Set the current costmap object to use for collision detection
+  */
   void setCostmap(CostmapT costmap);
 
 protected:

nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp

@@ -24,30 +24,49 @@
 
 namespace nav2_costmap_2d
 {
-
+/**
+ * @class FootprintSubscriber
+ * @brief Subscriber to the footprint topic to get current robot footprint
+ * (if changing) for use in collision avoidance
+ */
 class FootprintSubscriber
 {
 public:
+  /**
+   * @brief A constructor
+   */
   FootprintSubscriber(
     const nav2_util::LifecycleNode::WeakPtr & parent,
     const std::string & topic_name,
     const double & footprint_timeout);
 
+  /**
+   * @brief A constructor
+   */
   FootprintSubscriber(
     const rclcpp::Node::WeakPtr & parent,
     const std::string & topic_name,
     const double & footprint_timeout);
 
+  /**
+   * @brief A destructor
+   */
   ~FootprintSubscriber() {}
 
-  // Returns an oriented robot footprint at current time.
+  /**
+   * @brief Returns an oriented robot footprint at current time.
+   */
   bool getFootprint(
     std::vector<geometry_msgs::msg::Point> & footprint,
     rclcpp::Duration & valid_footprint_timeout);
+  /**
+   * @brief Returns an oriented robot footprint without timeout
+   */
   bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint);
 
-  // Returns an oriented robot footprint.
-  // The second parameter is the time the fooptrint was at this orientation.
+  /**
+   * @brief Returns an oriented robot footprint at stamped time.
+   */
   bool getFootprint(
     std::vector<geometry_msgs::msg::Point> & footprint,
     rclcpp::Time & stamp, rclcpp::Duration valid_footprint_timeout);
@@ -55,6 +74,9 @@ public:
 protected:
   rclcpp::Clock::SharedPtr clock_;
 
+  /**
+   * @brief Callback to process new footprint updates.
+   */
   void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg);
 
   std::string topic_name_;

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -73,26 +73,69 @@ public:
   unsigned int src_x_, src_y_;
 };
 
+/**
+ * @class InflationLayer
+ * @brief Layer to convolve costmap by robot's radius or footprint to prevent
+ * collisions and largely simply collision checking
+ */
 class InflationLayer : public Layer
 {
 public:
+  /**
+    * @brief A constructor
+    */
   InflationLayer();
 
+  /**
+    * @brief A destructor
+    */
   ~InflationLayer();
 
+  /**
+   * @brief Initialization process of layer on startup
+   */
   void onInitialize() override;
+
+  /**
+   * @brief Update the bounds of the master costmap by this layer's update dimensions
+   * @param robot_x X pose of robot
+   * @param robot_y Y pose of robot
+   * @param robot_yaw Robot orientation
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
   void updateBounds(
     double robot_x, double robot_y, double robot_yaw, double * min_x,
     double * min_y,
     double * max_x,
     double * max_y) override;
+  /**
+   * @brief Update the costs in the master costmap in the window
+   * @param master_grid The master costmap grid to update
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
   void updateCosts(
     nav2_costmap_2d::Costmap2D & master_grid,
     int min_i, int min_j, int max_i, int max_j) override;
 
+  /**
+   * @brief Match the size of the master costmap
+   */
   void matchSize() override;
+
+  /**
+   * @brief If clearing operations should be processed on this layer or not
+   */
   virtual bool isClearable() {return false;}
 
+  /**
+   * @brief Reset this costmap
+   */
   void reset() override
   {
     matchSize();
@@ -120,12 +163,19 @@ public:
 
   // Provide a typedef to ease future code maintenance
   typedef std::recursive_mutex mutex_t;
+
+  /**
+   * @brief Get the mutex of the inflation inforamtion
+   */
   mutex_t * getMutex()
   {
     return access_;
   }
 
 protected:
+  /**
+   * @brief Process updates on footprint changes to the inflation layer
+   */
   void onFootprintChanged() override;
 
 private:
@@ -163,15 +213,27 @@ private:
     return cached_costs_[dx * cache_length_ + dy];
   }
 
+  /**
+   * @brief Compute cached dsitances
+   */
   void computeCaches();
 
+  /**
+   * @brief Compute cached dsitances
+   */
   int generateIntegerDistances();
 
+  /**
+   * @brief Compute cached dsitances
+   */
   unsigned int cellDistance(double world_dist)
   {
     return layered_costmap_->getCostmap()->cellDistance(world_dist);
   }
 
+  /**
+   * @brief Enqueue new cells in cache distance update search
+   */
   inline void enqueue(
     unsigned int index, unsigned int mx, unsigned int my,
     unsigned int src_x, unsigned int src_y);

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -51,13 +51,25 @@ namespace nav2_costmap_2d
 {
 class LayeredCostmap;
 
-class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
+/**
+ * @class Layer
+ * @brief Abstract class for layered costmap plugin implementations
+ */
+class Layer
 {
 public:
+  /**
+   * @brief A constructor
+   */
   Layer();
+  /**
+   * @brief A destructor
+   */
   virtual ~Layer() {}
 
-  // TODO(mjeronimo): should the following functions changed to a lifecycle-style interface?
+  /**
+   * @brief Initialization process of layer on startup
+   */
   void initialize(
     LayeredCostmap * parent,
     std::string name,
@@ -65,9 +77,17 @@ public:
     const nav2_util::LifecycleNode::WeakPtr & node,
     rclcpp::Node::SharedPtr client_node,
     rclcpp::Node::SharedPtr rclcpp_node);
-  virtual void deactivate() {} /** @brief Stop publishers. */
-  virtual void activate() {}   /** @brief Restart publishers if they've been stopped. */
+  /** @brief Stop publishers. */
+  virtual void deactivate() {}
+  /** @brief Restart publishers if they've been stopped. */
+  virtual void activate() {}
+  /**
+   * @brief Reset this costmap
+   */
   virtual void reset() = 0;
+  /**
+   * @brief If clearing operations should be processed on this layer or not
+   */
   virtual bool isClearable() = 0;
 
   /**
@@ -99,7 +119,7 @@ public:
    * changes (via LayeredCostmap::setFootprint()).  Override to be
    * notified of changes to the robot's footprint. */
   virtual void onFootprintChanged() {}
-
+  /** @brief Get the name of the costmap layer */
   std::string getName() const
   {
     return name_;
@@ -127,10 +147,13 @@ public:
   void declareParameter(
     const std::string & param_name,
     const rclcpp::ParameterValue & value);
+  /** @brief Convenience functions for declaring ROS parameters */
   void declareParameter(
     const std::string & param_name,
     const rclcpp::ParameterType & param_type);
+  /** @brief Convenience functions for declaring ROS parameters */
   bool hasParameter(const std::string & param_name);
+  /** @brief Convenience functions for declaring ROS parameters */
   std::string getFullName(const std::string & param_name);
 
 protected:

nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp

@@ -78,11 +78,17 @@ public:
     return global_frame_;
   }
 
+  /**
+   * @brief Resize the map to a new size, resolution, or origin
+   */
   void resizeMap(
     unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
     double origin_y,
     bool size_locked = false);
 
+  /**
+   * @brief Get the size of the bounds for update
+   */
   void getUpdatedBounds(double & minx, double & miny, double & maxx, double & maxy)
   {
     minx = minx_;
@@ -91,38 +97,63 @@ public:
     maxy = maxy_;
   }
 
+  /**
+   * @brief If the costmap is current, e.g. are all the layers processing recent data
+   * and not stale information for a good state.
+   */
   bool isCurrent();
 
+  /**
+   * @brief Get the costmap pointer to the master costmap
+   */
   Costmap2D * getCostmap()
   {
     return &costmap_;
   }
 
+  /**
+   * @brief If this costmap is rolling or not
+   */
   bool isRolling()
   {
     return rolling_window_;
   }
 
+  /**
+   * @brief If this costmap is tracking unknown space or not
+   */
   bool isTrackingUnknown()
   {
     return costmap_.getDefaultValue() == nav2_costmap_2d::NO_INFORMATION;
   }
 
+  /**
+   * @brief Get the vector of pointers to the costmap plugins
+   */
   std::vector<std::shared_ptr<Layer>> * getPlugins()
   {
     return &plugins_;
   }
 
+  /**
+   * @brief Add a new plugin to the plugins vector to process
+   */
   void addPlugin(std::shared_ptr<Layer> plugin)
   {
     plugins_.push_back(plugin);
   }
 
+  /**
+   * @brief Get if the size of the costmap is locked
+   */
   bool isSizeLocked()
   {
     return size_locked_;
   }
 
+  /**
+   * @brief Get the bounds of the costmap
+   */
   void getBounds(unsigned int * x0, unsigned int * xn, unsigned int * y0, unsigned int * yn)
   {
     *x0 = bx0_;
@@ -131,6 +162,9 @@ public:
     *yn = byn_;
   }
 
+  /**
+   * @brief if the costmap is initialized
+   */
   bool isInitialized()
   {
     return initialized_;

nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp

@@ -55,7 +55,9 @@ public:
     raytrace_min_range_(0.0)
   {
   }
-
+  /**
+   * @brief A destructor
+   */
   virtual ~Observation()
   {
     delete cloud_;

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -61,28 +61,74 @@
 namespace nav2_costmap_2d
 {
 
+/**
+ * @class ObstacleLayer
+ * @brief Takes in laser and pointcloud data to populate into 2D costmap
+ */
 class ObstacleLayer : public CostmapLayer
 {
 public:
+  /**
+   * @brief A constructor
+   */
   ObstacleLayer()
   {
     costmap_ = NULL;  // this is the unsigned char* member of parent class Costmap2D.
   }
 
+  /**
+   * @brief A destructor
+   */
   virtual ~ObstacleLayer();
+  /**
+   * @brief Initialization process of layer on startup
+   */
   virtual void onInitialize();
+  /**
+   * @brief Update the bounds of the master costmap by this layer's update dimensions
+   * @param robot_x X pose of robot
+   * @param robot_y Y pose of robot
+   * @param robot_yaw Robot orientation
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
   virtual void updateBounds(
     double robot_x, double robot_y, double robot_yaw, double * min_x,
     double * min_y,
     double * max_x,
     double * max_y);
+  /**
+   * @brief Update the costs in the master costmap in the window
+   * @param master_grid The master costmap grid to update
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
   virtual void updateCosts(
     nav2_costmap_2d::Costmap2D & master_grid,
     int min_i, int min_j, int max_i, int max_j);
 
-  virtual void activate();
+  /**
+   * @brief Deactivate the layer
+   */
   virtual void deactivate();
+
+  /**
+   * @brief Activate the layer
+   */
+  virtual void activate();
+
+  /**
+   * @brief Reset this costmap
+   */
   virtual void reset();
+
+  /**
+   * @brief If clearing operations should be processed on this layer or not
+   */
   virtual bool isClearable() {return true;}
 
   /**
@@ -152,6 +198,9 @@ protected:
     double * max_x,
     double * max_y);
 
+  /**
+   * @brief Process update costmap with raytracing the window bounds
+   */
   void updateRaytraceBounds(
     double ox, double oy, double wx, double wy, double max_range, double min_range,
     double * min_x, double * min_y,
@@ -160,6 +209,9 @@ protected:
 
   std::vector<geometry_msgs::msg::Point> transformed_footprint_;
   bool footprint_clearing_enabled_;
+  /**
+   * @brief Clear costmap layer info below the robot's footprint
+   */
   void updateFootprint(
     double robot_x, double robot_y, double robot_yaw, double * min_x,
     double * min_y,

nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp

@@ -53,6 +53,10 @@
 namespace nav2_costmap_2d
 {
 
+/**
+ * @class RangeSensorLayer
+ * @brief Takes in IR/Sonar/similar point measurement sensors and populates in costmap
+ */
 class RangeSensorLayer : public CostmapLayer
 {
 public:
@@ -63,45 +67,132 @@ public:
     ALL
   };
 
+  /**
+   * @brief A constructor
+   */
   RangeSensorLayer();
 
+  /**
+   * @brief Initialization process of layer on startup
+   */
   virtual void onInitialize();
+
+  /**
+   * @brief Update the bounds of the master costmap by this layer's update dimensions
+   * @param robot_x X pose of robot
+   * @param robot_y Y pose of robot
+   * @param robot_yaw Robot orientation
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
   virtual void updateBounds(
     double robot_x, double robot_y, double robot_yaw,
     double * min_x, double * min_y, double * max_x, double * max_y);
+
+  /**
+   * @brief Update the costs in the master costmap in the window
+   * @param master_grid The master costmap grid to update
+   * @param min_x X min map coord of the window to update
+   * @param min_y Y min map coord of the window to update
+   * @param max_x X max map coord of the window to update
+   * @param max_y Y max map coord of the window to update
+   */
   virtual void updateCosts(
     nav2_costmap_2d::Costmap2D & master_grid, int min_i,
     int min_j, int max_i, int max_j);
+
+  /**
+   * @brief Reset this costmap
+   */
   virtual void reset();
+
+  /**
+   * @brief Deactivate the layer
+   */
   virtual void deactivate();
+
+  /**
+   * @brief Activate the layer
+   */
   virtual void activate();
+
+  /**
+   * @brief If clearing operations should be processed on this layer or not
+   */
   virtual bool isClearable() {return true;}
 
+  /**
+   * @brief Handle an incoming Range message to populate into costmap
+   */
   void bufferIncomingRangeMsg(const sensor_msgs::msg::Range::SharedPtr range_message);
 
 private:
+  /**
+   * @brief Processes all sensors into the costmap buffered from callbacks
+   */
   void updateCostmap();
+  /**
+   * @brief Update the actual costmap with the values processed
+   */
   void updateCostmap(sensor_msgs::msg::Range & range_message, bool clear_sensor_cone);
 
+  /**
+   * @brief Process general incoming range sensor data. If min=max ranges,
+   * fixed processor callback is used, else uses variable callback
+   */
   void processRangeMsg(sensor_msgs::msg::Range & range_message);
+  /**
+    * @brief Process fixed range incoming range sensor data
+    */
   void processFixedRangeMsg(sensor_msgs::msg::Range & range_message);
+  /**
+    * @brief Process variable range incoming range sensor data
+    */
   void processVariableRangeMsg(sensor_msgs::msg::Range & range_message);
 
+  /**
+   * @brief Reset the angle min/max x, and min/max y values
+   */
   void resetRange();
 
+  /**
+   * @brief Get the gamma value for an angle, theta
+   */
   inline double gamma(double theta);
+  /**
+   * @brief Get the delta value for an angle, phi
+   */
   inline double delta(double phi);
+  /**
+   * @brief Apply the sensor model of the layer for range sensors
+   */
   inline double sensor_model(double r, double phi, double theta);
 
+  /**
+   * @brief Get angles
+   */
   inline void get_deltas(double angle, double * dx, double * dy);
+
+  /**
+   * @brief Update the cost in a cell with information
+   */
   inline void update_cell(
     double ox, double oy, double ot,
     double r, double nx, double ny, bool clear);
 
+  /**
+   * @brief Find probability value of a cost
+   */
   inline double to_prob(unsigned char c)
   {
     return static_cast<double>(c) / nav2_costmap_2d::LETHAL_OBSTACLE;
   }
+
+  /**
+   * @brief Find cost value of a probability
+   */
   inline unsigned char to_cost(double p)
   {
     return static_cast<unsigned char>(p * nav2_costmap_2d::LETHAL_OBSTACLE);
@@ -126,11 +217,17 @@ private:
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr> range_subs_;
   double min_x_, min_y_, max_x_, max_y_;
 
+  /**
+   * @brief Find the area of 3 points of a triangle
+   */
   float area(int x1, int y1, int x2, int y2, int x3, int y3)
   {
     return fabs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0);
   }
 
+  /**
+   * @brief Find the cross product of 3 vectors, A,B,C
+   */
   int orient2d(int Ax, int Ay, int Bx, int By, int Cx, int Cy)
   {
     return (Bx - Ax) * (Cy - Ay) - (By - Ay) * (Cx - Ax);