Rpp interpolation (#2872)

* add lookahead interpolation test

* add use_lookahead_point_interpolation parameter

* initial implementation for lookahead interpolation

* fix lookahead interpolation unit test

* add use_lookahead_point_interpolation parameter to README

* replace std::pow with multiplication

* use double rather than auto

* simplify circleSegmentIntersection

* convert circleSegmentIntersection to method

* added more unit tests for circleSegmentIntersection

* fix circleSegmentIntersection to return correct intersection

* added interactive jupyter notebook to document interpolation formula

* black autoformat intersection notebook

* more comments on rpp interpolation

* added more unit tests for circleSegmentIntersection

* rename use_lookahead_point_interpolation to use_interpolation

* enable rpp interpolation by default

* fix circle-segment-intersection notebook

* formatting

.gitignore

@@ -44,6 +44,7 @@ install
 # Python artifacts
 __pycache__/
 *.py[cod]
+.ipynb_checkpoints
 
 sphinx_doc/_build
 

nav2_regulated_pure_pursuit_controller/README.md

@@ -76,6 +76,7 @@ Note: The maximum allowed time to collision is thresholded by the lookahead poin
 | `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `use_rotate_to_heading` is enabled. | 
 | `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | 
 | `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | 
+| `use_interpolation` | Enables interpolation between poses on the path for lookahead point selection. Helps sparse paths to avoid inducing discontinuous commanded velocities. Set this to false for a potential performance boost, at the expense of smooth control. | 
 
 Example fully-described XML with default parameter values:
 
@@ -123,6 +124,7 @@ controller_server:
       rotate_to_heading_min_angle: 0.785
       max_angular_accel: 3.2
       max_robot_pose_search_dist: 10.0
+      use_interpolation: false
       cost_scaling_dist: 0.3
       cost_scaling_gain: 1.0
       inflation_cost_scaling_factor: 3.0

nav2_regulated_pure_pursuit_controller/doc/circle-segment-intersection.ipynb

@@ -0,0 +1,170 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "id": "97dbdadd-7a94-4939-8ed5-c8551b662917",
+   "metadata": {},
+   "source": [
+    "# Circle Segment Intersection (for interpolation)\n",
+    "Here is an interactive plot that demonstrates the functionality of the formula to calculate the intersection of a line segment, and a circle centered at the origin."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 8,
+   "id": "d31dc723-a6dc-400d-8b31-fe84ea6d5e45",
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "application/vnd.jupyter.widget-view+json": {
+       "model_id": "cbfad4e8309a4ee2bef53994add83330",
+       "version_major": 2,
+       "version_minor": 0
+      },
+      "text/plain": [
+       "VBox(children=(Label(value='A and B can be moved with the mouse. One must be inside the circle, and one must b…"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    }
+   ],
+   "source": [
+    "from bqplot import *\n",
+    "import numpy as np\n",
+    "import ipywidgets as widgets\n",
+    "\n",
+    "\n",
+    "def circle_segment_intersection(p1, p2, r):\n",
+    "    x1, y1 = p1\n",
+    "    x2, y2 = p2\n",
+    "    dx = x2 - x1\n",
+    "    dy = y2 - y1\n",
+    "    dr2 = dx ** 2 + dy ** 2\n",
+    "    D = x1 * y2 - x2 * y1\n",
+    "    d1 = x1 ** 2 + y1 ** 2\n",
+    "    d2 = x2 ** 2 + y2 ** 2\n",
+    "    dd = d2 - d1\n",
+    "    sqrt_term = np.sqrt(r ** 2 * dr2 - D ** 2)\n",
+    "    x = (D * dy + np.copysign(1.0, dd) * dx * sqrt_term) / dr2\n",
+    "    y = (-D * dx + np.copysign(1.0, dd) * dy * sqrt_term) / dr2\n",
+    "    return x, y\n",
+    "\n",
+    "\n",
+    "MAX = 5.0\n",
+    "x_sc = LinearScale(min=-MAX, max=MAX)\n",
+    "y_sc = LinearScale(min=-MAX, max=MAX)\n",
+    "\n",
+    "ax_x = Axis(label=\"x\", scale=x_sc, tick_format=\"0.0f\")\n",
+    "ax_y = Axis(label=\"y\", scale=y_sc, orientation=\"vertical\", tick_format=\"0.0f\")\n",
+    "\n",
+    "points = Scatter(\n",
+    "    names=[\"A\", \"B\"], x=[0.0, 3.0], y=[2.0, 4.0], scales={\"x\": x_sc, \"y\": y_sc}, enable_move=True\n",
+    ")\n",
+    "\n",
+    "\n",
+    "def get_circle(r):\n",
+    "    t = np.linspace(0, 2 * np.pi)\n",
+    "    x = r * np.cos(t)\n",
+    "    y = r * np.sin(t)\n",
+    "    return x, y\n",
+    "\n",
+    "radius_slider = widgets.FloatSlider(min=0.0, max=MAX, value=3.0, description=\"Circle radius\")\n",
+    "circle_x, circle_y = get_circle(radius_slider.value)\n",
+    "\n",
+    "circle = Lines(x=circle_x, y=circle_y, scales={\"x\": x_sc, \"y\": y_sc}, colors=[\"green\"])\n",
+    "\n",
+    "x1, x2 = points.x\n",
+    "y1, y2 = points.y\n",
+    "xi, yi = circle_segment_intersection((x1, y1), (x2, y2), radius_slider.value)\n",
+    "\n",
+    "intersection = Scatter(\n",
+    "    names=[\"C\"],\n",
+    "    x=[xi],\n",
+    "    y=[yi],\n",
+    "    scales={\"x\": x_sc, \"y\": y_sc},\n",
+    "    enable_move=False,\n",
+    "    colors=[\"purple\"],\n",
+    ")\n",
+    "\n",
+    "fig = Figure(axes=[ax_x, ax_y], marks=[circle, points, intersection])\n",
+    "\n",
+    "fig.max_aspect_ratio = 1\n",
+    "fig.min_aspect_ratio = 1\n",
+    "\n",
+    "\n",
+    "def both_inside_or_both_outside_circle(points, r):\n",
+    "    x1, x2 = points.x\n",
+    "    y1, y2 = points.y\n",
+    "    d1 = x1 ** 2 + y1 ** 2\n",
+    "    d2 = x2 ** 2 + y2 ** 2\n",
+    "    if d1 < r ** 2 and d2 < r ** 2:\n",
+    "        return True\n",
+    "    elif d1 > r ** 2 and d2 > r ** 2:\n",
+    "        return True\n",
+    "    else:\n",
+    "        return False\n",
+    "\n",
+    "\n",
+    "def update_circle(message):\n",
+    "    circle_x, circle_y = get_circle(radius_slider.value)\n",
+    "    circle.x = circle_x\n",
+    "    circle.y = circle_y\n",
+    "    update_intersection(message)\n",
+    "\n",
+    "\n",
+    "def update_intersection(message):\n",
+    "    x1, x2 = points.x\n",
+    "    y1, y2 = points.y\n",
+    "    r = radius_slider.value\n",
+    "    if both_inside_or_both_outside_circle(points, r):\n",
+    "        circle.colors = [\"red\"]\n",
+    "        intersection.x = []\n",
+    "        intersection.y = []\n",
+    "    else:\n",
+    "        circle.colors = [\"green\"]\n",
+    "        xi, yi = circle_segment_intersection((x1, y1), (x2, y2), r)\n",
+    "        intersection.x = [xi]\n",
+    "        intersection.y = [yi]\n",
+    "\n",
+    "\n",
+    "points.observe(update_intersection, [\"x\", \"y\"])\n",
+    "\n",
+    "radius_slider.observe(update_circle, \"value\")\n",
+    "\n",
+    "widgets.VBox(\n",
+    "    [\n",
+    "        widgets.Label(\n",
+    "            \"A and B can be moved with the mouse. One must be inside the circle, and one must be outside.\",\n",
+    "            fixed=True,\n",
+    "        ),\n",
+    "        radius_slider,\n",
+    "        fig,\n",
+    "    ]\n",
+    ")"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.8.10"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -222,6 +222,20 @@ class RegulatedPurePursuitController : public nav2_core::Controller
     const double & curvature, const geometry_msgs::msg::Twist & speed,
     const double & pose_cost, double & linear_vel, double & sign);
 
+  /**
+   * @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
@@ -282,6 +296,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller
   double goal_dist_tol_;
   bool allow_reversing_;
   double max_robot_pose_search_dist_;
+  bool use_interpolation_;
 
   nav_msgs::msg::Path global_plan_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -108,6 +108,9 @@ void RegulatedPurePursuitController::configure(
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_robot_pose_search_dist",
     rclcpp::ParameterValue(getCostmapMaxExtent()));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".use_interpolation",
+    rclcpp::ParameterValue(true));
 
   node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_);
   base_desired_linear_vel_ = desired_linear_vel_;
@@ -153,6 +156,9 @@ void RegulatedPurePursuitController::configure(
   node->get_parameter(
     plugin_name_ + ".max_robot_pose_search_dist",
     max_robot_pose_search_dist_);
+  node->get_parameter(
+    plugin_name_ + ".use_interpolation",
+    use_interpolation_);
 
   transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
   control_duration_ = 1.0 / control_frequency;
@@ -372,6 +378,41 @@ void RegulatedPurePursuitController::rotateToHeading(
   angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
 }
 
+geometry_msgs::msg::Point RegulatedPurePursuitController::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 RegulatedPurePursuitController::getLookAheadPoint(
   const double & lookahead_dist,
   const nav_msgs::msg::Path & transformed_plan)
@@ -385,6 +426,21 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin
   // If the no pose is not far enough, take the last pose
   if (goal_pose_it == transformed_plan.poses.end()) {
     goal_pose_it = std::prev(transformed_plan.poses.end());
+  } else if (use_interpolation_ && 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);
+    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;
+    return pose;
   }
 
   return *goal_pose_it;