bounds to find path closest point for NHP

README.md

@@ -36,7 +36,7 @@ pip install conan
  | Parameter             | Type   | Definition                                                                                                                                                                                                                                                        |
  | --------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
  | iteration_count       | int    | Iteration count in MPPI algorithm                                                                                                                                                                                                                                 |
- | lookahead_dist        | double | Max lenght of the global plan that considered by local planner                                                                                                                                                                                                    |
+ | max_robot_pose_search_dist | double | Upper bound on integrated distance along the global plan to search for the closest pose to the robot pose. This should be left as the default unless there are paths with loops and intersections that do not leave the local costmap, in which case making this value smaller is necessary to prevent shortcutting. |
  | transform_tolerance   | double | TF tolerance to transform poses                                                                                                                                                                                                                                   |
  | batch_size            | int    | Count of randomly sampled trajectories                                                                                                                                                                                                                            |
  | time_steps            | int    | Number of time steps (points) in each sampled trajectory                                                                                                                                                                                                          |

include/mppic/path_handler.hpp

@@ -7,6 +7,7 @@
 #include "nav_msgs/msg/path.hpp"
 #include "std_msgs/msg/header.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/geometry_utils.hpp"
 
 namespace mppi
 {
@@ -61,7 +62,7 @@ class PathHandler
   nav_msgs::msg::Path global_plan_;
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 
-  double lookahead_dist_{0};
+  double max_robot_pose_search_dist_{0};
   double transform_tolerance_{0};
 };
 } // namespace mppi

src/path_handler.cpp

@@ -5,6 +5,8 @@
 namespace mppi
 {
 
+using nav2_util::geometry_utils::euclidean_distance;
+
 void PathHandler::initialize(
   rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap, std::shared_ptr<tf2_ros::Buffer> buffer)
@@ -16,7 +18,7 @@ void PathHandler::initialize(
   logger_ = node->get_logger();
 
   auto getParam = utils::getParamGetter(node, name_);
-  getParam(lookahead_dist_, "lookahead_dist", 1.0);
+  getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist());
   getParam(transform_tolerance_, "transform_tolerance", 0.1);
 }
 
@@ -26,21 +28,23 @@ auto PathHandler::getGlobalPlanConsideringBounds(
   auto begin = global_plan_.poses.begin();
   auto end = global_plan_.poses.end();
 
+  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_);
+
   // Find closest point to the robot
-  auto closest_point = std::min_element(
-    begin, end,
-    [&global_pose](
-      const geometry_msgs::msg::PoseStamped & lhs, const geometry_msgs::msg::PoseStamped & rhs) {
-      return utils::hypot(lhs, global_pose) < utils::hypot(rhs, global_pose);
+  auto closest_point = nav2_util::geometry_utils::min_by(
+    begin, closest_pose_upper_bound,
+    [&global_pose](const geometry_msgs::msg::PoseStamped & ps) {
+      return euclidean_distance(global_pose, ps);
     });
 
   // Find the furthest relevent point on the path to consider within costmap bounds
   auto max_costmap_dist = getMaxCostmapDist();
   auto last_point =
     std::find_if(
     closest_point, end, [&](const geometry_msgs::msg::PoseStamped & global_plan_pose) {
-      auto dist = utils::hypot(global_pose, global_plan_pose);
-      return dist > max_costmap_dist || dist > lookahead_dist_;
+      return euclidean_distance(global_pose, global_plan_pose) > max_costmap_dist;
     });
 
   return std::tuple{closest_point, last_point};