Major updates

CMakeLists.txt

@@ -81,7 +81,7 @@ if(BUILD_TESTING)
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
   add_subdirectory(test)
-  add_subdirectory(benchmark)
+  # add_subdirectory(benchmark)
 endif()
 
 ament_export_libraries(${libraries})

include/mppic/controller.hpp

@@ -70,6 +70,11 @@ class MPPIController : public nav2_core::Controller
     */
   void deactivate() override;
 
+  /**
+    * @brief Reset the controller state between tasks
+    */
+  void reset() override;
+
   /**
     * @brief Main method to compute velocities using the optimizer
     * @param robot_pose Robot pose

include/mppic/critic_data.hpp

@@ -16,6 +16,7 @@
 #define MPPIC__CRITIC_DATA_HPP_
 
 #include <memory>
+#include <vector>
 #include <xtensor/xtensor.hpp>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -46,6 +47,7 @@ struct CriticData
   bool fail_flag;
   nav2_core::GoalChecker * goal_checker;
   std::shared_ptr<MotionModel> motion_model;
+  std::optional<std::vector<bool>> path_pts_valid;
   std::optional<size_t> furthest_reached_path_point;
 };
 

include/mppic/critic_function.hpp

@@ -27,6 +27,16 @@
 namespace mppi::critics
 {
 
+/**
+ * @class mppi::critics::CollisionCost
+ * @brief Utility for storing cost information
+ */
+struct CollisionCost
+{
+  float cost{0};
+  bool using_footprint{false};
+};
+
 /**
  * @class mppi::critics::CriticFunction
  * @brief Abstract critic objective function to score trajectories

include/mppic/critics/goal_critic.hpp

@@ -44,6 +44,7 @@ class GoalCritic : public CriticFunction
 protected:
   unsigned int power_{0};
   float weight_{0};
+  float threshold_to_consider_{0};
 };
 
 }  // namespace mppi::critics

include/mppic/critics/obstacles_critic.hpp

@@ -26,19 +26,12 @@
 namespace mppi::critics
 {
 
-/**
- * @class mppi::critics::CollisionCost
- * @brief Utility for storing cost information
- */
-struct CollisionCost
-{
-  float cost;
-  bool using_footprint;
-};
-
 /**
  * @class mppi::critics::ConstraintCritic
- * @brief Critic objective function for avoiding obstacles
+ * @brief Critic objective function for avoiding obstacles, allowing it to deviate off
+ * the planned path. This is important to tune in tandem with PathAlign to make a balance
+ * between path-tracking and dynamic obstacle avoidance capabilities as desirable for a
+ * particular application
  */
 class ObstaclesCritic : public CriticFunction
 {
@@ -100,15 +93,14 @@ class ObstaclesCritic : public CriticFunction
 
   bool consider_footprint_{true};
   double collision_cost_{0};
-  float inflation_scale_factor_;
+  float inflation_scale_factor_{0}, inflation_radius_{0};
 
   float possibly_inscribed_cost_;
-  float trajectory_penalty_distance_;
   float collision_margin_distance_;
   float near_goal_distance_;
 
   unsigned int power_{0};
-  float weight_{0};
+  float repulsion_weight_, critical_weight_{0};
 };
 
 }  // namespace mppi::critics

include/mppic/critics/path_align_critic.hpp

@@ -24,7 +24,10 @@ namespace mppi::critics
 
 /**
  * @class mppi::critics::ConstraintCritic
- * @brief Critic objective function for aligning to the path
+ * @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.
+ * This is an important critic to tune and consider in tandem with Obstacle.
  */
 class PathAlignCritic : public CriticFunction
 {
@@ -42,10 +45,10 @@ class PathAlignCritic : public CriticFunction
   void score(CriticData & data) override;
 
 protected:
-  unsigned int path_point_step_{0};
-  unsigned int trajectory_point_step_{0};
+  size_t offset_from_furthest_{0};
+  int trajectory_point_step_{0};
   float threshold_to_consider_{0};
-
+  float max_path_occupancy_ratio_{0};
   unsigned int power_{0};
   float weight_{0};
 };

include/mppic/critics/path_angle_critic.hpp

@@ -25,6 +25,7 @@ namespace mppi::critics
 /**
  * @class mppi::critics::ConstraintCritic
  * @brief Critic objective function for aligning to path in cases of extreme misalignment
+ * or turning
  */
 class PathAngleCritic : public CriticFunction
 {

include/mppic/critics/path_follow_critic.hpp

@@ -24,7 +24,11 @@ namespace mppi::critics
 
 /**
  * @class mppi::critics::ConstraintCritic
- * @brief Critic objective function for following the path
+ * @brief Critic objective function for following the path approximately
+ * To allow for deviation from path in case of dynamic obstacles. Path Align
+ * is what aligns the trajectories to the path more or less precisely, if desireable.
+ * A higher weight here with an offset > 1 will accelerate the samples to full speed
+ * faster and push the follow point further ahead, creating some shortcutting.
  */
 class PathFollowCritic : public CriticFunction
 {
@@ -43,7 +47,7 @@ class PathFollowCritic : public CriticFunction
   void score(CriticData & data) override;
 
 protected:
-  float max_path_ratio_{0};
+  float threshold_to_consider_{0};
   size_t offset_from_furthest_{0};
 
   unsigned int power_{0};

include/mppic/optimizer.hpp

@@ -111,6 +111,11 @@ class Optimizer
    */
   void setSpeedLimit(double speed_limit, bool percentage);
 
+  /**
+   * @brief Reset the optimization problem to initial conditions
+   */
+  void reset();
+
 protected:
   /**
    * @brief Main function to generate, score, and return trajectories
@@ -134,11 +139,6 @@ class Optimizer
    */
   void getParams();
 
-  /**
-   * @brief Reset the optimization problem to initial conditions
-   */
-  void reset();
-
   /**
    * @brief Set the motion model of the vehicle platform
    * @param model Model string to use
@@ -254,7 +254,7 @@ class Optimizer
 
   CriticData critics_data_ =
   {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr,
-    std::nullopt};  /// Caution, keep references
+    std::nullopt, std::nullopt};  /// Caution, keep references
 
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 };

include/mppic/tools/utils.hpp

@@ -19,6 +19,7 @@
 #include <chrono>
 #include <string>
 #include <limits>
+#include <memory>
 
 #include <xtensor/xarray.hpp>
 #include <xtensor/xnorm.hpp>
@@ -311,7 +312,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data)
   for (size_t i = 0; i < dists.shape(0); i++) {
     size_t min_id_by_path = 0;
     for (size_t j = 0; j < dists.shape(1); j++) {
-      if (min_distance_by_path > dists(i, j)) {
+      if (dists(i, j) < min_distance_by_path) {
         min_distance_by_path = dists(i, j);
         min_id_by_path = j;
       }
@@ -321,6 +322,31 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data)
   return max_id_by_trajectories;
 }
 
+/**
+ * @brief Evaluate closest point idx of data.path which is
+ * nearset to the start of the trajectory in data.trajectories
+ * @param data Data to use
+ * @return Idx of closest path point at start of the trajectories
+ */
+inline size_t findPathTrajectoryInitialPoint(const CriticData & data)
+{
+  // First point should be the same for all trajectories from initial conditions
+  const auto dx = data.path.x - data.trajectories.x(0, 0);
+  const auto dy = data.path.y - data.trajectories.y(0, 0);
+  const auto dists = dx * dx + dy * dy;
+
+  double min_distance_by_path = std::numeric_limits<float>::max();
+  size_t min_id = 0;
+  for (size_t j = 0; j < dists.shape(0); j++) {
+    if (dists(j) < min_distance_by_path) {
+      min_distance_by_path = dists(j);
+      min_id = j;
+    }
+  }
+
+  return min_id;
+}
+
 /**
  * @brief evaluate path furthest point if it is not set
  * @param data Data to use
@@ -332,6 +358,58 @@ inline void setPathFurthestPointIfNotSet(CriticData & data)
   }
 }
 
+/**
+ * @brief evaluate path costs
+ * @param data Data to use
+ */
+inline void findPathCosts(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  auto * costmap = costmap_ros->getCostmap();
+  unsigned int map_x, map_y;
+  const size_t path_segments_count = data.path.x.shape(0) - 1;
+  data.path_pts_valid->resize(path_segments_count - 1, false);
+  for (unsigned int idx = 0; idx < path_segments_count - 1; idx++) {
+    const auto path_x = data.path.x(idx);
+    const auto path_y = data.path.y(idx);
+    if (!costmap->worldToMap(path_x, path_y, map_x, map_y)) {
+      (*data.path_pts_valid)[idx] = false;
+      continue;
+    }
+
+    switch (costmap->getCost(map_x, map_y)) {
+      using namespace nav2_costmap_2d; // NOLINT
+      case (LETHAL_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (INSCRIBED_INFLATED_OBSTACLE):
+        (*data.path_pts_valid)[idx] = false;
+        continue;
+      case (NO_INFORMATION):
+        const bool is_tracking_unknown =
+          costmap_ros->getLayeredCostmap()->isTrackingUnknown();
+        (*data.path_pts_valid)[idx] = is_tracking_unknown ? true : false;
+        continue;
+    }
+
+    (*data.path_pts_valid)[idx] = true;
+  }
+}
+
+/**
+ * @brief evaluate path costs if it is not set
+ * @param data Data to use
+ */
+inline void setPathCostsIfNotSet(
+  CriticData & data,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  if (!data.path_pts_valid) {
+    findPathCosts(data, costmap_ros);
+  }
+}
+
 /**
  * @brief evaluate angle from pose (have angle) to point (no angle)
  * @param pose pose
@@ -349,22 +427,6 @@ inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point
   return abs(angles::shortest_angular_distance(yaw, pose_yaw));
 }
 
-/**
- * @brief Evaluate ratio of data.path which reached by all trajectories in data.trajectories
- * @param data Data to use
- * @return ratio of path reached
- */
-inline float getPathRatioReached(const CriticData & data)
-{
-  if (!data.furthest_reached_path_point) {
-    throw std::runtime_error("Furthest point not computed yet");
-  }
-
-  auto path_points_count = static_cast<float>(data.path.x.shape(0));
-  auto furthest_reached_path_point = static_cast<float>(*data.furthest_reached_path_point);
-  return furthest_reached_path_point / path_points_count;
-}
-
 /**
  * @brief Apply Savisky-Golay filter to optimal trajectory
  * @param control_sequence Sequence to apply filter to

src/controller.cpp

@@ -69,6 +69,11 @@ void MPPIController::deactivate()
   RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
 }
 
+void MPPIController::reset()
+{
+  optimizer_.reset();
+}
+
 geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   const geometry_msgs::msg::PoseStamped & robot_pose,
   const geometry_msgs::msg::Twist & robot_speed,

src/critics/goal_angle_critic.cpp

@@ -39,22 +39,18 @@ void GoalAngleCritic::score(CriticData & data)
     return;
   }
 
-  const auto goal_idx = data.path.x.shape(0) - 1;
-
-  const float goal_x = data.path.x(goal_idx);
-  const float goal_y = data.path.y(goal_idx);
-
-  const float dx = data.state.pose.pose.position.x - goal_x;
-  const float dy = data.state.pose.pose.position.y - goal_y;
+  if (!utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, data.path))
+  {
+    return;
+  }
 
-  const float dist = std::sqrt(dx * dx + dy * dy);
+  const auto goal_idx = data.path.x.shape(0) - 1;
+  const float goal_yaw = data.path.yaws(goal_idx);
 
-  if (dist < threshold_to_consider_) {
-    const float goal_yaw = data.path.yaws(goal_idx);
-    data.costs += xt::pow(
-      xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) *
-      weight_, power_);
-  }
+  data.costs += xt::pow(
+    xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) *
+    weight_, power_);
 }
 
 }  // namespace mppi::critics

src/critics/goal_critic.cpp

@@ -23,6 +23,8 @@ void GoalCritic::initialize()
 
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 5.0);
+  getParam(threshold_to_consider_, "threshold_to_consider", 1.0);
+
   RCLCPP_INFO(
     logger_, "GoalCritic instantiated with %d power and %f weight.",
     power_, weight_);
@@ -34,6 +36,12 @@ void GoalCritic::score(CriticData & data)
     return;
   }
 
+  if (!utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, data.path))
+  {
+    return;
+  }
+
   const auto goal_idx = data.path.x.shape(0) - 1;
 
   const auto goal_x = data.path.x(goal_idx);