Improve MPPI tracking precision and make sensor usage flexible

* Implement local path cropping for better lookahead and goal selection.
* Fix cost penalties by evaluating exact distance to the reference path.
* Remove internal velocity filters to prevent double filtering and fix sluggish response.
* Tune angular proportional gain and statistical variance to allow sharp turns.
* Enable continuous controller operation by skipping obstacle avoidance if spatial perception data is missing.

Author: midemig

controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp

@@ -182,19 +182,28 @@ MPPIController::update_rt(NavState & nav_state)
   }
 
   const auto & pose = nav_state.get<nav_msgs::msg::Odometry>("robot_pose").pose.pose;
-  const auto & perceptions = nav_state.get_no_group<PointPerception>();
+
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
-  const auto & filtered = PointPerceptionsOpsView(perceptions)
-    .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0})
-    .fuse(tf_info.map_frame)
-    .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
-    .collapse({NAN, NAN, 0.1})
-    .downsample(0.1)
-    .as_points();
-
-  if (filtered.empty()) {
+
+  pcl::PointCloud<pcl::PointXYZ> filtered;
+
+  if (!nav_state.has("points")) {
     RCLCPP_WARN(get_node()->get_logger(),
-        "No valid points available for MPPI optimization, using the path only.");
+        "No point sensor configured (\"points\" not in nav_state). Running MPPI without obstacle avoidance.");
+  } else {
+    const auto & perceptions = nav_state.get_no_group<PointPerception>();
+    filtered = PointPerceptionsOpsView(perceptions)
+      .filter({-1.0, -1.0, -1.0}, {1.0, 1.0, 1.0})
+      .fuse(tf_info.map_frame)
+      .filter({NAN, NAN, 0.1}, {NAN, NAN, NAN})
+      .collapse({NAN, NAN, 0.1})
+      .downsample(0.1)
+      .as_points();
+
+    if (filtered.empty()) {
+      RCLCPP_WARN(get_node()->get_logger(),
+          "No valid points available for MPPI optimization, using the path only.");
+    }
   }
 
   // Compute the control using MPPI with points

controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp

@@ -23,36 +23,17 @@ MPPIOptimizer::MPPIOptimizer(
 std::vector<std::pair<double, double>>
 MPPIOptimizer::simulate_trajectory(
   double x, double y, double yaw, double v, double w,
-  const nav_msgs::msg::Path & path, int steps)
+  const nav_msgs::msg::Path & /*path*/, int steps)
 {
   size_t horizon_steps = static_cast<size_t>(steps);
   std::vector<std::pair<double, double>> trajectory;
   trajectory.reserve(horizon_steps);
 
-  bool has_path = !path.poses.empty();
-
   for (size_t i = 0; i < horizon_steps; ++i) {
-    // MPPI noise sampling
-    double v_sample = v + normal_(rng_);
-    double w_sample = w + normal_(rng_);
-
     // Differential drive kinematics
-    x += v_sample * std::cos(yaw) * dt_;
-    y += v_sample * std::sin(yaw) * dt_;
-    yaw += w_sample * dt_;
-
-    if (has_path) {
-      // Calculate the index in the path based on the current step
-      size_t path_idx = std::min(static_cast<size_t>((i * path.poses.size()) / horizon_steps),
-          path.poses.size() - 1);
-      double target_x = path.poses[path_idx].pose.position.x;
-      double target_y = path.poses[path_idx].pose.position.y;
-
-      // Smooth motion towards the target
-      double alpha = 0.2;
-      x += alpha * (target_x - x);
-      y += alpha * (target_y - y);
-    }
+    x += v * std::cos(yaw) * dt_;
+    y += v * std::sin(yaw) * dt_;
+    yaw += w * dt_;
 
     trajectory.emplace_back(x, y);
   }
@@ -87,21 +68,30 @@ double MPPIOptimizer::compute_cost(
   double cost = 0.0;
   size_t path_size = path.poses.size();
 
+  if (path_size == 0) return std::numeric_limits<double>::max();
+
   // --- Path-Tracking Penalties ---
   for (size_t i = 0; i < trajectory.size(); ++i) {
     const auto &[x, y] = trajectory[i];
 
-    // Cost is distributed along the trajectory
-    double path_alpha = static_cast<double>(i) / trajectory.size();
-    size_t idx = std::min(static_cast<size_t>(path_alpha * path_size), path_size - 1);
-
-    double dx = path.poses[idx].pose.position.x - x;
-    double dy = path.poses[idx].pose.position.y - y;
-    double dist = std::hypot(dx, dy);
+    // Find closest point on path (path is already cropped to local window)
+    double min_dist2 = std::numeric_limits<double>::max();
+    size_t closest_idx = 0;
+    
+    for (size_t j = 0; j < path_size; ++j) {
+      double dx = path.poses[j].pose.position.x - x;
+      double dy = path.poses[j].pose.position.y - y;
+      double dist2 = dx*dx + dy*dy;
+      if (dist2 < min_dist2) {
+        min_dist2 = dist2;
+        closest_idx = j;
+      }
+    }
+    double dist = std::sqrt(min_dist2);
 
     // Heading error is calculated based on the initial yaw
-    double heading_penalty = heading_error(initial_yaw, path.poses[idx].pose.position.x,
-                                               path.poses[idx].pose.position.y, x, y);
+    double heading_penalty = heading_error(initial_yaw, path.poses[closest_idx].pose.position.x,
+                                               path.poses[closest_idx].pose.position.y, x, y);
 
     // FOV penalty: discourage trajectories outside robot's view
     double angle_to_goal = heading_error(initial_yaw, trajectory.back().first,
@@ -187,10 +177,39 @@ MPPIResult MPPIOptimizer::compute_control(
   double y = current_pose.position.y;
   double yaw = tf2::getYaw(current_pose.orientation);
 
-  // Select goal pose (within horizon, or last path point)
-  int last_pose = std::min(static_cast<size_t>(horizon_steps_), path.poses.size() - 1);
-  int sim_steps = std::max(1, last_pose);
-  const auto & path_pose = path.poses[last_pose].pose;
+  // Find closest point on global path
+  size_t closest_idx = 0;
+  double min_dist_to_robot = std::numeric_limits<double>::max();
+  for (size_t i = 0; i < path.poses.size(); ++i) {
+    double dist = std::hypot(path.poses[i].pose.position.x - x, path.poses[i].pose.position.y - y);
+    if (dist < min_dist_to_robot) {
+      min_dist_to_robot = dist;
+      closest_idx = i;
+    }
+  }
+
+  // Crop the path up to a reasonable lookahead distance
+  nav_msgs::msg::Path local_path;
+  local_path.header = path.header;
+  
+  double max_lookahead_dist = max_lin_vel_ * horizon_steps_ * dt_ * 1.5; 
+  if (max_lookahead_dist < 2.0) max_lookahead_dist = 2.0;
+
+  double path_accum_dist = 0.0;
+  local_path.poses.push_back(path.poses[closest_idx]);
+  for (size_t i = closest_idx + 1; i < path.poses.size(); ++i) {
+    local_path.poses.push_back(path.poses[i]);
+    double dx = path.poses[i].pose.position.x - path.poses[i-1].pose.position.x;
+    double dy = path.poses[i].pose.position.y - path.poses[i-1].pose.position.y;
+    path_accum_dist += std::hypot(dx, dy);
+    if (path_accum_dist > max_lookahead_dist) {
+      break;
+    }
+  }
+
+  // Select local goal pose from the cropped path
+  int sim_steps = horizon_steps_;
+  const auto & path_pose = local_path.poses.back().pose;
   double px = path_pose.position.x;
   double py = path_pose.position.y;
   double dist_to_goal = std::hypot(px - x, py - y);
@@ -222,19 +241,22 @@ MPPIResult MPPIOptimizer::compute_control(
   double base_v = max_lin_vel_ * std::min(dist_to_goal, 1.0) * v_scale;
   base_v = std::clamp(base_v, 0.0, max_lin_vel_);
 
-  // Angular velocity is proportional to the heading error: turn faster if more misaligned
-  double w_scale = std::min(1.0, 2.0 * angle_mag / M_PI);
-  double base_w = std::clamp(w_scale * angle_error, -max_ang_vel_, max_ang_vel_);
+  // Apply a stronger proportional gain for angular tracking to allow sharp turns
+  double base_w = std::clamp(2.5 * angle_error, -max_ang_vel_, max_ang_vel_);
+
+  // Use proper spread variance to allow MPPI to explore turns
+  std::normal_distribution<double> v_dist(0.0, max_lin_vel_ * 0.2); 
+  std::normal_distribution<double> w_dist(0.0, max_ang_vel_ * 0.4); 
 
   // Generate candidate trajectories
   for (int i = 0; i < num_samples_; ++i) {
     // Sample noise for velocities with Gaussian distribution and clamp
-    double v = std::clamp(base_v + v_noise_(rng_), -max_lin_vel_, max_lin_vel_);
-    double w = std::clamp(base_w + w_noise_(rng_), -max_ang_vel_, max_ang_vel_);
+    double v = std::clamp(base_v + v_dist(rng_), -max_lin_vel_, max_lin_vel_);
+    double w = std::clamp(base_w + w_dist(rng_), -max_ang_vel_, max_ang_vel_);
 
     // Simulate trajectory and compute its cost
-    auto traj = simulate_trajectory(x, y, yaw, v, w, path, sim_steps);
-    double cost = compute_cost(traj, path, v, w, yaw, points);
+    auto traj = simulate_trajectory(x, y, yaw, v, w, local_path, sim_steps);
+    double cost = compute_cost(traj, local_path, v, w, yaw, points);
     all_trajs.push_back(traj);
 
     // Store the sample with its cost
@@ -273,18 +295,11 @@ MPPIResult MPPIOptimizer::compute_control(
     vrot += sample.w * sample.cost / denom;
   }
 
-  // Calculate the maximum change in velocities based on acceleration limits
-  double max_v_delta = max_lin_acc_ * dt_;
-  double max_w_delta = max_ang_acc_ * dt_;
-
-  // Limit velocity changes (slew rate)
-  vlin = last_v_ + std::clamp(vlin - last_v_, -max_v_delta, max_v_delta);
-  vrot = last_w_ + std::clamp(vrot - last_w_, -max_w_delta, max_w_delta);
-
-  // Smooth velocities
-  double alpha_smooth = 0.2;  // lower is more smooth
-  last_v_ = alpha_smooth * vlin + (1.0 - alpha_smooth) * last_v_;
-  last_w_ = alpha_smooth * vrot + (1.0 - alpha_smooth) * last_w_;
+  // We remove the internal slew rate and low-pass filters here because MPPIController
+  // already rigorously applies kinematic constraints (accelerations). Double filtering 
+  // causes severe sluggishness and path divergence.
+  last_v_ = vlin;
+  last_w_ = vrot;
 
   // Return final control command and trajectories
   return MPPIResult{last_v_, last_w_, all_trajs, best_traj};