reverting bdafb48 from galactic (#2773)

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -21,7 +21,6 @@
 #include <memory>
 #include <algorithm>
 
-#include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_core/controller.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "pluginlib/class_loader.hpp"
@@ -184,16 +183,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller
     const double &, const double &);
 
   /**
-   * @brief checks for collision at projected pose
+   * @brief Whether point is in collision
    * @param x Pose of pose x
    * @param y Pose of pose y
    * @param theta orientation of Yaw
    * @return Whether in collision
    */
-  bool inCollision(
-    const double & x,
-    const double & y,
-    const double & theta);
+  bool inCollision(const double & x, const double & y);
+
   /**
    * @brief Cost at a point
    * @param x Pose of pose x
@@ -268,8 +265,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
   carrot_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
-  std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
-  collision_checker_;
 };
 
 }  // namespace nav2_regulated_pure_pursuit_controller

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -172,11 +172,6 @@ void RegulatedPurePursuitController::configure(
   global_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("received_global_plan", 1);
   carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>("lookahead_point", 1);
   carrot_arc_pub_ = node->create_publisher<nav_msgs::msg::Path>("lookahead_collision_arc", 1);
-
-  // initialize collision checker and set costmap
-  collision_checker_ = std::make_unique<nav2_costmap_2d::
-      FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(costmap_);
-  collision_checker_->setCostmap(costmap_);
 }
 
 void RegulatedPurePursuitController::cleanup()
@@ -382,10 +377,7 @@ bool RegulatedPurePursuitController::isCollisionImminent(
   // odom frame and the carrot_pose is in robot base frame.
 
   // check current point is OK
-  if (inCollision(
-      robot_pose.pose.position.x, robot_pose.pose.position.y,
-      tf2::getYaw(robot_pose.pose.orientation)))
-  {
+  if (inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y)) {
     return true;
   }
 
@@ -404,9 +396,13 @@ bool RegulatedPurePursuitController::isCollisionImminent(
   curr_pose.y = robot_pose.pose.position.y;
   curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation);
 
-  // only forward simulate within time requested
   int i = 1;
-  while (i * projection_time < max_allowed_time_to_collision_) {
+  while (true) {
+    // only forward simulate within time requested
+    if (i * projection_time > max_allowed_time_to_collision_) {
+      break;
+    }
+
     i++;
 
     // apply velocity at curr_pose over distance
@@ -420,8 +416,8 @@ bool RegulatedPurePursuitController::isCollisionImminent(
     pose_msg.pose.position.z = 0.01;
     arc_pts_msg.poses.push_back(pose_msg);
 
-    // check for collision at the projected pose
-    if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) {
+    // check for collision at this point
+    if (inCollision(curr_pose.x, curr_pose.y)) {
       carrot_arc_pub_->publish(arc_pts_msg);
       return true;
     }
@@ -432,10 +428,7 @@ bool RegulatedPurePursuitController::isCollisionImminent(
   return false;
 }
 
-bool RegulatedPurePursuitController::inCollision(
-  const double & x,
-  const double & y,
-  const double & theta)
+bool RegulatedPurePursuitController::inCollision(const double & x, const double & y)
 {
   unsigned int mx, my;
 
@@ -448,16 +441,13 @@ bool RegulatedPurePursuitController::inCollision(
     return false;
   }
 
-  double footprint_cost = collision_checker_->footprintCostAtPose(
-    x, y, theta, costmap_ros_->getRobotFootprint());
-  if (footprint_cost == static_cast<double>(NO_INFORMATION) &&
-    costmap_ros_->getLayeredCostmap()->isTrackingUnknown())
-  {
-    return false;
-  }
+  unsigned char cost = costmap_->getCost(mx, my);
 
-  // if occupied or unknown and not to traverse unknown space
-  return footprint_cost >= static_cast<double>(LETHAL_OBSTACLE);
+  if (costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) {
+    return cost >= INSCRIBED_INFLATED_OBSTACLE && cost != NO_INFORMATION;
+  } else {
+    return cost >= INSCRIBED_INFLATED_OBSTACLE;
+  }
 }
 
 double RegulatedPurePursuitController::costAtPose(const double & x, const double & y)