Bound closest pose by costmap and turnaround point

nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp

@@ -46,13 +46,15 @@
 #include "nav_2d_utils/conversions.hpp"
 #include "nav_2d_utils/parameters.hpp"
 #include "nav_2d_utils/tf_help.hpp"
+#include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 
 using nav2_util::declare_parameter_if_not_declared;
+using nav2_util::geometry_utils::euclidean_distance;
 
 namespace dwb_core
 {
@@ -434,17 +436,6 @@ DWBLocalPlanner::scoreTrajectory(
   return score;
 }
 
-double
-getSquareDistance(
-  const geometry_msgs::msg::Pose2D & pose_a,
-  const geometry_msgs::msg::Pose2D & pose_b)
-{
-  double x_diff = pose_a.x - pose_b.x;
-  double y_diff = pose_a.y - pose_b.y;
-
-  return x_diff * x_diff + y_diff * y_diff;
-}
-
 nav_2d_msgs::msg::Path2D
 DWBLocalPlanner::transformGlobalPlan(
   const nav_2d_msgs::msg::Pose2DStamped & pose)
@@ -467,47 +458,62 @@ DWBLocalPlanner::transformGlobalPlan(
   nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
   double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) *
     costmap->getResolution() / 2.0;
-  double sq_dist_threshold = dist_threshold * dist_threshold;
+
 
   // If prune_plan is enabled (it is by default) then we want to restrict the
   // plan to distances within that range as well.
-  double sq_prune_dist = prune_distance_ * prune_distance_;
+  double prune_dist = prune_distance_;
 
   // Set the maximum distance we'll include points before getting to the part
   // of the path where the robot is located (the start of the plan). Basically,
   // these are the points the robot has already passed.
-  double sq_transform_start_threshold;
+  double transform_start_threshold;
   if (prune_plan_) {
-    sq_transform_start_threshold = std::min(sq_dist_threshold, sq_prune_dist);
+    transform_start_threshold = std::min(dist_threshold, prune_dist);
   } else {
-    sq_transform_start_threshold = sq_dist_threshold;
+    transform_start_threshold = dist_threshold;
   }
 
   // Set the maximum distance we'll include points after the part of the part of
   // the plan near the robot (the end of the plan). This determines the amount
   // of the plan passed on to the critics
-  double sq_transform_end_threshold;
+  double transform_end_threshold;
   if (shorten_transformed_plan_) {
-    sq_transform_end_threshold = std::min(sq_dist_threshold, sq_prune_dist);
+    transform_end_threshold = std::min(dist_threshold, prune_dist);
   } else {
-    sq_transform_end_threshold = sq_dist_threshold;
+    transform_end_threshold = dist_threshold;
   }
 
-  // Find the first pose in the plan that's less than sq_transform_start_threshold
+  // Find the first pose in the global plan that's further than prune distance
+  // from the robot using integrated distance
+  auto prune_point = nav2_util::geometry_utils::first_element_beyond(
+    global_plan_.poses.begin(), global_plan_.poses.end(), prune_dist);
+
+  // Find the first pose in the plan (upto prune_point) that's less than transform_start_threshold
   // from the robot.
   auto transformation_begin = std::find_if(
-    begin(global_plan_.poses), end(global_plan_.poses),
+    begin(global_plan_.poses), prune_point,
     [&](const auto & global_plan_pose) {
-      return getSquareDistance(robot_pose.pose, global_plan_pose) < sq_transform_start_threshold;
+      return euclidean_distance(robot_pose.pose, global_plan_pose) < transform_start_threshold;
     });
 
-  // Find the first pose in the end of the plan that's further than sq_transform_end_threshold
-  // from the robot
-  auto transformation_end = std::find_if(
-    transformation_begin, end(global_plan_.poses),
-    [&](const auto & global_plan_pose) {
-      return getSquareDistance(robot_pose.pose, global_plan_pose) > sq_transform_end_threshold;
-    });
+  // Find the first pose in the end of the plan that's further than transform_end_threshold
+  // from the robot using integrated distance
+  auto transformation_end =
+    nav2_util::geometry_utils::first_element_beyond(
+    transformation_begin, global_plan_.poses.end(), transform_end_threshold);
+
+  for (auto it = transformation_begin + 1; it != transformation_end - 1; ++it) {
+    double oa_x = it->x - (it - 1 )->x;
+    double oa_y = it->y - (it - 1)->y;
+    double ab_x = (it + 1)->x - it->x;
+    double ab_y = (it + 1)->y - it->y;
+
+    if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) {
+      transformation_end = it;
+      break;
+    }
+  }
 
   // Transform the near part of the global plan into the robot's frame of reference.
   nav_2d_msgs::msg::Path2D transformed_plan;

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -116,7 +116,9 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @param pose pose to transform
    * @return Path in new frame
    */
-  nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose);
+  nav_msgs::msg::Path transformGlobalPlan(
+    const geometry_msgs::msg::PoseStamped & pose,
+    double dist_to_direction_change);
 
   /**
    * @brief Transform a pose to another frame.

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -261,21 +261,17 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
     goal_dist_tol_ = pose_tolerance.position.x;
   }
 
+  double dist_to_direction_change = findDirectionChange(pose);
+
   // Transform path to robot base frame
-  auto transformed_plan = transformGlobalPlan(pose);
+  auto transformed_plan = transformGlobalPlan(pose, dist_to_direction_change);
 
   // Find look ahead distance and point on path and publish
   double lookahead_dist = getLookAheadDistance(speed);
 
-  // Check for reverse driving
-  if (allow_reversing_) {
-    // Cusp check
-    double dist_to_direction_change = findDirectionChange(pose);
-
-    // if the lookahead distance is further than the cusp, use the cusp distance instead
-    if (dist_to_direction_change < lookahead_dist) {
-      lookahead_dist = dist_to_direction_change;
-    }
+  // if the lookahead distance is further than the cusp, use the cusp distance instead
+  if (dist_to_direction_change < lookahead_dist) {
+    lookahead_dist = dist_to_direction_change;
   }
 
   auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan);
@@ -589,7 +585,7 @@ void RegulatedPurePursuitController::setSpeedLimit(
 }
 
 nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
-  const geometry_msgs::msg::PoseStamped & pose)
+  const geometry_msgs::msg::PoseStamped & pose, double dist_to_direction_change)
 {
   if (global_plan_.poses.empty()) {
     throw nav2_core::PlannerException("Received plan with zero length");
@@ -604,22 +600,29 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
   // We'll discard points on the plan that are outside the local costmap
   nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
   const double max_costmap_dim = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY());
-  const double max_transform_dist = max_costmap_dim * costmap->getResolution() / 2.0;
+  const double max_costmap_extent = max_costmap_dim * costmap->getResolution() / 2.0;
+  const double max_transform_dist = std::min(max_costmap_extent, dist_to_direction_change);
+
+  // TODO(aposhian): for long paths, this might get computationally expensive to do all the time,
+  // and we should be able to cache the results of this distance accumulation
+  auto closest_pose_upper_bound =
+    nav2_util::geometry_utils::first_element_beyond(
+    global_plan_.poses.begin(), global_plan_.poses.end(), max_transform_dist);
 
   // First find the closest pose on the path to the robot
+  // bounded by when the path turns around (if it does) so we don't get a pose from a later
+  // portion of the path
   auto transformation_begin =
     nav2_util::geometry_utils::min_by(
-    global_plan_.poses.begin(), global_plan_.poses.end(),
+    global_plan_.poses.begin(), closest_pose_upper_bound,
     [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) {
       return euclidean_distance(robot_pose, ps);
     });
 
-  // Find points definitely outside of the costmap so we won't transform them.
-  auto transformation_end = std::find_if(
-    transformation_begin, end(global_plan_.poses),
-    [&](const auto & global_plan_pose) {
-      return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist;
-    });
+  // Find points up to max_transform_dist so we only transform them.
+  auto transformation_end =
+    nav2_util::geometry_utils::first_element_beyond(
+    transformation_begin, global_plan_.poses.end(), max_transform_dist);
 
   // Lambda to transform a PoseStamped from global frame to local
   auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) {

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -19,6 +19,7 @@
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -86,6 +87,22 @@ inline double euclidean_distance(
   return euclidean_distance(pos1.pose, pos2.pose);
 }
 
+/**
+ * @brief Get the L2 distance between 2 geometry_msgs::Pose2D
+ * @param pos1 First pose
+ * @param pos1 Second pose
+ * @return double L2 distance
+ */
+inline double euclidean_distance(
+  const geometry_msgs::msg::Pose2D & pos1,
+  const geometry_msgs::msg::Pose2D & pos2)
+{
+  double dx = pos1.x - pos2.x;
+  double dy = pos1.y - pos2.y;
+
+  return hypot(dx, dy);
+}
+
 /**
  * Find element in iterator with the minimum calculated value
  */
@@ -107,6 +124,25 @@ inline Iter min_by(Iter begin, Iter end, Getter getCompareVal)
   return lowest_it;
 }
 
+/**
+ * Find first element in iterator that is greater integrated distance than comparevalue
+ */
+template<typename Iter, typename Getter>
+inline Iter first_element_beyond(Iter begin, Iter end, Getter getCompareVal)
+{
+  if (begin == end) {
+    return end;
+  }
+  Getter dist = 0.0;
+  for (Iter it = begin; it != end - 1; it++) {
+    dist += euclidean_distance(*it, *(it + 1));
+    if (dist > getCompareVal) {
+      return it + 1;
+    }
+  }
+  return end;
+}
+
 /**
  * @brief Calculate the length of the provided path, starting at the provided index
  * @param path Path containing the poses that are planned