fixing test failure

ros-navigation · Mar 22, 2024 · 96d6c65 · 96d6c65
1 parent d0fea59
commit 96d6c65
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 25 deletions.
diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp
@@ -55,12 +55,12 @@ void ConstraintCritic::score(CriticData & data)
           (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_);
+          data.model_dt, {1}, immediate) * weight_, power_);
     } else {
       data.costs += xt::sum(
-          (std::move(
-            xt::maximum(data.state.vx - max_vel_, 0.0f) +
-            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;
@@ -77,12 +77,12 @@ void ConstraintCritic::score(CriticData & data)
           (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_);
+          data.model_dt, {1}, immediate) * weight_, power_);
     } else {
       data.costs += xt::sum(
-          (std::move(
-            xt::maximum(vel_total - max_vel_, 0.0f) +
-            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;
@@ -101,12 +101,12 @@ void ConstraintCritic::score(CriticData & data)
           (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_);
+          data.model_dt, {1}, immediate) * weight_, power_);
     } else {
       data.costs += xt::sum(
-          (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)) *
+        (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_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
@@ -107,7 +107,7 @@ void PathAngleCritic::score(CriticData & data)
       {
         auto yaws =
           xt::fabs(
-            utils::shortest_angular_distance(
+          utils::shortest_angular_distance(
             xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points));
         if (power_ > 1u) {
           data.costs += xt::pow(std::move(yaws) * weight_, power_);
@@ -120,7 +120,7 @@ void PathAngleCritic::score(CriticData & data)
       {
         auto yaws =
           xt::fabs(
-            utils::shortest_angular_distance(
+          utils::shortest_angular_distance(
             xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points));
         const auto yaws_between_points_corrected = xt::where(
           yaws < M_PIF_2, yaws_between_points,
@@ -129,9 +129,9 @@ void PathAngleCritic::score(CriticData & data)
           utils::shortest_angular_distance(
             xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points_corrected));
         if (power_ > 1u) {
-          data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_);
+          data.costs += xt::pow(std::move(corrected_yaws) * weight_, power_);
         } else {
-          data.costs += xt::mean(corrected_yaws, {1}, immediate) * weight_;
+          data.costs += std::move(corrected_yaws) * weight_;
         }
         return;
       }
@@ -144,9 +144,9 @@ void PathAngleCritic::score(CriticData & data)
           utils::shortest_angular_distance(
             xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points_corrected));
         if (power_ > 1u) {
-          data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_);
+          data.costs += xt::pow(std::move(corrected_yaws) * weight_, power_);
         } else {
-          data.costs += xt::mean(corrected_yaws, {1}, immediate) * weight_;
+          data.costs += std::move(corrected_yaws) * weight_;
         }
         return;
       }

diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
@@ -46,7 +46,7 @@ void PreferForwardCritic::score(CriticData & data)
           xt::maximum(-data.state.vx, 0)) * data.model_dt, {1}, immediate) * weight_, power_);
   } else {
     data.costs += xt::sum(
-          std::move(xt::maximum(-data.state.vx, 0)) * data.model_dt, {1}, immediate) * weight_;
+      std::move(xt::maximum(-data.state.vx, 0)) * data.model_dt, {1}, immediate) * weight_;
   }
 }
 

diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
@@ -285,8 +285,8 @@ void Optimizer::integrateStateVelocities(
 
   xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw;
 
-  auto yaw_cos = xt::eval(xt::cos(traj_yaws));
-  auto yaw_sin = xt::eval(xt::sin(traj_yaws));
+  auto yaw_cos = xt::roll(xt::eval(xt::cos(traj_yaws)), 1);
+  auto yaw_sin = xt::roll(xt::eval(xt::sin(traj_yaws)), 1);
   xt::view(yaw_cos, 0) = cosf(initial_yaw);
   xt::view(yaw_sin, 0) = sinf(initial_yaw);
 
@@ -312,8 +312,8 @@ void Optimizer::integrateStateVelocities(
   xt::noalias(trajectories.yaws) =
     xt::cumsum(state.wz * settings_.model_dt, {1}) + initial_yaw;
 
-  auto yaw_cos = xt::eval(xt::cos(trajectories.yaws));
-  auto yaw_sin = xt::eval(xt::sin(trajectories.yaws));
+  auto yaw_cos = xt::roll(xt::eval(xt::cos(trajectories.yaws)), 1, 1);
+  auto yaw_sin = xt::roll(xt::eval(xt::sin(trajectories.yaws)), 1, 1);
   xt::view(yaw_cos, xt::all(), 0) = cosf(initial_yaw);
   xt::view(yaw_sin, xt::all(), 0) = sinf(initial_yaw);
 

diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp
@@ -627,8 +627,8 @@ TEST(OptimizerTests, integrateStateVelocitiesTests)
   float y = 0;
   for (unsigned int i = 1; i != traj.x.shape(1); i++) {
     std::cout << i << std::endl;
-    x += (0.1 /*vx*/ * cos(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/;
-    y += (0.1 /*vx*/ * sin(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/;
+    x += (0.1 /*vx*/ * cosf(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/;
+    y += (0.1 /*vx*/ * sinf(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/;
 
     EXPECT_NEAR(traj.x(1, i), x, 1e-6);
     EXPECT_NEAR(traj.y(1, i), y, 1e-6);