Update mppi controller with the latest changes

logivations · Jul 11, 2023 · 087ce49 · 087ce49
2 parents 4a82bf2 + ead48a9
commit 087ce49
Show file tree

Hide file tree

Showing 6 changed files with 18 additions and 19 deletions.
diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp
@@ -26,7 +26,7 @@ namespace mppi::critics
  * @class mppi::critics::ConstraintCritic
  * @brief Critic objective function for aligning to the path. Note:
  * High settings of this will follow the path more precisely, but also makes it
- * difficult (or impossible) to deviate in the presense of dynamic obstacles.
+ * difficult (or impossible) to deviate in the presence of dynamic obstacles.
  * This is an important critic to tune and consider in tandem with Obstacle.
  */
 class PathAlignCritic : public CriticFunction

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
@@ -112,7 +112,7 @@ class AckermannMotionModel : public MotionModel
     auto & vx = control_sequence.vx;
     auto & wz = control_sequence.wz;
 
-    auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) > min_turning_r_);
+    auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) < min_turning_r_);
     view = xt::sign(wz) * vx / min_turning_r_;
   }
 

diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_mppi_controller</name>
-  <version>0.2.1</version>
+  <version>1.1.8</version>
   <description>nav2_mppi_controller</description>
   <maintainer email="[email protected]">Aleksei Budyakov</maintainer>
   <maintainer email="[email protected]">Steve Macenski</maintainer>

diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp
@@ -34,16 +34,15 @@ void PathFollowCritic::initialize()
 
 void PathFollowCritic::score(CriticData & data)
 {
-  const size_t path_size = data.path.x.shape(0) - 1;
-
-  if (!enabled_ || path_size == 0 ||
+  if (!enabled_ || data.path.x.shape(0) < 2 ||
     utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path))
   {
     return;
   }
 
   utils::setPathFurthestPointIfNotSet(data);
   utils::setPathCostsIfNotSet(data, costmap_ros_);
+  const size_t path_size = data.path.x.shape(0) - 1;
 
   auto offseted_idx = std::min(
     *data.furthest_reached_path_point + offset_from_furthest_, path_size);

diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp
@@ -156,7 +156,7 @@ TEST(MotionModelTests, AckermannTest)
   // Check that application of constraints are non-empty for Ackermann Drive
   for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
     control_sequence.vx(i) = i * i * i;
-    control_sequence.wz(i) = i * i * i;
+    control_sequence.wz(i) = i * i * i * i;
   }
 
   models::ControlSequence initial_control_sequence = control_sequence;
@@ -168,7 +168,7 @@ TEST(MotionModelTests, AckermannTest)
   // Now, check the specifics of the minimum curvature constraint
   EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6);
   for (unsigned int i = 1; i != control_sequence.vx.shape(0); i++) {
-    EXPECT_TRUE(fabs(control_sequence.vx(i)) / fabs(control_sequence.wz(i)) <= 0.2);
+    EXPECT_TRUE(fabs(control_sequence.vx(i)) / fabs(control_sequence.wz(i)) >= 0.2);
   }
 
   // Check that Ackermann Drive is properly non-holonomic and parameterized