diff --git a/.gitignore b/.gitignore index 88c775d303..7fb9789cb5 100644 --- a/.gitignore +++ b/.gitignore @@ -44,6 +44,7 @@ install # Python artifacts __pycache__/ *.py[cod] +.ipynb_checkpoints sphinx_doc/_build diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index e285e30e96..6ef3260d6b 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/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 diff --git a/nav2_regulated_pure_pursuit_controller/doc/circle-segment-intersection.ipynb b/nav2_regulated_pure_pursuit_controller/doc/circle-segment-intersection.ipynb new file mode 100644 index 0000000000..550238704e --- /dev/null +++ b/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 +} diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 157ead6519..15500cf056 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/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> global_path_pub_; diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index cd5532fc3c..9441f2d69f 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/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; diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index b8d7ecdad7..8b9b45821b 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -60,6 +60,14 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure return getLookAheadDistance(twist); } + static geometry_msgs::msg::Point circleSegmentIntersectionWrapper( + const geometry_msgs::msg::Point & p1, + const geometry_msgs::msg::Point & p2, + double r) + { + return circleSegmentIntersection(p1, p2, r); + } + geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( const double & dist, const nav_msgs::msg::Path & path) { @@ -185,6 +193,130 @@ TEST(RegulatedPurePursuitTest, findVelocitySignChange) EXPECT_EQ(rtn, std::numeric_limits::max()); } +using CircleSegmentIntersectionParam = std::tuple< + std::pair, + std::pair, + double, + std::pair +>; + +class CircleSegmentIntersectionTest + : public ::testing::TestWithParam +{}; + +TEST_P(CircleSegmentIntersectionTest, circleSegmentIntersection) +{ + auto pair1 = std::get<0>(GetParam()); + auto pair2 = std::get<1>(GetParam()); + auto r = std::get<2>(GetParam()); + auto expected_pair = std::get<3>(GetParam()); + auto pair_to_point = [](std::pair p) -> geometry_msgs::msg::Point { + geometry_msgs::msg::Point point; + point.x = p.first; + point.y = p.second; + point.z = 0.0; + return point; + }; + auto p1 = pair_to_point(pair1); + auto p2 = pair_to_point(pair2); + auto actual = BasicAPIRPP::circleSegmentIntersectionWrapper(p1, p2, r); + auto expected_point = pair_to_point(expected_pair); + EXPECT_DOUBLE_EQ(actual.x, expected_point.x); + EXPECT_DOUBLE_EQ(actual.y, expected_point.y); + // Expect that the intersection point is actually r away from the origin + EXPECT_DOUBLE_EQ(r, std::hypot(actual.x, actual.y)); +} + +INSTANTIATE_TEST_SUITE_P( + InterpolationTest, + CircleSegmentIntersectionTest, + testing::Values( + // Origin to the positive X axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {2.0, 0.0}, + 1.0, + {1.0, 0.0} +}, + // Origin to hte negative X axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-2.0, 0.0}, + 1.0, + {-1.0, 0.0} +}, + // Origin to the positive Y axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, 2.0}, + 1.0, + {0.0, 1.0} +}, + // Origin to the negative Y axis + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, -2.0}, + 1.0, + {0.0, -1.0} +}, + // non-origin to the X axis with non-unit circle, with the second point inside + CircleSegmentIntersectionParam{ + {4.0, 0.0}, + {-1.0, 0.0}, + 2.0, + {2.0, 0.0} +}, + // non-origin to the Y axis with non-unit circle, with the second point inside + CircleSegmentIntersectionParam{ + {0.0, 4.0}, + {0.0, -0.5}, + 2.0, + {0.0, 2.0} +}, + // origin to the positive X axis, on the circle + CircleSegmentIntersectionParam{ + {2.0, 0.0}, + {0.0, 0.0}, + 2.0, + {2.0, 0.0} +}, + // origin to the positive Y axis, on the circle + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {0.0, 2.0}, + 2.0, + {0.0, 2.0} +}, + // origin to the upper-right quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {6.0, 8.0}, + 5.0, + {3.0, 4.0} +}, + // origin to the lower-left quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-6.0, -8.0}, + 5.0, + {-3.0, -4.0} +}, + // origin to the upper-left quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {-6.0, 8.0}, + 5.0, + {-3.0, 4.0} +}, + // origin to the lower-right quadrant (3-4-5 triangle) + CircleSegmentIntersectionParam{ + {0.0, 0.0}, + {6.0, -8.0}, + 5.0, + {3.0, -4.0} +} +)); + TEST(RegulatedPurePursuitTest, lookaheadAPI) { auto ctrl = std::make_shared(); @@ -234,7 +366,12 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) auto pt = ctrl->getLookAheadPointWrapper(dist, path); EXPECT_EQ(pt.pose.position.x, 1.0); - // test getting next closest point + // test getting next closest point without interpolation + node->set_parameter( + rclcpp::Parameter( + name + ".use_interpolation", + rclcpp::ParameterValue(false))); + ctrl->configure(node, name, tf, costmap); dist = 3.8; pt = ctrl->getLookAheadPointWrapper(dist, path); EXPECT_EQ(pt.pose.position.x, 4.0); @@ -243,6 +380,16 @@ TEST(RegulatedPurePursuitTest, lookaheadAPI) dist = 100.0; pt = ctrl->getLookAheadPointWrapper(dist, path); EXPECT_EQ(pt.pose.position.x, 9.0); + + // test interpolation + node->set_parameter( + rclcpp::Parameter( + name + ".use_interpolation", + rclcpp::ParameterValue(true))); + ctrl->configure(node, name, tf, costmap); + dist = 3.8; + pt = ctrl->getLookAheadPointWrapper(dist, path); + EXPECT_EQ(pt.pose.position.x, 3.8); } TEST(RegulatedPurePursuitTest, rotateTests)