Add Interpolation Logic to Graceful Controller

Calculate Lookahead Point using Interpolation for Graceful Controller.
Support both for in trajectory and goal extrapolation.

- Add parameters for lookahead resolution and interpolate after goal.
- Add logic to calculate lookahead point using interpolation.

Goal Extrapolation can be switched on and off using `interpolate_after_goal` parameter.

Signed-off-by: Sakshay Mahna <sakshum19@gmail.com>

Author: SakshayMahna

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

@@ -107,6 +107,32 @@ class GracefulController : public nav2_core::Controller
   void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
 
 protected:
+  /**
+   * @brief Find the intersection a circle and a line segment.
+   * This assumes the circle is centered at the origin.
+   * If no intersection is found, a floating point error will occur.
+   * @param p1 first endpoint of line segment
+   * @param p2 second endpoint of line segment
+   * @param r radius of circle
+   * @return point of intersection
+   */
+  static geometry_msgs::msg::Point circleSegmentIntersection(
+    const geometry_msgs::msg::Point & p1,
+    const geometry_msgs::msg::Point & p2,
+    double r);
+
+  /**
+   * @brief Get lookahead point
+   * @param lookahead_dist Optimal lookahead distance
+   * @param path Current global path
+   * @param interpolate_after_goal If true, interpolate the lookahead point after the goal based
+   * on the orientation given by the position of the last two pose of the path
+   * @return Lookahead point
+   */
+  geometry_msgs::msg::PoseStamped getLookAheadPoint(
+    const double &, const nav_msgs::msg::Path &,
+    bool interpolate_after_goal = false);
+
   /**
    * @brief Simulate trajectory calculating in every step the new velocity command based on
    * a new curvature value and checking for collisions.

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -35,6 +35,8 @@ struct Parameters
   double transform_tolerance;
   double min_lookahead;
   double max_lookahead;
+  double lookahead_resolution;
+  bool interpolate_after_goal;
   double max_robot_pose_search_dist;
   double k_phi;
   double k_delta;

nav2_graceful_controller/params/graceful_controller_params.yaml

@@ -110,6 +110,8 @@ controller_server:
       transform_tolerance: 0.1
       min_lookahead: 0.25
       max_lookahead: 1.0
+      lookahead_resolution: 0.1
+      interpolate_after_goal: true
       initial_rotation: true
       initial_rotation_threshold: 0.75
       prefer_final_rotation: true

nav2_graceful_controller/src/graceful_controller.cpp

@@ -193,21 +193,19 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
     // Else, fall through and see if we should follow control law longer
   }
 
-  // Precompute distance to candidate poses
-  std::vector<double> target_distances;
-  computeDistanceAlongPath(transformed_plan.poses, target_distances);
+  // Calculate pose for reducing lookaheads
+  float lookahead = params_->max_lookahead;
 
-  // Work back from the end of plan to find valid target pose
-  for (int i = transformed_plan.poses.size() - 1; i >= 0; --i) {
+  while (lookahead > params_->min_lookahead) {
     // Underlying control law needs a single target pose, which should:
     //  * Be as far away as possible from the robot (for smoothness)
     //  * But no further than the max_lookahed_ distance
     //  * Be feasible to reach in a collision free manner
-    geometry_msgs::msg::PoseStamped target_pose = transformed_plan.poses[i];
-    double dist_to_target = target_distances[i];
+    geometry_msgs::msg::PoseStamped target_pose = getLookAheadPoint(
+      lookahead, transformed_plan, params_->interpolate_after_goal);
 
-    // Continue if target_pose is too far away from robot
-    if (dist_to_target > params_->max_lookahead) {continue;}
+    // Reduce lookahead
+    lookahead = lookahead - params_->lookahead_resolution;
 
     if (dist_to_goal < params_->max_lookahead) {
       if (params_->prefer_final_rotation) {
@@ -216,9 +214,6 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
         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
-      break;
     }
 
     // Flip the orientation of the motion target if the robot is moving backwards
@@ -281,6 +276,128 @@ void GracefulController::setSpeedLimit(
   }
 }
 
+geometry_msgs::msg::Point GracefulController::circleSegmentIntersection(
+  const geometry_msgs::msg::Point & p1,
+  const geometry_msgs::msg::Point & p2,
+  double r)
+{
+  // Formula for intersection of a line with a circle centered at the origin,
+  // modified to always return the point that is on the segment between the two points.
+  // https://mathworld.wolfram.com/Circle-LineIntersection.html
+  // This works because the poses are transformed into the robot frame.
+  // This can be derived from solving the system of equations of a line and a circle
+  // which results in something that is just a reformulation of the quadratic formula.
+  // Interactive illustration in doc/circle-segment-intersection.ipynb as well as at
+  // https://www.desmos.com/calculator/td5cwbuocd
+  double x1 = p1.x;
+  double x2 = p2.x;
+  double y1 = p1.y;
+  double y2 = p2.y;
+
+  double dx = x2 - x1;
+  double dy = y2 - y1;
+  double dr2 = dx * dx + dy * dy;
+  double D = x1 * y2 - x2 * y1;
+
+  // Augmentation to only return point within segment
+  double d1 = x1 * x1 + y1 * y1;
+  double d2 = x2 * x2 + y2 * y2;
+  double dd = d2 - d1;
+
+  geometry_msgs::msg::Point p;
+  double sqrt_term = std::sqrt(r * r * dr2 - D * D);
+  p.x = (D * dy + std::copysign(1.0, dd) * dx * sqrt_term) / dr2;
+  p.y = (-D * dx + std::copysign(1.0, dd) * dy * sqrt_term) / dr2;
+  return p;
+}
+
+geometry_msgs::msg::PoseStamped GracefulController::getLookAheadPoint(
+  const double & lookahead_dist,
+  const nav_msgs::msg::Path & transformed_plan,
+  bool interpolate_after_goal)
+{
+  // Find the first pose which is at a distance greater than the lookahead distance
+  auto goal_pose_it = std::find_if(
+    transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) {
+      return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist;
+    });
+
+  // If the no pose is not far enough, take the last pose
+  if (goal_pose_it == transformed_plan.poses.end()) {
+    if (interpolate_after_goal) {
+      auto last_pose_it = std::prev(transformed_plan.poses.end());
+      auto prev_last_pose_it = std::prev(last_pose_it);
+
+      double end_path_orientation = atan2(
+        last_pose_it->pose.position.y - prev_last_pose_it->pose.position.y,
+        last_pose_it->pose.position.x - prev_last_pose_it->pose.position.x);
+
+      // Project the last segment out to guarantee it is beyond the look ahead
+      // distance
+      auto projected_position = last_pose_it->pose.position;
+      projected_position.x += cos(end_path_orientation) * lookahead_dist;
+      projected_position.y += sin(end_path_orientation) * lookahead_dist;
+
+      // Use the circle intersection to find the position at the correct look
+      // ahead distance
+      const auto interpolated_position = circleSegmentIntersection(
+        last_pose_it->pose.position, projected_position, lookahead_dist);
+
+      // Calculate orientation towards interpolated position
+      // Convert yaw to quaternion
+      double interpolated_yaw = atan2(
+        interpolated_position.y - last_pose_it->pose.position.y,
+        interpolated_position.x - last_pose_it->pose.position.x);
+      
+      geometry_msgs::msg::Quaternion interpolated_orientation;
+      interpolated_orientation.x = 0.0;
+      interpolated_orientation.y = 0.0;
+      interpolated_orientation.z = sin(interpolated_yaw / 2.0);
+      interpolated_orientation.w = cos(interpolated_yaw / 2.0);
+
+      geometry_msgs::msg::PoseStamped interpolated_pose;
+      interpolated_pose.header = last_pose_it->header;
+      interpolated_pose.pose.position = interpolated_position;
+      interpolated_pose.pose.orientation = interpolated_orientation;
+
+      return interpolated_pose;
+    } else {
+      goal_pose_it = std::prev(transformed_plan.poses.end());
+    }
+  } else if (goal_pose_it != transformed_plan.poses.begin()) {
+    // Find the point on the line segment between the two poses
+    // that is exactly the lookahead distance away from the robot pose (the origin)
+    // This can be found with a closed form for the intersection of a segment and a circle
+    // Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
+    // and goal_pose is guaranteed to be outside the circle.
+    auto prev_pose_it = std::prev(goal_pose_it);
+    auto point = circleSegmentIntersection(
+      prev_pose_it->pose.position,
+      goal_pose_it->pose.position, lookahead_dist);
+
+    // Calculate orientation towards interpolated position
+    // Convert yaw to quaternion
+    double yaw = atan2(
+      point.y - prev_pose_it->pose.position.y,
+      point.x - prev_pose_it->pose.position.x);
+
+    geometry_msgs::msg::Quaternion orientation;
+    orientation.x = 0.0;
+    orientation.y = 0.0;
+    orientation.z = sin(yaw / 2.0);
+    orientation.w = cos(yaw / 2.0);
+
+    geometry_msgs::msg::PoseStamped pose;
+    pose.header.frame_id = prev_pose_it->header.frame_id;
+    pose.header.stamp = goal_pose_it->header.stamp;
+    pose.pose.position = point;
+    pose.pose.orientation = orientation;
+    return pose;
+  }
+
+  return *goal_pose_it;
+}
+
 bool GracefulController::simulateTrajectory(
   const geometry_msgs::msg::PoseStamped & motion_target,
   const geometry_msgs::msg::TransformStamped & costmap_transform,

nav2_graceful_controller/src/parameter_handler.cpp

@@ -41,6 +41,10 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".min_lookahead", rclcpp::ParameterValue(0.25));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_lookahead", rclcpp::ParameterValue(1.0));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".lookahead_resolution", rclcpp::ParameterValue(0.1));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".interpolate_after_goal", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_robot_pose_search_dist",
     rclcpp::ParameterValue(costmap_size_x / 2.0));
@@ -74,6 +78,8 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
   node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead);
   node->get_parameter(plugin_name_ + ".max_lookahead", params_.max_lookahead);
+  node->get_parameter(plugin_name_ + ".lookahead_resolution", params_.lookahead_resolution);
+  node->get_parameter(plugin_name_ + ".interpolate_after_goal", params_.interpolate_after_goal);
   node->get_parameter(
     plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist);
   if (params_.max_robot_pose_search_dist < 0.0) {
@@ -141,6 +147,8 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
         params_.min_lookahead = parameter.as_double();
       } else if (name == plugin_name_ + ".max_lookahead") {
         params_.max_lookahead = parameter.as_double();
+      } else if (name == plugin_name_ + ".lookahead_resolution") {
+        params_.lookahead_resolution = parameter.as_double();
       } else if (name == plugin_name_ + ".k_phi") {
         params_.k_phi = parameter.as_double();
       } else if (name == plugin_name_ + ".k_delta") {
@@ -179,6 +187,8 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
         params_.initial_rotation = parameter.as_bool();
       } else if (name == plugin_name_ + ".prefer_final_rotation") {
         params_.prefer_final_rotation = parameter.as_bool();
+      } else if (name == plugin_name_ + ".interpolate_after_goal") {
+        params_.interpolate_after_goal = parameter.as_bool();
       } else if (name == plugin_name_ + ".allow_backward") {
         if (params_.initial_rotation && parameter.as_bool()) {
           RCLCPP_WARN(