diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp
index fbbd31b6b3a4f7b1d9edbe71619cddeb5bfbe5cc..5ce782506cfafbdd208dde03c8f0da21b0fd80a0 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp
@@ -52,6 +52,9 @@ protected:
double pointCost(int x, int y) const;
bool getRobotPose(geometry_msgs::msg::Pose & current_pose);
void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint);
+ void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
+ Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose);
+ double footprintCost(const Footprint footprint);
std::shared_ptr<Costmap2D> costmap_;
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 de5066d5f913878eeb1cb43cd9f6e71597b6d173..f4d2df468debba7f5dbb447f06d1df57cc02a37f 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp
@@ -53,7 +53,7 @@ protected:
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
void toCostmap2D();
- void costmap_callback(const nav2_msgs::msg::Costmap::SharedPtr msg);
+ void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg);
std::shared_ptr<Costmap2D> costmap_;
nav2_msgs::msg::Costmap::SharedPtr costmap_msg_;
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp
index 91891264b5c235471aaaaf500672281920d3e645..0646555dadc2236d2b08c27f4b8b90d4e17808fa 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/exceptions.hpp
@@ -46,7 +46,6 @@ class CollisionCheckerException : public std::runtime_error
public:
explicit CollisionCheckerException(const std::string description)
: std::runtime_error(description) {}
- typedef std::shared_ptr<CollisionCheckerException> Ptr;
};
/**
diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp
index 252868d305df868f977b939d7d8c8222d689fd41..d9cf3a201d31072b82e6e772084fd313cb207793 100644
--- a/nav2_costmap_2d/src/collision_checker.cpp
+++ b/nav2_costmap_2d/src/collision_checker.cpp
@@ -67,6 +67,19 @@ double CollisionChecker::scorePose(
throw IllegalPoseException(name_, "Pose Goes Off Grid.");
}
+ return footprintCost(getFootprint(pose));
+}
+
+void CollisionChecker::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my)
+{
+ if (!costmap_->worldToMap(wx, wy, mx, my)) {
+ RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", mx, my);
+ throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
+ }
+}
+
+Footprint CollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose)
+{
Footprint footprint;
if (!footprint_sub_.getFootprint(footprint)) {
throw CollisionCheckerException("Footprint not available.");
@@ -76,44 +89,34 @@ double CollisionChecker::scorePose(
unorientFootprint(footprint, footprint_spec);
transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint);
+ return footprint;
+}
+
+double CollisionChecker::footprintCost(const Footprint footprint)
+{
// now we really have to lay down the footprint in the costmap_ grid
unsigned int x0, x1, y0, y1;
- double line_cost = 0.0;
double footprint_cost = 0.0;
// we need to rasterize each line in the footprint
for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
// get the cell coord of the first point
- if (!costmap_->worldToMap(footprint[i].x, footprint[i].y, x0, y0)) {
- RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x0, y0);
- throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
- }
+ worldToMap(footprint[i].x, footprint[i].y, x0, y0);
// get the cell coord of the second point
- if (!costmap_->worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) {
- RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x1, y1);
- throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
- }
+ worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1);
- line_cost = lineCost(x0, x1, y0, y1);
- footprint_cost = std::max(line_cost, footprint_cost);
+ footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
}
// we also need to connect the first point in the footprint to the last point
// get the cell coord of the last point
- if (!costmap_->worldToMap(footprint.back().x, footprint.back().y, x0, y0)) {
- RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x0, y0);
- throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
- }
+ worldToMap(footprint.back().x, footprint.back().y, x0, y0);
// get the cell coord of the first point
- if (!costmap_->worldToMap(footprint.front().x, footprint.front().y, x1, y1)) {
- RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x1, y1);
- throw IllegalPoseException(name_, "Footprint Goes Off Grid.");
- }
+ worldToMap(footprint.front().x, footprint.front().y, x1, y1);
- line_cost = lineCost(x0, x1, y0, y1);
- footprint_cost = std::max(line_cost, footprint_cost);
+ footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost);
// if all line costs are legal... then we can return that the footprint is legal
return footprint_cost;
diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
index fc9490886d73e2e74e00ce868531a38fbb188c88..abc5ccb7b442155345c28bf4beefa27727243864 100644
--- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp
+++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp
@@ -57,11 +57,11 @@ Costmap2DPublisher::Costmap2DPublisher(
auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
// TODO(bpwilcox): port onNewSubscription functionality for publisher
- costmap_pub_ = ros_node->create_publisher<nav_msgs::msg::OccupancyGrid>(topic_name,
+ costmap_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(topic_name,
custom_qos);
- costmap_raw_pub_ = ros_node->create_publisher<nav2_msgs::msg::Costmap>(topic_name + "_raw",
+ costmap_raw_pub_ = node_->create_publisher<nav2_msgs::msg::Costmap>(topic_name + "_raw",
custom_qos);
- costmap_update_pub_ = ros_node->create_publisher<map_msgs::msg::OccupancyGridUpdate>(
+ costmap_update_pub_ = node_->create_publisher<map_msgs::msg::OccupancyGridUpdate>(
topic_name + "_updates", custom_qos);
if (cost_translation_table_ == NULL) {
diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp
index 1e7dcec46a1d57262c2e20646ceb1668cc2ad353..8f6af0d7a9772fc3eb8421ad19e81ac82be25a4d 100644
--- a/nav2_costmap_2d/src/costmap_subscriber.cpp
+++ b/nav2_costmap_2d/src/costmap_subscriber.cpp
@@ -49,7 +49,7 @@ CostmapSubscriber::CostmapSubscriber(
{
costmap_sub_ = rclcpp::create_subscription<nav2_msgs::msg::Costmap>(node_topics_, topic_name_,
rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
- std::bind(&CostmapSubscriber::costmap_callback, this, std::placeholders::_1));
+ std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1));
}
std::shared_ptr<Costmap2D> CostmapSubscriber::getCostmap()
@@ -91,7 +91,7 @@ void CostmapSubscriber::toCostmap2D()
}
}
-void CostmapSubscriber::costmap_callback(const nav2_msgs::msg::Costmap::SharedPtr msg)
+void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg)
{
costmap_msg_ = msg;
if (!costmap_received_) {