feat(pure_pursuit): add predicted trajectory

[brkay54]

Signed-off-by: Berkay Karaman berkay@leodrive.ai

Description

Closes #2066

In current implementation of autoware universe, pure_pursuit does not create predicted trajectory. In this PR, pure_pursuit creates predicted trajectory by iterating estimated pose.

Related links

Tests performed

Tested in real vehicle tests, worked good.

Notes for reviewers

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[TakaHoribe]

@brkay54 Thank you for your great work!!!

In the code, the run() function, which is the main loop of the pure_pursuit, is getting a little messy. Would you simplify the function with a nice modification?

I want it to be like

boost::optional<LateralOutput> PurePursuitLateralController::run()
{
  if (!isDataReady()) {
    return boost::none;
  }

  // update closest_idx, remaining_distance, etc in member variables
  if (!updateData()) {
    return boost::none;
  }

  // calculate control command
  output.control_cmd = generateControlCommand();
  output.sync_data.is_steer_converged = calcIsSteerConverged():

  // calculate predicted trajectory with iterative calculation
  const auto predicted_trajectory = generatePredictedTrajectory();
  pub_predicted_trajectory_->publish(predicted_trajectory);

  return output;
}

(This is just an image. I'm not sure if it can be implemented.)

[codecov]

Codecov Report

Base: 11.89% // Head: 11.87% // Decreases project coverage by -0.02% ⚠️

Coverage data is based on head (5067346) compared to base (7c9f6e2).
Patch coverage: 0.00% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2115      +/-   ##
==========================================
- Coverage   11.89%   11.87%   -0.03%     
==========================================
  Files        1111     1111              
  Lines       80812    80991     +179     
  Branches    20737    20831      +94     
==========================================
  Hits         9614     9614              
- Misses      61072    61250     +178     
- Partials    10126    10127       +1     

Flag	Coverage Δ		*Carryforward flag
differential	12.93% <0.00%> (?)
total	11.86% <0.00%> (ø)		Carriedforward from 7c9f6e2

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files	Coverage Δ
...rsuit/include/pure_pursuit/util/planning_utils.hpp	0.00% <ø> (ø)
...c/pure_pursuit/pure_pursuit_lateral_controller.cpp	0.00% <0.00%> (ø)
...e_pursuit/src/pure_pursuit_core/planning_utils.cpp	0.00% <0.00%> (ø)

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[brkay54]

@TakaHoribe Thank you for your review, you are right, it was little messy. I organised the run() function, does it seem good to you now?.

[brkay54]

The following PRs should be merged after this PR merged:

• refactor(pure_pursuit): make lookahead distance calculation more tunable #2127
• feat(pure_pursuit): adding average trajectory smoother #2141

[mitsudome-r]

@TakaHoribe Would you take another look at this PR? @brkay54 seems to have updates to your comment.

[TakaHoribe]

THank you for the refactoring. Basically, it looks good to me! One minor comment before merge.

[xmfcx]

@TakaHoribe could you review this please?

[TakaHoribe]

LGTM! Please take one suggestion before merge.