[Graceful Controller] Enhance approach goal logic

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

@@ -114,20 +114,54 @@ class GracefulController : public nav2_core::Controller
    * @brief Validate a given target pose for calculating command velocity
    * @param target_pose Target pose to validate
    * @param dist_to_target Distance to target pose
-   * @param dist_to_goal Distance to navigation goal
    * @param trajectory Trajectory to validate in simulation
    * @param costmap_transform Transform between global and local costmap
    * @param cmd_vel Initial command velocity to validate in simulation
    * @return true if target pose is valid, false otherwise
    */
   bool validateTargetPose(
+    geometry_msgs::msg::PoseStamped & target_pose,
+    double dist_to_target,
+    nav_msgs::msg::Path & trajectory,
+    geometry_msgs::msg::TransformStamped & costmap_transform,
+    geometry_msgs::msg::TwistStamped & cmd_vel);
+
+  /**
+   * @brief Validate a given target pose for calculating command velocity on approach to goal
+   * @param target_pose Target pose to validate
+   * @param dist_to_target Distance to target pose
+   * @param dist_to_goal Distance to navigation goal
+   * @param trajectory Trajectory to validate in simulation
+   * @param costmap_transform Transform between global and local costmap
+   * @param cmd_vel Initial command velocity to validate in simulation
+   * @return true if target pose is valid, false otherwise
+   */
+  bool validateTargetPoseOnApproach(
     geometry_msgs::msg::PoseStamped & target_pose,
     double dist_to_target,
     double dist_to_goal,
     nav_msgs::msg::Path & trajectory,
     geometry_msgs::msg::TransformStamped & costmap_transform,
     geometry_msgs::msg::TwistStamped & cmd_vel);
 
+  /**
+   * @brief Find the best approach trajectory by searching multiple orientations
+   * @param target_pose Base target pose (position will be used)
+   * @param dist_to_target Distance to target
+   * @param costmap_transform Transform
+   * @param safety_cost Safety cost threshold
+   * @param best_trajectory Reference to store best trajectory
+   * @param best_cmd_vel Reference to store best cmd_vel
+   * @return true if a valid trajectory was found
+   */
+  bool findBestApproachTrajectory(
+    geometry_msgs::msg::PoseStamped & target_pose,
+    double dist_to_target,
+    geometry_msgs::msg::TransformStamped & costmap_transform,
+    double safety_cost,
+    nav_msgs::msg::Path & best_trajectory,
+    geometry_msgs::msg::TwistStamped & best_cmd_vel);
+
   /**
    * @brief Simulate trajectory calculating in every step the new velocity command based on
    * a new curvature value and checking for collisions.
@@ -166,6 +200,16 @@ class GracefulController : public nav2_core::Controller
     const double & x, const double & y, const double & theta,
     double inflation_scale = 1.0);
 
+  /**
+   * @brief Get the maximum cost of a path
+   * @param path Path to check
+   * @param costmap_transform Transform between global and local costmap
+   * @return Maximum cost encountered
+   */
+  double getMaxCost(
+    const nav_msgs::msg::Path & path,
+    geometry_msgs::msg::TransformStamped & costmap_transform);
+
   /**
    * @brief Compute the distance to each pose in a path
    * @param poses Poses to compute distances with
@@ -195,6 +239,8 @@ class GracefulController : public nav2_core::Controller
   // True from the time a new path arrives until we have completed an initial rotation
   bool do_initial_rotation_;
 
+  std::optional<double> safe_approach_angle_;
+
   nav2::Publisher<nav_msgs::msg::Path>::SharedPtr local_plan_pub_;
   nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr motion_target_pub_;
   nav2::Publisher<visualization_msgs::msg::Marker>::SharedPtr slowdown_pub_;

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -53,6 +53,8 @@ struct Parameters
   double footprint_scaling_linear_vel;
   double footprint_scaling_factor;
   double footprint_scaling_step;
+  int obstacle_cost_margin;
+  double final_rotation_search_step;
 };
 
 /**

nav2_graceful_controller/src/graceful_controller.cpp

@@ -58,6 +58,16 @@ void GracefulController::configure(
         FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>(costmap_ros_->getCostmap());
   }
 
+  double max_valid_cost = costmap_ros_->getUseRadius() ?
+    static_cast<double>(nav2_costmap_2d::MAX_NON_OBSTACLE) :
+    static_cast<double>(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
+  if (max_valid_cost - static_cast<double>(params_->obstacle_cost_margin) < 0.0) {
+    RCLCPP_WARN(
+      logger_, "obstacle_cost_margin (%d) is higher than max cost (%d).",
+      params_->obstacle_cost_margin, nav2_costmap_2d::MAX_NON_OBSTACLE);
+    throw nav2_core::ControllerException("obstacle_cost_margin is higher than max cost.");
+  }
+
   // Publishers
   local_plan_pub_ = node->create_publisher<nav_msgs::msg::Path>("local_plan");
   motion_target_pub_ = node->create_publisher<geometry_msgs::msg::PoseStamped>("motion_target");
@@ -222,8 +232,10 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
     }
 
     // Compute velocity at this moment if valid target pose is found
-    if (validateTargetPose(
-        target_pose, dist_to_target, dist_to_goal, local_plan, costmap_transform, cmd_vel))
+    if (
+      validateTargetPoseOnApproach(target_pose, dist_to_target, dist_to_goal, local_plan,
+        costmap_transform, cmd_vel) ||
+      validateTargetPose(target_pose, dist_to_target, local_plan, costmap_transform, cmd_vel))
     {
       // Publish the selected target_pose
       motion_target_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(target_pose));
@@ -246,6 +258,7 @@ void GracefulController::newPathReceived(const nav_msgs::msg::Path & /*raw_globa
 {
   goal_reached_ = false;
   do_initial_rotation_ = true;
+  safe_approach_angle_.reset();
 }
 
 void GracefulController::setSpeedLimit(
@@ -273,30 +286,15 @@ void GracefulController::setSpeedLimit(
 }
 
 bool GracefulController::validateTargetPose(
-  geometry_msgs::msg::PoseStamped & target_pose,
-  double dist_to_target,
-  double dist_to_goal,
-  nav_msgs::msg::Path & trajectory,
-  geometry_msgs::msg::TransformStamped & costmap_transform,
+  geometry_msgs::msg::PoseStamped & target_pose, double dist_to_target,
+  nav_msgs::msg::Path & trajectory, geometry_msgs::msg::TransformStamped & costmap_transform,
   geometry_msgs::msg::TwistStamped & cmd_vel)
 {
   // Continue if target_pose is too far away from robot
   if (dist_to_target > params_->max_lookahead) {
     return false;
   }
 
-  if (dist_to_goal < params_->max_lookahead) {
-    if (params_->prefer_final_rotation) {
-      // Avoid instability and big sweeping turns at the end of paths by
-      // ignoring final heading
-      double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x);
-      target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
-    }
-  } else if (dist_to_target < params_->min_lookahead) {
-    // Make sure target is far enough away to avoid instability
-    return false;
-  }
-
   // Flip the orientation of the motion target if the robot is moving backwards
   bool reversing = false;
   if (params_->allow_backward && target_pose.pose.position.x < 0.0) {
@@ -308,8 +306,7 @@ bool GracefulController::validateTargetPose(
   // Actually simulate the path
   double sim_linear_velocity = params_->v_linear_max;
   do {
-    control_law_->setSpeedLimit(params_->v_linear_min, sim_linear_velocity,
-      params_->v_angular_max);
+    control_law_->setSpeedLimit(params_->v_linear_min, sim_linear_velocity, params_->v_angular_max);
     if (simulateTrajectory(target_pose, costmap_transform, trajectory, cmd_vel, reversing)) {
       // Successfully simulated to target_pose
       return true;
@@ -322,6 +319,40 @@ bool GracefulController::validateTargetPose(
   return false;
 }
 
+bool GracefulController::validateTargetPoseOnApproach(
+  geometry_msgs::msg::PoseStamped & target_pose, double dist_to_target, double dist_to_goal,
+  nav_msgs::msg::Path & trajectory, geometry_msgs::msg::TransformStamped & costmap_transform,
+  geometry_msgs::msg::TwistStamped & cmd_vel)
+{
+  // Not approaching goal with large lookahead and don't evaluate shortcut trajectories
+  // when we do not prefer rotating to goal at the end.
+  if (dist_to_goal >= params_->max_lookahead || !params_->prefer_final_rotation) {
+    return false;
+  }
+  // Avoid instability and big sweeping turns at the end of paths by
+  // ignoring final heading
+  double yaw = std::atan2(target_pose.pose.position.y, target_pose.pose.position.x);
+  target_pose.pose.orientation =
+    nav2_util::geometry_utils::orientationAroundZAxis(yaw);
+
+  if (validateTargetPose(target_pose, dist_to_target, trajectory, costmap_transform, cmd_vel)) {
+    // Determine the maximum valid cost based on robot footprint type
+    double max_valid_cost =
+      costmap_ros_->getUseRadius() ? static_cast<double>(nav2_costmap_2d::MAX_NON_OBSTACLE) :
+      static_cast<double>(nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
+
+    // Check if the final rotation path is risky
+    double safety_threshold = max_valid_cost - static_cast<double>(params_->obstacle_cost_margin);
+    if (getMaxCost(trajectory, costmap_transform) >= safety_threshold) {
+      // Try to find a better approach by searching spiral curves
+      findBestApproachTrajectory(
+            target_pose, dist_to_target, costmap_transform, max_valid_cost, trajectory, cmd_vel);
+    }
+    return true;
+  }
+  return false;
+}
+
 bool GracefulController::simulateTrajectory(
   const geometry_msgs::msg::PoseStamped & motion_target,
   const geometry_msgs::msg::TransformStamped & costmap_transform,
@@ -426,6 +457,25 @@ geometry_msgs::msg::Twist GracefulController::rotateToTarget(double angle_to_tar
   return vel;
 }
 
+double GracefulController::getMaxCost(
+  const nav_msgs::msg::Path & path, geometry_msgs::msg::TransformStamped & costmap_transform)
+{
+  double max_cost = 0.0;
+
+  for (const auto & pose : path.poses) {
+    geometry_msgs::msg::PoseStamped costmap_pose;
+    tf2::doTransform(pose, costmap_pose, costmap_transform);
+    unsigned int mx, my;
+    if (costmap_ros_->getCostmap()->worldToMap(costmap_pose.pose.position.x,
+        costmap_pose.pose.position.y, mx, my))
+    {
+      max_cost = std::max(max_cost, collision_checker_->pointCost(mx, my));
+    }
+  }
+
+  return max_cost;
+}
+
 bool GracefulController::inCollision(
   const double & x, const double & y, const double & theta,
   double inflation_scale)
@@ -515,6 +565,76 @@ void GracefulController::validateOrientations(
   }
 }
 
+bool GracefulController::findBestApproachTrajectory(
+  geometry_msgs::msg::PoseStamped & target_pose, double dist_to_target,
+  geometry_msgs::msg::TransformStamped & costmap_transform, double safety_cost,
+  nav_msgs::msg::Path & best_trajectory, geometry_msgs::msg::TwistStamped & best_cmd_vel)
+{
+  bool found_valid = false;
+  double best_eta = std::numeric_limits<double>::max();
+
+  for (int i = 0; i < 2 * M_PI / params_->final_rotation_search_step; ++i) {
+    double angle = static_cast<double>(i) * params_->final_rotation_search_step;
+    // Prioritize previously selected approach angles
+    if (safe_approach_angle_.has_value()) {
+      angle += safe_approach_angle_.value();
+    }
+
+    // Create candidate pose
+    auto candidate_pose = target_pose;
+    candidate_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(angle);
+
+    nav_msgs::msg::Path candidate_path = best_trajectory;
+    geometry_msgs::msg::TwistStamped candidate_cmd_vel = best_cmd_vel;
+
+    // Validate the candidate
+    if (validateTargetPose(
+          candidate_pose, dist_to_target, candidate_path, costmap_transform,
+          candidate_cmd_vel))
+    {
+      double candidate_cost = getMaxCost(candidate_path, costmap_transform);
+
+      bool reversing = false;
+      if (params_->allow_backward && target_pose.pose.position.x < 0.0) {
+        reversing = true;
+      }
+      // Calculate ETA
+      double eta = 0.0;
+      for (size_t j = 1; j < candidate_path.poses.size(); ++j) {
+        auto current_pose = candidate_path.poses[j - 1];
+        auto next_pose = candidate_path.poses[j];
+        auto cmd = control_law_->calculateRegularVelocity(candidate_pose.pose, current_pose.pose,
+          reversing);
+        double speed = std::abs(cmd.linear.x);
+        // Avoid division by zero
+        speed = std::max(speed, 1e-3);
+        double step_dist = nav2_util::geometry_utils::euclidean_distance(
+          current_pose.pose, next_pose.pose);
+        double step_time = step_dist / speed;
+        eta += step_time;
+      }
+
+      // Selection logic: Pick the fastest among the safe ones
+      if (eta < best_eta) {
+        best_eta = eta;
+        if (candidate_cost < safety_cost) {
+          best_trajectory = candidate_path;
+          best_cmd_vel = candidate_cmd_vel;
+          target_pose = candidate_pose;
+          found_valid = true;
+          // Reuse known safe approach angle if still valid
+          if (safe_approach_angle_.value_or(1e3 /*Never in (-PI, PI]*/) == angle) {
+            break;
+          }
+          safe_approach_angle_ = angle;
+        }
+      }
+    }
+  }
+
+  return found_valid;
+}
+
 }  // namespace nav2_graceful_controller
 
 // Register this controller as a nav2_core plugin

nav2_graceful_controller/src/parameter_handler.cpp

@@ -76,6 +76,10 @@ ParameterHandler::ParameterHandler(
     plugin_name_ + ".footprint_scaling_factor", 0.25);
   params_.footprint_scaling_step = node->declare_or_get_parameter(
     plugin_name_ + ".footprint_scaling_step", 0.1);
+  params_.obstacle_cost_margin = node->declare_or_get_parameter(
+    plugin_name_ + ".obstacle_cost_margin", 1);
+  params_.final_rotation_search_step = node->declare_or_get_parameter(
+    plugin_name_ + ".final_rotation_search_step", 0.1);
   if (params_.initial_rotation && params_.allow_backward) {
     RCLCPP_WARN(
       logger_, "Initial rotation and allow backward parameters are both true, "
@@ -172,6 +176,8 @@ ParameterHandler::updateParametersCallback(
         params_.footprint_scaling_factor = parameter.as_double();
       } else if (param_name == plugin_name_ + ".footprint_scaling_step") {
         params_.footprint_scaling_step = parameter.as_double();
+      } else if (param_name == plugin_name_ + ".final_rotation_search_step") {
+        params_.final_rotation_search_step = parameter.as_double();
       }
     } else if (param_type == ParameterType::PARAMETER_BOOL) {
       if (param_name == plugin_name_ + ".initial_rotation") {
@@ -183,6 +189,10 @@ ParameterHandler::updateParametersCallback(
       } else if (param_name == plugin_name_ + ".use_collision_detection") {
         params_.use_collision_detection = parameter.as_bool();
       }
+    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
+      if (param_name == plugin_name_ + ".obstacle_cost_margin") {
+        params_.obstacle_cost_margin = parameter.as_int();
+      }
     }
   }
 }