path align improvements

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -697,17 +697,21 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
  * @brief Compare to trajectory points to find closest path point along integrated distances
  * @param vec Vect to check
  * @return dist Distance to look for
+ * @return init Starting index to indec from
  */
-inline size_t findClosestPathPt(const std::vector<float> & vec, float dist, size_t init = 0)
+inline size_t findClosestPathPt(
+  const std::vector<float> & vec, const float dist, const size_t init)
 {
-  auto iter = std::lower_bound(vec.begin() + init, vec.end(), dist);
-  if (iter == vec.begin() + init) {
-    return 0;
-  }
-  if (dist - *(iter - 1) < *iter - dist) {
-    return iter - 1 - vec.begin();
+  for (size_t i = init; i != vec.size(); i++) {
+    if (vec[i] > dist) {
+      if (i > 0 && dist - vec[i - 1] < vec[i] - dist) {
+        return i - 1;
+      } else {
+        return i;
+      }
+    }
   }
-  return iter - vec.begin();
+  return vec.size() - 1;
 }
 
 }  // namespace mppi::utils

nav2_mppi_controller/src/critics/path_align_critic.cpp

@@ -118,7 +118,7 @@ void PathAlignCritic::score(CriticData & data)
         dy = P_y(path_pt) - Ty;
         num_samples += 1.0f;
         if (use_path_orientations_) {
-          dyaw = angles::shortest_angular_distance(P_yaw(path_pt), data.trajectories.yaws(t, p));
+          dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(t, p));
           summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw);
         } else {
           summed_path_dist += sqrtf(dx * dx + dy * dy);