Skip to content

Commit

Permalink
Use circleSegmentIntersection when projecting past end of path
Browse files Browse the repository at this point in the history 
This guarantees that the look ahead distance is maintained

Signed-off-by: James Ward <[email protected]>
  • Loading branch information
@james-ward
james-ward authored and Guillaume Doisy committed Mar 13, 2024
1 parent 4ce993b commit 39882d4
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 27 deletions.
30 changes: 15 additions & 15 deletions nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -351,21 +351,21 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin
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);

auto current_robot_pose_it = transformed_plan.poses.begin();

double distance_after_last_pose = lookahead_dist -
std::hypot(
last_pose_it->pose.position.x - current_robot_pose_it->pose.position.x,
last_pose_it->pose.position.y - current_robot_pose_it->pose.position.y);

geometry_msgs::msg::PoseStamped interpolated_point;
interpolated_point.header = last_pose_it->header;
interpolated_point.pose.position.x = last_pose_it->pose.position.x +
cos(end_path_orientation) * distance_after_last_pose;
interpolated_point.pose.position.y = last_pose_it->pose.position.y +
sin(end_path_orientation) * distance_after_last_pose;

return interpolated_point;
// 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());
}
Expand Down
24 changes: 12 additions & 12 deletions nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -369,7 +369,7 @@ TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
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.x, 10.0, EPSILON);
EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);

// 2 poses at 45°
Expand All @@ -380,8 +380,8 @@ TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
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);
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();
Expand All @@ -392,7 +392,7 @@ TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
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);
EXPECT_NEAR(pt.pose.position.y, sin(90.0 * M_PI / 180) * 10.0, EPSILON);

// 2 poses at 135°
path.poses.clear();
Expand All @@ -402,16 +402,16 @@ TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
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);
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, -12.0, EPSILON);
EXPECT_NEAR(pt.pose.position.x, -10.0, EPSILON);
EXPECT_NEAR(pt.pose.position.y, 0.0, EPSILON);

// 2 poses at -135°
Expand All @@ -422,8 +422,8 @@ TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
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);
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();
Expand All @@ -434,7 +434,7 @@ TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
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);
EXPECT_NEAR(pt.pose.position.y, sin(-90.0 * M_PI / 180) * 10.0, EPSILON);

// 2 poses at -45°
path.poses.clear();
Expand All @@ -444,8 +444,8 @@ TEST(RegulatedPurePursuitTest, projectCarrotPastGoal) {
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);
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)
Expand Down

0 comments on commit 39882d4

Please sign in to comment.