diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
index 589f11781a6aadc0d5dc4844b8147022817b44a2..46d79bd9df52b6fbd0ade67e2068d41b0eb98b72 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp
+++ b/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;
};
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
index 77339772a261bf47df9926de389d70332ec9eddf..ba9f88056015cefcdd3044636b6c689fb104708e 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp
+++ b/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;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
index ee276442ca471045554a112bbc1c3bf8c3ad4cc4..bb3869e4f7e247c6679ee98220b1f6eaf8360a98 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp
+++ b/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. */
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
index 3d6011f4f172678e3bfa410e4dd72ca4f1546141..15872d61138ad485146c89719d7fe9f96ea5d94d 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
+++ b/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_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
index 9f265e41e86231c4bef195aeeeba01584d874e0f..ecfb9d71f8c3e9c60f482aa1ff61cca4caaae4fc 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
+++ b/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 **/
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
index 01b40a7bfd7bad207aed1186cc1d93cd087ee5e4..1755d57eb7730ed84c5d56c5470da2c165dd043b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp
+++ b/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_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp
index 148180eb3395f018d76c1e8f4e60a3088c7a7bcf..c6b86799aa888a5c9bd6dc554c8562a6b2c5e564 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp
+++ b/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_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
index 5df24b543b18b57e84c0f6e25356afc129bb96eb..15285df5fe262f220c7a06caae476012eed7ecad 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
+++ b/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);
/**
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp
index 795bac53fc46dde14b79a6ffc128ef9998b0d9f5..2c21d859b20112faf882ef748382ee02524de0a6 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp
+++ b/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_
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
index 0873d8781ece13aaad2e3ac1241fe25aa9462174..5c65b1161c7352851cb7ec641ad8a8632015a606 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
+++ b/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_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
index 9f54728e635e0b8c95ec031db7069e5e13bfef94..b436f98cbd7771b84bcf479f79992ec2fc6c834b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp
+++ b/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
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp
index 0646555dadc2236d2b08c27f4b8b90d4e17808fa..0db73867cb72ee2bb361709aab81ff636195c012 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp
+++ b/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:
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp
index 39c51328b8d33cea2edd76de5be939729ec4b6b0..662dbbdd706371564966597841c1451fbfc4e0e0 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp
+++ b/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:
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp
index b6eef0c043e029cc2cef1174d890b75ca0c935da..d757db53bdc742f3c6c43befa58b9079a67b8041 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp
+++ b/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_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
index 225c0fbbc77f1a946aaebf5e76a4affe379da287..66f6163b3cfb4e76975a3350f7564eafb45bf714 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
+++ b/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);
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
index 63d69625dd7b9a554559ea77df2edd2ae030e5e6..5f8c152dc436b43c5c1c437e243570f643d84a2f 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
+++ b/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:
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp
index 4964b03ef8a6a406f569405f7a1f5a01dd282f39..cbd965bb350389ded0a0f3faca9e49cc6bc388e8 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.hpp
+++ b/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_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp
index e17d51a07c5ea464223e1cb4bf710f7678185d67..850f551e9377e406e0faa261b025ecc6933a8a0d 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp
+++ b/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_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
index 953b1763a294c9f3de95a62968bacd5bb4b6bee9..deac55b2695be8485c6bf06cc00f1c05aead776b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp
+++ b/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,
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp
index 840ee20a0f1e38946545702444f57b6cacd96374..9097869bce1a8ebe3a3f505486768e111aabc06b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp
+++ b/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);
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
index e2be08d010ff93eac029fb32a5ffa2691cb5828f..bf047d7f3835c85e25693f1064b832ef24f58c53 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
@@ -51,30 +51,86 @@
namespace nav2_costmap_2d
{
+/**
+ * @class StaticLayer
+ * @brief Takes in a map generated from SLAM to add costs to costmap
+ */
class StaticLayer : public CostmapLayer
{
public:
+ /**
+ * @brief Static Layer constructor
+ */
StaticLayer();
+ /**
+ * @brief Static Layer destructor
+ */
virtual ~StaticLayer();
+ /**
+ * @brief Initialization process of layer on startup
+ */
virtual void onInitialize();
+
+ /**
+ * @brief Activate this layer
+ */
virtual void activate();
+ /**
+ * @brief Deactivate this layer
+ */
virtual void deactivate();
+
+ /**
+ * @brief Reset this costmap
+ */
virtual void reset();
+
+ /**
+ * @brief If clearing operations should be processed on this layer or not
+ */
virtual bool isClearable() {return false;}
+ /**
+ * @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 Match the size of the master costmap
+ */
virtual void matchSize();
private:
+ /**
+ * @brief Get parameters of layer
+ */
void getParameters();
+
+ /**
+ * @brief Process a new map coming from a topic
+ */
void processMap(const nav_msgs::msg::OccupancyGrid & new_map);
/**
@@ -84,8 +140,16 @@ private:
* static map are overwritten.
*/
void incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map);
+ /**
+ * @brief Callback to update the costmap's map from the map_server (or SLAM)
+ * with an update in a particular area of the map
+ */
void incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update);
+ /**
+ * @brief Interpret the value in the static map given on the topic to
+ * convert into costs for the costmap to utilize
+ */
unsigned char interpretValue(unsigned char value);
std::string global_frame_; ///< @brief The global frame for the costmap
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
index 376ae45be8ef01325bd3d17399bd482ab91442cf..f2f6635c3133bed567fae9596a556f2a8d736b1c 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
@@ -55,39 +55,90 @@
namespace nav2_costmap_2d
{
+/**
+ * @class VoxelLayer
+ * @brief Takes laser and pointcloud data to populate a 3D voxel representation of the environment
+ */
class VoxelLayer : public ObstacleLayer
{
public:
+ /**
+ * @brief Voxel Layer constructor
+ */
VoxelLayer()
: voxel_grid_(0, 0, 0)
{
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D
}
+ /**
+ * @brief Voxel Layer destructor
+ */
virtual ~VoxelLayer();
+ /**
+ * @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 layer's origin to a new pose, often when in a rolling costmap
+ */
void updateOrigin(double new_origin_x, double new_origin_y);
+
+ /**
+ * @brief If layer is discretely populated
+ */
bool isDiscretized()
{
return true;
}
+
+ /**
+ * @brief Match the size of the master costmap
+ */
virtual void matchSize();
+
+ /**
+ * @brief Reset this costmap
+ */
virtual void reset();
+
+ /**
+ * @brief If clearing operations should be processed on this layer or not
+ */
virtual bool isClearable() {return true;}
protected:
+ /**
+ * @brief Reset internal maps
+ */
virtual void resetMaps();
private:
- void reconfigureCB();
+ /**
+ * @brief Clear any non-lethal cost columns
+ */
void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
+ /**
+ * @brief Use raycasting between 2 points to clear freespace
+ */
virtual void raytraceFreespace(
const nav2_costmap_2d::Observation & clearing_observation,
double * min_x, double * min_y,
@@ -102,6 +153,9 @@ private:
rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::PointCloud2>::SharedPtr
clearing_endpoints_pub_;
+ /**
+ * @brief Covert world coordinates into map coordinates
+ */
inline bool worldToMap3DFloat(
double wx, double wy, double wz, double & mx, double & my,
double & mz)
@@ -119,6 +173,9 @@ private:
return false;
}
+ /**
+ * @brief Covert world coordinates into map coordinates
+ */
inline bool worldToMap3D(
double wx, double wy, double wz, unsigned int & mx, unsigned int & my,
unsigned int & mz)
@@ -138,6 +195,9 @@ private:
return false;
}
+ /**
+ * @brief Covert map coordinates into world coordinates
+ */
inline void mapToWorld3D(
unsigned int mx, unsigned int my, unsigned int mz, double & wx,
double & wy,
@@ -149,11 +209,17 @@ private:
wz = origin_z_ + (mz + 0.5) * z_resolution_;
}
+ /**
+ * @brief Find L2 norm distance in 3D
+ */
inline double dist(double x0, double y0, double z0, double x1, double y1, double z1)
{
return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
}
+ /**
+ * @brief Get the height of the voxel sizes in meters
+ */
double getSizeInMetersZ() const
{
return (size_z_ - 1 + 0.5) * z_resolution_;