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 30d1cb7fa6..75bebd7a89 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) { @@ -331,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, 12.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 + 2.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0 + 2.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 + 2.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 - 2.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0 + 2.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, -12.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 - 2.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0 - 2.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 - 2.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 + 2.0, EPSILON); + EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0 - 2.0, EPSILON); +} + TEST(RegulatedPurePursuitTest, lookaheadAPI) { auto ctrl = std::make_shared();