From c87f7813526e5157c33f1bc270a0262173690423 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Thu, 14 Mar 2024 16:10:30 -0700
Subject: [PATCH] updates

---
 .../nav2_mppi_controller/tools/utils.hpp      |  4 +--
 .../src/critics/cost_critic.cpp               | 36 +++++++++----------
 .../src/critics/path_angle_critic.cpp         |  4 +--
 3 files changed, 22 insertions(+), 22 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
index b21125f1a4..fe1d2aa794 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
@@ -270,8 +270,8 @@ inline bool withinPositionGoalTolerance(
 template<typename T>
 auto normalize_angles(const T & angles)
 {
-  auto && theta = xt::eval(xt::fmod(angles + M_PIF, 2.0f * M_PIF));
-  return xt::eval(xt::where(theta <= 0.0f, theta + M_PIF, theta - M_PIF));
+  auto theta = xt::eval(xt::fmod(angles + M_PIF, 2.0f * M_PIF));
+  return xt::eval(xt::where(theta < 0.0f, theta + M_PIF, theta - M_PIF));
 }
 
 /**
diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
index 3a4ae41c44..729528dd41 100644
--- a/nav2_mppi_controller/src/critics/cost_critic.cpp
+++ b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -117,31 +117,31 @@ void CostCritic::score(CriticData & data)
   }
 
   auto && repulsive_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
-
-  unsigned int x_i = 0u, y_i = 0u;
-  float pose_cost, Tx, Ty;
+  bool all_trajectories_collide = true;
 
   const size_t traj_len = floor(data.trajectories.x.shape(1) / trajectory_point_step_);
-  bool all_trajectories_collide = true;
+  const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::range(0, _, trajectory_point_step_));
+  const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::range(0, _, trajectory_point_step_));
+  const auto traj_yaw = xt::view(
+    data.trajectories.yaws, xt::all(), xt::range(0, _, trajectory_point_step_));
+
   for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) {
     bool trajectory_collide = false;
-    const auto traj_x = xt::view(data.trajectories.x, i, xt::range(0, _, trajectory_point_step_));
-    const auto traj_y = xt::view(data.trajectories.y, i, xt::range(0, _, trajectory_point_step_));
-    const auto traj_yaw = xt::view(
-      data.trajectories.yaws, i, xt::range(0, _, trajectory_point_step_));
-    pose_cost = 0.0f;
-    repulsive_cost[i] = 0.0f;
+    float pose_cost = 0.0f;
+    float & traj_cost = repulsive_cost[i];
+    traj_cost = 0.0f;
 
-    for (size_t j = 0; j < traj_x.shape(0); j++) {
-      Tx = traj_x(j);
-      Ty = traj_y(j);
+    for (size_t j = 0; j < traj_len; j++) {
+      float Tx = traj_x(i, j);
+      float Ty = traj_y(i, j);
+      unsigned int x_i = 0u, y_i = 0u;
 
       // The getCost doesn't use orientation
       // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it
       // So the center point has more information than the footprint
       if (!worldToMapFloat(Tx, Ty, x_i, y_i)) {
         if (!is_tracking_unknown_) {
-          repulsive_cost[i] = collision_cost_;
+          traj_cost = collision_cost_;
           trajectory_collide = true;
           break;
         }
@@ -151,8 +151,8 @@ void CostCritic::score(CriticData & data)
         if (pose_cost < 1.0f) {
           continue;  // In free space
         }
-        if (inCollision(pose_cost, Tx, Ty, traj_yaw(j))) {
-          repulsive_cost[i] = collision_cost_;
+        if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
+          traj_cost = collision_cost_;
           trajectory_collide = true;
           break;
         }
@@ -162,9 +162,9 @@ void CostCritic::score(CriticData & data)
       // Note that we collision check based on the footprint actual,
       // but score based on the center-point cost regardless
       if (pose_cost >= 253.0f /*INSCRIBED_INFLATED_OBSTACLE in float*/) {
-        repulsive_cost[i] += critical_cost_;
+        traj_cost += critical_cost_;
       } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
-        repulsive_cost[i] += pose_cost;
+        traj_cost += pose_cost;
       }
     }
 
diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
index f0a2a3968c..491adc4580 100644
--- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp
+++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
@@ -109,9 +109,9 @@ void PathAngleCritic::score(CriticData & data)
           xt::abs(utils::shortest_angular_distance(
             xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points));
         if (power_ > 1u) {
-          data.costs += xt::pow(yaws * weight_, power_);
+          data.costs += xt::pow(std::move(yaws) * weight_, power_);
         } else {
-          data.costs += yaws * weight_;
+          data.costs += std::move(yaws) * weight_;
         }
         return;
       }