Skip to content

Commit a3b230b

Browse files
author
Guillaume Doisy
committed
projectCarrotPastGoal test
Signed-off-by: Guillaume Doisy <[email protected]>
1 parent 00219c2 commit a3b230b

1 file changed

Lines changed: 117 additions & 0 deletions

File tree

‎nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,14 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure
6868
return circleSegmentIntersection(p1, p2, r);
6969
}
7070

71+
geometry_msgs::msg::PoseStamped
72+
projectCarrotPastGoalWrapper(
73+
const double & dist,
74+
const nav_msgs::msg::Path & path)
75+
{
76+
return getLookAheadPoint(dist, path, true);
77+
}
78+
7179
geometry_msgs::msg::PoseStamped getLookAheadPointWrapper(
7280
const double & dist, const nav_msgs::msg::Path & path)
7381
{
@@ -331,6 +339,115 @@ INSTANTIATE_TEST_SUITE_P(
331339
}
332340
));
333341

342+
TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
343+
auto ctrl = std::make_shared<BasicAPIRPP>();
344+
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testRPP");
345+
std::string name = "PathFollower";
346+
auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
347+
auto costmap =
348+
std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
349+
rclcpp_lifecycle::State state;
350+
costmap->on_configure(state);
351+
ctrl->configure(node, name, tf, costmap);
352+
353+
double EPSILON = std::numeric_limits<float>::epsilon();
354+
355+
nav_msgs::msg::Path path;
356+
// More than 2 poses
357+
path.poses.resize(4);
358+
path.poses[0].pose.position.x = 0.0;
359+
path.poses[1].pose.position.x = 1.0;
360+
path.poses[2].pose.position.x = 2.0;
361+
path.poses[3].pose.position.x = 3.0;
362+
auto pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
363+
EXPECT_NEAR(pt.pose.position.x, 10.0, EPSILON);
364+
EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
365+
366+
// 2 poses fwd
367+
path.poses.clear();
368+
path.poses.resize(2);
369+
path.poses[0].pose.position.x = 2.0;
370+
path.poses[1].pose.position.x = 3.0;
371+
pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
372+
EXPECT_NEAR(pt.pose.position.x, 12.0, EPSILON);
373+
EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
374+
375+
// 2 poses at 45°
376+
path.poses.clear();
377+
path.poses.resize(2);
378+
path.poses[0].pose.position.x = 2.0;
379+
path.poses[0].pose.position.y = 2.0;
380+
path.poses[1].pose.position.x = 3.0;
381+
path.poses[1].pose.position.y = 3.0;
382+
pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
383+
EXPECT_NEAR(pt.pose.position.x, cos(45.0 * M_PI / 180) * 10.0 + 2.0, EPSILON);
384+
EXPECT_NEAR(pt.pose.position.y, sin(45.0 * M_PI / 180) * 10.0 + 2.0, EPSILON);
385+
386+
// 2 poses at 90°
387+
path.poses.clear();
388+
path.poses.resize(2);
389+
path.poses[0].pose.position.x = 0.0;
390+
path.poses[0].pose.position.y = 2.0;
391+
path.poses[1].pose.position.x = 0.0;
392+
path.poses[1].pose.position.y = 3.0;
393+
pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
394+
EXPECT_NEAR(pt.pose.position.x, cos(90.0 * M_PI / 180) * 10.0, EPSILON);
395+
EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0 + 2.0, EPSILON);
396+
397+
// 2 poses at 135°
398+
path.poses.clear();
399+
path.poses.resize(2);
400+
path.poses[0].pose.position.x = -2.0;
401+
path.poses[0].pose.position.y = 2.0;
402+
path.poses[1].pose.position.x = -3.0;
403+
path.poses[1].pose.position.y = 3.0;
404+
pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
405+
EXPECT_NEAR(pt.pose.position.x, cos(135.0 * M_PI / 180) * 10.0 - 2.0, EPSILON);
406+
EXPECT_NEAR(pt.pose.position.y, sin(135.0 * M_PI / 180) * 10.0 + 2.0, EPSILON);
407+
408+
// 2 poses bck
409+
path.poses.clear();
410+
path.poses.resize(2);
411+
path.poses[0].pose.position.x = -2.0;
412+
path.poses[1].pose.position.x = -3.0;
413+
pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
414+
EXPECT_NEAR(pt.pose.position.x, -12.0, EPSILON);
415+
EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);
416+
417+
// 2 poses at -135°
418+
path.poses.clear();
419+
path.poses.resize(2);
420+
path.poses[0].pose.position.x = -2.0;
421+
path.poses[0].pose.position.y = -2.0;
422+
path.poses[1].pose.position.x = -3.0;
423+
path.poses[1].pose.position.y = -3.0;
424+
pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
425+
EXPECT_NEAR(pt.pose.position.x, cos(-135.0 * M_PI / 180) * 10.0 - 2.0, EPSILON);
426+
EXPECT_NEAR(pt.pose.position.y, sin(-135.0 * M_PI / 180) * 10.0 - 2.0, EPSILON);
427+
428+
// 2 poses at -90°
429+
path.poses.clear();
430+
path.poses.resize(2);
431+
path.poses[0].pose.position.x = 0.0;
432+
path.poses[0].pose.position.y = -2.0;
433+
path.poses[1].pose.position.x = 0.0;
434+
path.poses[1].pose.position.y = -3.0;
435+
pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
436+
EXPECT_NEAR(pt.pose.position.x, cos(-90.0 * M_PI / 180) * 10.0, EPSILON);
437+
EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0 - 2.0, EPSILON);
438+
439+
// 2 poses at -45°
440+
path.poses.clear();
441+
path.poses.resize(2);
442+
path.poses[0].pose.position.x = 2.0;
443+
path.poses[0].pose.position.y = -2.0;
444+
path.poses[1].pose.position.x = 3.0;
445+
path.poses[1].pose.position.y = -3.0;
446+
pt = ctrl->projectCarrotPastGoalWrapper(10.0, path);
447+
EXPECT_NEAR(pt.pose.position.x, cos(-45.0 * M_PI / 180) * 10.0 + 2.0, EPSILON);
448+
EXPECT_NEAR(pt.pose.position.y, sin(-45.0 * M_PI / 180) * 10.0 - 2.0, EPSILON);
449+
}
450+
334451
TEST(RegulatedPurePursuitTest, lookaheadAPI)
335452
{
336453
auto ctrl = std::make_shared<BasicAPIRPP>();

0 commit comments

Comments
 (0)