From 4e4d81de2e7b7b6260449b4d1bace684c6b84158 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Fri, 15 Mar 2024 14:40:52 -0400 Subject: [PATCH] RPP Dexory tweaks (#4140) * RPP Dexory tweaks Signed-off-by: Guillaume Doisy * Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp Signed-off-by: Steve Macenski * Update nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp Signed-off-by: Steve Macenski * projectCarrotPastGoal test Signed-off-by: Guillaume Doisy * Use circleSegmentIntersection when projecting past end of path This guarantees that the look ahead distance is maintained Signed-off-by: James Ward * lint Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Signed-off-by: Steve Macenski Signed-off-by: James Ward Co-authored-by: Guillaume Doisy Co-authored-by: Steve Macenski Co-authored-by: James Ward --- .../CMakeLists.txt | 2 + .../README.md | 2 + .../parameter_handler.hpp | 1 + .../path_handler.hpp | 3 +- .../regulated_pure_pursuit_controller.hpp | 12 +- .../package.xml | 1 + .../src/parameter_handler.cpp | 28 ++-- .../src/path_handler.cpp | 16 ++- .../src/regulated_pure_pursuit_controller.cpp | 64 ++++++++-- .../test/test_regulated_pp.cpp | 120 +++++++++++++++++- 10 files changed, 218 insertions(+), 31 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 6f84b05382..78beab89fe 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(nav2_regulated_pure_pursuit_controller) find_package(ament_cmake REQUIRED) +find_package(nav2_amcl REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) @@ -26,6 +27,7 @@ set(dependencies nav2_costmap_2d pluginlib nav_msgs + nav2_amcl nav2_util nav2_core tf2 diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 350b1926da..57b820b16d 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -90,6 +90,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. | +| `interpolate_curvature_after_goal` | Needs use_fixed_curvature_lookahead to be true. Interpolate a carrot after the goal dedicated to the curvate calculation (to avoid oscilaltions at the end of the path) | Example fully-described XML with default parameter values: @@ -137,6 +138,7 @@ controller_server: rotate_to_heading_min_angle: 0.785 max_angular_accel: 3.2 max_robot_pose_search_dist: 10.0 + interpolate_curvature_after_goal: 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/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp index 39f5de6fa0..d6ca585a49 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -56,6 +56,7 @@ struct Parameters double rotate_to_heading_min_angle; bool allow_reversing; double max_robot_pose_search_dist; + bool interpolate_curvature_after_goal; bool use_collision_detection; double transform_tolerance; }; diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp index 22bc0266a6..1e603f3c8b 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -58,11 +58,12 @@ class PathHandler * - Outside the local_costmap (collision avoidance cannot be assured) * @param pose pose to transform * @param max_robot_pose_search_dist Distance to search for matching nearest path point + * @param reject_unit_path If true, fail if path has only one pose * @return Path in new frame */ nav_msgs::msg::Path transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist); + double max_robot_pose_search_dist, bool reject_unit_path = false); /** * @brief Transform a pose to another frame. 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 7b28ca9720..417dc1c811 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 @@ -131,10 +131,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @brief Whether robot should rotate to rough path heading * @param carrot_pose current lookahead point * @param angle_to_path Angle of robot output relatie to carrot marker + * @param x_vel_sign Velocoty sign (forward or backward) * @return Whether should rotate to path heading */ bool shouldRotateToPath( - const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path); + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path, + double & x_vel_sign); /** * @brief Whether robot should rotate to final goal orientation @@ -185,9 +187,13 @@ class RegulatedPurePursuitController : public nav2_core::Controller * @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 &); + geometry_msgs::msg::PoseStamped getLookAheadPoint( + const double &, const nav_msgs::msg::Path &, + bool interpolate_after_goal = false); /** * @brief checks for the cusp position @@ -210,6 +216,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; + std::shared_ptr> + curvature_carrot_pub_; std::shared_ptr> carrot_arc_pub_; std::unique_ptr path_handler_; std::unique_ptr param_handler_; diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 2b2a6ff96b..e8a1865288 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -10,6 +10,7 @@ ament_cmake + nav2_amcl nav2_common nav2_core nav2_util diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp index 016a626c28..dea2bc59a4 100644 --- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -90,6 +90,9 @@ ParameterHandler::ParameterHandler( declare_parameter_if_not_declared( node, plugin_name_ + ".max_robot_pose_search_dist", rclcpp::ParameterValue(costmap_size_x / 2.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".interpolate_curvature_after_goal", + rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true)); @@ -159,6 +162,15 @@ ParameterHandler::ParameterHandler( params_.max_robot_pose_search_dist = std::numeric_limits::max(); } + node->get_parameter( + plugin_name_ + ".interpolate_curvature_after_goal", + params_.interpolate_curvature_after_goal); + if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) { + RCLCPP_WARN( + logger_, "For interpolate_curvature_after_goal to be set to true, " + "use_fixed_curvature_lookahead should be true, it is currently set to false. Disabling."); + params_.interpolate_curvature_after_goal = false; + } node->get_parameter( plugin_name_ + ".use_collision_detection", params_.use_collision_detection); @@ -170,16 +182,6 @@ ParameterHandler::ParameterHandler( params_.use_cost_regulated_linear_velocity_scaling = false; } - /** Possible to drive in reverse direction if and only if - "use_rotate_to_heading" parameter is set to false **/ - - if (params_.use_rotate_to_heading && params_.allow_reversing) { - RCLCPP_WARN( - logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. By default setting use_rotate_to_heading true"); - params_.allow_reversing = false; - } - dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind( &ParameterHandler::dynamicParametersCallback, @@ -250,12 +252,6 @@ ParameterHandler::dynamicParametersCallback( } else if (name == plugin_name_ + ".use_collision_detection") { params_.use_collision_detection = parameter.as_bool(); } else if (name == plugin_name_ + ".use_rotate_to_heading") { - if (parameter.as_bool() && params_.allow_reversing) { - RCLCPP_WARN( - logger_, "Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. Rejecting parameter update."); - continue; - } params_.use_rotate_to_heading = parameter.as_bool(); } else if (name == plugin_name_ + ".allow_reversing") { if (params_.use_rotate_to_heading && parameter.as_bool()) { diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp index 90b7c4a3e0..66b19be323 100644 --- a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -47,12 +47,17 @@ double PathHandler::getCostmapMaxExtent() const nav_msgs::msg::Path PathHandler::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose, - double max_robot_pose_search_dist) + double max_robot_pose_search_dist, + bool reject_unit_path) { if (global_plan_.poses.empty()) { throw nav2_core::InvalidPath("Received plan with zero length"); } + if (reject_unit_path && global_plan_.poses.size() == 1) { + throw nav2_core::InvalidPath("Received plan with length of one"); + } + // let's get the pose of the robot in the frame of the plan geometry_msgs::msg::PoseStamped robot_pose; if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { @@ -73,6 +78,15 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan( return euclidean_distance(robot_pose, ps); }); + // Make sure we always have at least 2 points on the transformed plan and that we don't prune + // the global plan below 2 points in order to have always enough point to interpolate the + // end of path direction + if (global_plan_.poses.begin() != closest_pose_upper_bound && global_plan_.poses.size() > 1 && + transformation_begin == std::prev(closest_pose_upper_bound)) + { + transformation_begin = std::prev(std::prev(closest_pose_upper_bound)); + } + // We'll discard points on the plan that are outside the local costmap const double max_costmap_extent = getCostmapMaxExtent(); auto transformation_end = std::find_if( 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 efcd24ea2d..7394d1e92c 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 @@ -20,6 +20,7 @@ #include #include +#include "nav2_amcl/angleutils.hpp" #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" #include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" @@ -73,6 +74,8 @@ void RegulatedPurePursuitController::configure( global_path_pub_ = node->create_publisher("received_global_plan", 1); carrot_pub_ = node->create_publisher("lookahead_point", 1); + curvature_carrot_pub_ = node->create_publisher( + "curvature_lookahead_point", 1); } void RegulatedPurePursuitController::cleanup() @@ -84,6 +87,7 @@ void RegulatedPurePursuitController::cleanup() plugin_name_.c_str()); global_path_pub_.reset(); carrot_pub_.reset(); + curvature_carrot_pub_.reset(); } void RegulatedPurePursuitController::activate() @@ -95,6 +99,7 @@ void RegulatedPurePursuitController::activate() plugin_name_.c_str()); global_path_pub_->on_activate(); carrot_pub_->on_activate(); + curvature_carrot_pub_->on_activate(); } void RegulatedPurePursuitController::deactivate() @@ -106,6 +111,7 @@ void RegulatedPurePursuitController::deactivate() plugin_name_.c_str()); global_path_pub_->on_deactivate(); carrot_pub_->on_deactivate(); + curvature_carrot_pub_->on_deactivate(); } std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( @@ -171,7 +177,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Transform path to robot base frame auto transformed_plan = path_handler_->transformGlobalPlan( - pose, params_->max_robot_pose_search_dist); + pose, params_->max_robot_pose_search_dist, params_->interpolate_curvature_after_goal); global_path_pub_->publish(transformed_plan); // Find look ahead distance and point on path and publish @@ -190,6 +196,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Get the particular point on the path at the lookahead distance auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + auto rotate_to_path_carrot_pose = carrot_pose; carrot_pub_->publish(createCarrotMsg(carrot_pose)); double linear_vel, angular_vel; @@ -200,33 +207,39 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity if (params_->use_fixed_curvature_lookahead) { auto curvature_lookahead_pose = getLookAheadPoint( params_->curvature_lookahead_dist, - transformed_plan); + transformed_plan, params_->interpolate_curvature_after_goal); + rotate_to_path_carrot_pose = curvature_lookahead_pose; regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position); + curvature_carrot_pub_->publish(createCarrotMsg(curvature_lookahead_pose)); } // Setting the velocity direction - double sign = 1.0; + double x_vel_sign = 1.0; if (params_->allow_reversing) { - sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; + x_vel_sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; } linear_vel = params_->desired_linear_vel; // Make sure we're in compliance with basic constraints + // For shouldRotateToPath, using x_vel_sign in order to support allow_reversing + // and rotate_to_path_carrot_pose for the direction carrot pose: + // - equal to "normal" carrot_pose when curvature_lookahead_pose = false + // - otherwise equal to curvature_lookahead_pose (which can be interpolated after goal) double angle_to_heading; if (shouldRotateToGoalHeading(carrot_pose)) { double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation); rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed); - } else if (shouldRotateToPath(carrot_pose, angle_to_heading)) { + } else if (shouldRotateToPath(rotate_to_path_carrot_pose, angle_to_heading, x_vel_sign)) { rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed); } else { applyConstraints( regulation_curvature, speed, collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, - linear_vel, sign); + linear_vel, x_vel_sign); // Apply curvature to angular velocity after constraining linear velocity - angular_vel = linear_vel * lookahead_curvature; + angular_vel = linear_vel * regulation_curvature; } // Collision checking on this velocity heading @@ -246,10 +259,15 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } bool RegulatedPurePursuitController::shouldRotateToPath( - const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path, + double & x_vel_sign) { // Whether we should rotate robot to rough path heading angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); + // In case we are reversing + if (x_vel_sign < 0.0) { + angle_to_path = nav2_amcl::angleutils::normalize(angle_to_path + M_PI); + } return params_->use_rotate_to_heading && fabs(angle_to_path) > params_->rotate_to_heading_min_angle; } @@ -314,7 +332,8 @@ geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersect geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( const double & lookahead_dist, - const nav_msgs::msg::Path & transformed_plan) + 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( @@ -324,7 +343,32 @@ 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()); + 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); + + geometry_msgs::msg::PoseStamped interpolated_pose; + interpolated_pose.header = last_pose_it->header; + interpolated_pose.pose.position = interpolated_position; + 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) 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 1557dad2d2..38cc195bc6 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -68,6 +68,14 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure return circleSegmentIntersection(p1, p2, r); } + geometry_msgs::msg::PoseStamped + projectCarrotPastGoalWrapper( + const double & dist, + const nav_msgs::msg::Path & path) + { + return getLookAheadPoint(dist, path, true); + } + geometry_msgs::msg::PoseStamped getLookAheadPointWrapper( const double & dist, const nav_msgs::msg::Path & path) { @@ -77,7 +85,8 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure bool shouldRotateToPathWrapper( const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) { - return shouldRotateToPath(carrot_pose, angle_to_path); + double x_vel_sign = 1.0; + return shouldRotateToPath(carrot_pose, angle_to_path, x_vel_sign); } bool shouldRotateToGoalHeadingWrapper(const geometry_msgs::msg::PoseStamped & carrot_pose) @@ -330,6 +339,115 @@ INSTANTIATE_TEST_SUITE_P( } )); +TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) { + auto ctrl = std::make_shared(); + auto node = std::make_shared("testRPP"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = + std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + ctrl->configure(node, name, tf, costmap); + + double EPSILON = std::numeric_limits::epsilon(); + + nav_msgs::msg::Path path; + // More than 2 poses + path.poses.resize(4); + path.poses[0].pose.position.x = 0.0; + path.poses[1].pose.position.x = 1.0; + path.poses[2].pose.position.x = 2.0; + path.poses[3].pose.position.x = 3.0; + auto pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); + + // 2 poses fwd + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 2.0; + path.poses[1].pose.position.x = 3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); + + // 2 poses at 45° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 2.0; + path.poses[0].pose.position.y = 2.0; + path.poses[1].pose.position.x = 3.0; + path.poses[1].pose.position.y = 3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(45.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at 90° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 0.0; + path.poses[0].pose.position.y = 2.0; + path.poses[1].pose.position.x = 0.0; + path.poses[1].pose.position.y = 3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(90.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at 135° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = -2.0; + path.poses[0].pose.position.y = 2.0; + path.poses[1].pose.position.x = -3.0; + path.poses[1].pose.position.y = 3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses bck + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = -2.0; + path.poses[1].pose.position.x = -3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, -10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON); + + // 2 poses at -135° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = -2.0; + path.poses[0].pose.position.y = -2.0; + path.poses[1].pose.position.x = -3.0; + path.poses[1].pose.position.y = -3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(-135.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at -90° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 0.0; + path.poses[0].pose.position.y = -2.0; + path.poses[1].pose.position.x = 0.0; + path.poses[1].pose.position.y = -3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(-90.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0, EPSILON); + + // 2 poses at -45° + path.poses.clear(); + path.poses.resize(2); + path.poses[0].pose.position.x = 2.0; + path.poses[0].pose.position.y = -2.0; + path.poses[1].pose.position.x = 3.0; + path.poses[1].pose.position.y = -3.0; + pt = ctrl->projectCarrotPastGoalWrapper(10.0, path); + EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0, EPSILON); +} + TEST(RegulatedPurePursuitTest, lookaheadAPI) { auto ctrl = std::make_shared();