removing some jitter

ros-navigation · Mar 14, 2024 · 18288e9 · 18288e9
1 parent eff04a3
commit 18288e9
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 19 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
@@ -318,7 +318,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data)
     min_id_by_path = 0;
     min_distance_by_path = std::numeric_limits<float>::max();
     for (size_t j = max_id_by_trajectories; j < dists.shape(1); j++) {
-      const float & cur_dist = dists(i, j);
+      const float cur_dist = dists(i, j);
       if (cur_dist < min_distance_by_path) {
         min_distance_by_path = cur_dist;
         min_id_by_path = j;

diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp
@@ -52,13 +52,13 @@ void ConstraintCritic::score(CriticData & data)
     if (power_ > 1u) {
       data.costs += xt::pow(
         xt::sum(
-          (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) +
-          std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f))) *
+          (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f) +
+          xt::maximum(min_vel_ - data.state.vx, 0.0f))) *
           data.model_dt, {1}, immediate) * weight_, power_);
     } else {
       data.costs += xt::sum(
-          (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) +
-          std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f))) *
+          (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f) +
+          xt::maximum(min_vel_ - data.state.vx, 0.0f))) *
           data.model_dt, {1}, immediate) * weight_;
     }
     return;
@@ -72,13 +72,13 @@ void ConstraintCritic::score(CriticData & data)
     if (power_ > 1u) {
       data.costs += xt::pow(
         xt::sum(
-          (std::move(xt::maximum(vel_total - max_vel_, 0.0f)) +
-          std::move(xt::maximum(min_vel_ - vel_total, 0.0f))) *
+          (std::move(xt::maximum(vel_total - max_vel_, 0.0f) +
+          xt::maximum(min_vel_ - vel_total, 0.0f))) *
           data.model_dt, {1}, immediate) * weight_, power_);
     } else {
       data.costs += xt::sum(
-          (std::move(xt::maximum(vel_total - max_vel_, 0.0f)) +
-          std::move(xt::maximum(min_vel_ - vel_total, 0.0f))) *
+          (std::move(xt::maximum(vel_total - max_vel_, 0.0f) +
+          xt::maximum(min_vel_ - vel_total, 0.0f))) *
           data.model_dt, {1}, immediate) * weight_;
     }
     return;
@@ -94,15 +94,13 @@ void ConstraintCritic::score(CriticData & data)
     if (power_ > 1u) {
       data.costs += xt::pow(
         xt::sum(
-          (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) +
-          std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f)) +
-          std::move(out_of_turning_rad_motion)) *
+          (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f) +
+          xt::maximum(min_vel_ - data.state.vx, 0.0f) + out_of_turning_rad_motion)) *
           data.model_dt, {1}, immediate) * weight_, power_);
     } else {
       data.costs += xt::sum(
-          (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) +
-          std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f)) +
-          std::move(out_of_turning_rad_motion)) *
+          (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f) +
+          xt::maximum(min_vel_ - data.state.vx, 0.0f) + out_of_turning_rad_motion)) *
           data.model_dt, {1}, immediate) * weight_;
     }
     return;

diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp
@@ -70,7 +70,6 @@ void PathAlignCritic::score(CriticData & data)
   }
 
   const size_t batch_size = data.trajectories.x.shape(0);
-  const size_t time_steps = data.trajectories.x.shape(1);
   auto && cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
 
   // Find integrated distance in the path
@@ -96,13 +95,13 @@ void PathAlignCritic::score(CriticData & data)
   // Get strided trajectory information
   const auto T_x = xt::view(
     data.trajectories.x, xt::all(),
-    xt::range(trajectory_point_step_, time_steps, trajectory_point_step_));
+    xt::range(trajectory_point_step_, _, trajectory_point_step_));
   const auto T_y = xt::view(
     data.trajectories.y, xt::all(),
-    xt::range(trajectory_point_step_, time_steps, trajectory_point_step_));
+    xt::range(trajectory_point_step_, _, trajectory_point_step_));
   const auto T_yaw = xt::view(
     data.trajectories.yaws, xt::all(),
-    xt::range(trajectory_point_step_, time_steps, trajectory_point_step_));
+    xt::range(trajectory_point_step_, _, trajectory_point_step_));
   const auto traj_sampled_size = T_x.shape(1);
 
   for (size_t t = 0; t < batch_size; ++t) {