Add collision checking for footprint without using subscibers. (#1703)

* Add collision checking for footprint without using subscibers.

* Address reviewer's comments

- Changed the design if the footprint collision checkers
- And propogate the changes to dependencies such as nav2_recoveries and nav2_core

* Remove some extra headers

* Remove debuging code

* Add requested test

* Change weird test names.

* Remove unorientFootprint function dependency

* Imporve tests

* Fix commented Varible

nav2_core/include/nav2_core/recovery.hpp

@@ -21,7 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "tf2_ros/buffer.h"
-#include "nav2_costmap_2d/collision_checker.hpp"
+#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 
 namespace nav2_core
 {
@@ -49,7 +49,7 @@ class Recovery
   virtual void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
     const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
-    std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker) = 0;
+    std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker) = 0;
 
   /**
    * @brief Method to cleanup resources used on shutdown.

nav2_costmap_2d/CMakeLists.txt

@@ -47,6 +47,7 @@ add_library(nav2_costmap_2d_core SHARED
   src/costmap_layer.cpp
   src/observation_buffer.cpp
   src/clear_costmap_service.cpp
+  src/footprint_collision_checker.cpp
 )
 
 # prevent pluginlib from using boost
@@ -94,7 +95,7 @@ target_link_libraries(layers
 add_library(nav2_costmap_2d_client SHARED
   src/footprint_subscriber.cpp
   src/costmap_subscriber.cpp
-  src/collision_checker.cpp
+  src/costmap_topic_collision_checker.cpp
 )
 
 ament_target_dependencies(nav2_costmap_2d_client

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp → nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp

@@ -11,9 +11,11 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
+//
+// Modified by: Shivang Patel (shivaan14@gmail.com)
 
-#ifndef NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_
-#define NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_
+#ifndef NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_
+#define NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_
 
 #include <string>
 #include <vector>
@@ -24,6 +26,7 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose2_d.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "nav2_costmap_2d/footprint_subscriber.hpp"
 #include "nav2_util/robot_utils.hpp"
@@ -34,41 +37,36 @@
 
 namespace nav2_costmap_2d
 {
-typedef std::vector<geometry_msgs::msg::Point> Footprint;
 
-class CollisionChecker
+class CostmapTopicCollisionChecker
 {
 public:
-  CollisionChecker(
+  CostmapTopicCollisionChecker(
     CostmapSubscriber & costmap_sub,
     FootprintSubscriber & footprint_sub,
     tf2_ros::Buffer & tf,
     std::string name = "collision_checker",
     std::string global_frame = "map");
 
-  ~CollisionChecker();
+  ~CostmapTopicCollisionChecker() = default;
 
   // Returns the obstacle footprint score for a particular pose
   double scorePose(const geometry_msgs::msg::Pose2D & pose);
   bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose);
 
 protected:
-  double lineCost(int x0, int x1, int y0, int y1) const;
-  double pointCost(int x, int y) const;
   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_;
 
   // Name used for logging
   std::string name_;
   std::string global_frame_;
   tf2_ros::Buffer & tf_;
   CostmapSubscriber & costmap_sub_;
   FootprintSubscriber & footprint_sub_;
+  FootprintCollisionChecker collision_checker_;
 };
+
 }  // namespace nav2_costmap_2d
 
-#endif  // NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_
+#endif  // NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp

@@ -0,0 +1,53 @@
+// Copyright (c) 2019 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Modified by: Shivang Patel (shivaang14@gmail.com)
+
+#ifndef NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
+#define NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_util/robot_utils.hpp"
+
+namespace nav2_costmap_2d
+{
+typedef std::vector<geometry_msgs::msg::Point> Footprint;
+
+class FootprintCollisionChecker
+{
+public:
+  FootprintCollisionChecker();
+  explicit FootprintCollisionChecker(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap);
+  double footprintCost(const Footprint footprint);
+  double footprintCostAtPose(double x, double y, double theta, const Footprint footprint);
+  double lineCost(int x0, int x1, int y0, int y1) const;
+  bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my);
+  double pointCost(int x, int y) const;
+  void setCostmap(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap);
+
+private:
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_

nav2_costmap_2d/src/collision_checker.cpp → nav2_costmap_2d/src/costmap_topic_collision_checker.cpp

@@ -11,13 +11,16 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
+//
+// Modified by: Shivang Patel (shivaan14@gmail.com)
 
 #include <memory>
 #include <string>
 #include <vector>
 #include <algorithm>
+#include <iostream>
 
-#include "nav2_costmap_2d/collision_checker.hpp"
+#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_costmap_2d/exceptions.hpp"
@@ -29,7 +32,7 @@ using namespace std::chrono_literals;
 namespace nav2_costmap_2d
 {
 
-CollisionChecker::CollisionChecker(
+CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
   CostmapSubscriber & costmap_sub,
   FootprintSubscriber & footprint_sub,
   tf2_ros::Buffer & tf,
@@ -39,15 +42,12 @@ CollisionChecker::CollisionChecker(
   global_frame_(global_frame),
   tf_(tf),
   costmap_sub_(costmap_sub),
-  footprint_sub_(footprint_sub)
+  footprint_sub_(footprint_sub),
+  collision_checker_(nullptr)
 {
 }
 
-CollisionChecker::~CollisionChecker()
-{
-}
-
-bool CollisionChecker::isCollisionFree(
+bool CostmapTopicCollisionChecker::isCollisionFree(
   const geometry_msgs::msg::Pose2D & pose)
 {
   try {
@@ -67,33 +67,25 @@ bool CollisionChecker::isCollisionFree(
   }
 }
 
-double CollisionChecker::scorePose(
+double CostmapTopicCollisionChecker::scorePose(
   const geometry_msgs::msg::Pose2D & pose)
 {
   try {
-    costmap_ = costmap_sub_.getCostmap();
+    collision_checker_.setCostmap(costmap_sub_.getCostmap());
   } catch (const std::runtime_error & e) {
     throw CollisionCheckerException(e.what());
   }
 
   unsigned int cell_x, cell_y;
-  if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
+  if (!collision_checker_.worldToMap(pose.x, pose.y, cell_x, cell_y)) {
     RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", cell_x, cell_y);
     throw IllegalPoseException(name_, "Pose Goes Off Grid.");
   }
 
-  return footprintCost(getFootprint(pose));
+  return collision_checker_.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 CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose)
 {
   Footprint footprint;
   if (!footprint_sub_.getFootprint(footprint)) {
@@ -107,68 +99,7 @@ Footprint CollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose
   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 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
-    worldToMap(footprint[i].x, footprint[i].y, x0, y0);
-
-    // get the cell coord of the second point
-    worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1);
-
-    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
-  worldToMap(footprint.back().x, footprint.back().y, x0, y0);
-
-  // get the cell coord of the first point
-  worldToMap(footprint.front().x, footprint.front().y, x1, y1);
-
-  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;
-}
-
-double CollisionChecker::lineCost(int x0, int x1, int y0, int y1) const
-{
-  double line_cost = 0.0;
-  double point_cost = -1.0;
-
-  for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) {
-    point_cost = pointCost(line.getX(), line.getY());   // Score the current point
-
-    if (line_cost < point_cost) {
-      line_cost = point_cost;
-    }
-  }
-
-  return line_cost;
-}
-
-double CollisionChecker::pointCost(int x, int y) const
-{
-  unsigned char cost = costmap_->getCost(x, y);
-  // if the cell is in an obstacle the path is invalid or unknown
-  if (cost == LETHAL_OBSTACLE) {
-    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x, y);
-    throw IllegalPoseException(name_, "Footprint Hits Obstacle.");
-  } else if (cost == NO_INFORMATION) {
-    RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x, y);
-    throw IllegalPoseException(name_, "Footprint Hits Unknown Region.");
-  }
-
-  return cost;
-}
-
-void CollisionChecker::unorientFootprint(
+void CostmapTopicCollisionChecker::unorientFootprint(
   const std::vector<geometry_msgs::msg::Point> & oriented_footprint,
   std::vector<geometry_msgs::msg::Point> & reset_footprint)
 {
@@ -186,4 +117,5 @@ void CollisionChecker::unorientFootprint(
   transformFootprint(0, 0, -theta, temp, reset_footprint);
 }
 
+
 }  // namespace nav2_costmap_2d