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
 {
@@ -87,7 +89,7 @@ void DWBLocalPlanner::configure(
     rclcpp::ParameterValue(true));
   declare_parameter_if_not_declared(
     node, dwb_plugin_name_ + ".prune_distance",
-    rclcpp::ParameterValue(1.0));
+    rclcpp::ParameterValue(2.0));
   declare_parameter_if_not_declared(
     node, dwb_plugin_name_ + ".debug_trajectory_details",
     rclcpp::ParameterValue(false));
@@ -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,46 +458,51 @@ 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_after_integrated_distance(
+    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
+  // 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 = 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;
+    transformation_begin, global_plan_.poses.end(),
+    [&](const auto & pose) {
+      return euclidean_distance(pose, robot_pose.pose) > transform_end_threshold;
     });
 
   // Transform the near part of the global plan into the robot's frame of reference.

nav2_regulated_pure_pursuit_controller/README.md

@@ -77,6 +77,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
 | `use_rotate_to_heading` | Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | 
 | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | 
 | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | 
+| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | 
 
 Example fully-described XML with default parameter values:
 
@@ -123,6 +124,7 @@ controller_server:
       use_rotate_to_heading: true
       rotate_to_heading_min_angle: 0.785
       max_angular_accel: 3.2
+      max_robot_pose_search_dist: 10.0
       cost_scaling_dist: 0.3
       cost_scaling_gain: 1.0
       inflation_cost_scaling_factor: 3.0

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -112,11 +112,14 @@ class RegulatedPurePursuitController : public nav2_core::Controller
 
 protected:
   /**
-   * @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses
+   * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint
+   * Points ineligible to be selected as a lookahead point if they are any of the following:
+   * - Outside the local_costmap (collision avoidance cannot be assured)
    * @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);
 
   /**
    * @brief Transform a pose to another frame.
@@ -232,7 +235,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller
    * @param pose Pose input to determine the cusp position
    * @return robot distance from the cusp
    */
-  double findDirectionChange(const geometry_msgs::msg::PoseStamped & pose);
+  double findVelocitySignChange(const geometry_msgs::msg::PoseStamped & pose);
+
+  /**
+   * Get the greatest extent of the costmap in meters from the center.
+   * @return max of distance from center in meters to edge of costmap
+   */
+  double getCostmapMaxExtent() const;
 
   /**
    * @brief Callback executed when a parameter change is detected
@@ -272,6 +281,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   double rotate_to_heading_min_angle_;
   double goal_dist_tol_;
   bool allow_reversing_;
+  double max_robot_pose_search_dist_;
 
   nav_msgs::msg::Path global_plan_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -105,6 +105,9 @@ void RegulatedPurePursuitController::configure(
     node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".max_robot_pose_search_dist",
+    rclcpp::ParameterValue(getCostmapMaxExtent()));
 
   node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_);
   base_desired_linear_vel_ = desired_linear_vel_;
@@ -147,6 +150,9 @@ void RegulatedPurePursuitController::configure(
   node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_);
   node->get_parameter(plugin_name_ + ".allow_reversing", allow_reversing_);
   node->get_parameter("controller_frequency", control_frequency);
+  node->get_parameter(
+    plugin_name_ + ".max_robot_pose_search_dist",
+    max_robot_pose_search_dist_);
 
   transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
   control_duration_ = 1.0 / control_frequency;
@@ -232,7 +238,8 @@ std::unique_ptr<geometry_msgs::msg::PointStamped> RegulatedPurePursuitController
   return carrot_msg;
 }
 
-double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed)
+double RegulatedPurePursuitController::getLookAheadDistance(
+  const geometry_msgs::msg::Twist & speed)
 {
   // If using velocity-scaled look ahead distances, find and clamp the dist
   // Else, use the static look ahead distance
@@ -270,11 +277,11 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   // Check for reverse driving
   if (allow_reversing_) {
     // Cusp check
-    double dist_to_direction_change = findDirectionChange(pose);
+    double dist_to_cusp = findVelocitySignChange(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 (dist_to_cusp < lookahead_dist) {
+      lookahead_dist = dist_to_cusp;
     }
   }
 
@@ -602,23 +609,27 @@ 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;
+  double max_costmap_extent = getCostmapMaxExtent();
+
+  auto closest_pose_upper_bound =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_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.
+  // Find points up to max_transform_dist so we only 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;
+    transformation_begin, global_plan_.poses.end(),
+    [&](const auto & pose) {
+      return euclidean_distance(pose, robot_pose) > max_costmap_extent;
     });
 
   // Lambda to transform a PoseStamped from global frame to local
@@ -652,7 +663,7 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
   return transformed_plan;
 }
 
-double RegulatedPurePursuitController::findDirectionChange(
+double RegulatedPurePursuitController::findVelocitySignChange(
   const geometry_msgs::msg::PoseStamped & pose)
 {
   // Iterating through the global path to determine the position of the cusp
@@ -700,6 +711,13 @@ bool RegulatedPurePursuitController::transformPose(
   return false;
 }
 
+double RegulatedPurePursuitController::getCostmapMaxExtent() const
+{
+  const double max_costmap_dim_meters = std::max(
+    costmap_->getSizeInMetersX(), costmap_->getSizeInMetersX());
+  return max_costmap_dim_meters / 2.0;
+}
+
 
 rcl_interfaces::msg::SetParametersResult
 RegulatedPurePursuitController::dynamicParametersCallback(

nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt

@@ -1,10 +1,15 @@
 # tests for regulated PP
 ament_add_gtest(test_regulated_pp
   test_regulated_pp.cpp
+  path_utils/path_utils.cpp
 )
 ament_target_dependencies(test_regulated_pp
   ${dependencies}
 )
 target_link_libraries(test_regulated_pp
   ${library_name}
 )
+
+# Path utils test
+ament_add_gtest(test_path_utils path_utils/test_path_utils.cpp path_utils/path_utils.cpp)
+ament_target_dependencies(test_path_utils nav_msgs geometry_msgs)

nav2_regulated_pure_pursuit_controller/test/path_utils/path_utils.cpp

@@ -0,0 +1,88 @@
+// Copyright (c) 2022 Adam Aposhian
+//
+// 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.
+
+#include <memory>
+
+#include "path_utils.hpp"
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace path_utils
+{
+
+void append_transform_to_path(
+  nav_msgs::msg::Path & path,
+  tf2::Transform & relative_transform)
+{
+  // Add a new empty pose
+  path.poses.emplace_back();
+  // Get the previous, last pose (after the emplace_back so the reference isn't invalidated)
+  auto & previous_pose = *(path.poses.end() - 2);
+  auto & new_pose = path.poses.back();
+
+  // get map_transform of previous_pose
+  tf2::Transform map_transform;
+  tf2::fromMsg(previous_pose.pose, map_transform);
+
+  tf2::Transform full_transform;
+  full_transform.mult(map_transform, relative_transform);
+
+  tf2::toMsg(full_transform, new_pose.pose);
+  new_pose.header.frame_id = previous_pose.header.frame_id;
+}
+
+void Straight::append(nav_msgs::msg::Path & path, double spacing) const
+{
+  auto num_points = std::floor(length_ / spacing);
+  path.poses.reserve(path.poses.size() + num_points);
+  tf2::Transform translation(tf2::Quaternion::getIdentity(), tf2::Vector3(spacing, 0.0, 0.0));
+  for (size_t i = 1; i <= num_points; ++i) {
+    append_transform_to_path(path, translation);
+  }
+}
+
+double chord_length(double radius, double radians)
+{
+  return 2 * radius * sin(radians / 2);
+}
+
+void Arc::append(nav_msgs::msg::Path & path, double spacing) const
+{
+  double length = radius_ * std::abs(radians_);
+  size_t num_points = std::floor(length / spacing);
+  double radians_per_step = radians_ / num_points;
+  tf2::Transform transform(
+    tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), radians_per_step),
+    tf2::Vector3(chord_length(radius_, std::abs(radians_per_step)), 0.0, 0.0));
+  path.poses.reserve(path.poses.size() + num_points);
+  for (size_t i = 0; i < num_points; ++i) {
+    append_transform_to_path(path, transform);
+  }
+}
+
+nav_msgs::msg::Path generate_path(
+  geometry_msgs::msg::PoseStamped start,
+  double spacing,
+  std::initializer_list<std::unique_ptr<PathSegment>> segments)
+{
+  nav_msgs::msg::Path path;
+  path.header = start.header;
+  path.poses.push_back(start);
+  for (const auto & segment : segments) {
+    segment->append(path, spacing);
+  }
+  return path;
+}
+
+}  // namespace path_utils