fix(trajectory_follower_nodes): change control running order to keep stopped state

[kosuke55]

Description

• fix keep stopped until the steer is converged when the new trajectory is received.
• steer convergence is checked in the lateral controller for a new trajectory. In order to pass it immediately to the longitudinal controller, the lateral controller must run first then the longitudinal.
  • If two are executed at the same time, the longitudinal controller uses the steer convergence information of the trajectory one step earlier and can not keep stopped.

Related links

#901

Tests performed

checked keeping stopped state until steer is converged when receiving a new trajectory.

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.

[codecov]

Codecov Report

Merging #1252 (1e0e9e2) into main (b190ea7) will increase coverage by 0.05%.
The diff coverage is 76.92%.

@@           Coverage Diff            @@
##            main   #1252      +/-   ##
========================================
+ Coverage   9.66%   9.71%   +0.05%     
========================================
  Files       1054    1056       +2     
  Lines      72647   72763     +116     
  Branches   15819   15863      +44     
========================================
+ Hits        7018    7070      +52     
- Misses     59307   59623     +316     
+ Partials    6322    6070     -252     

Flag	Coverage Δ		*Carryforward flag
differential	42.11% <80.00%> (?)
total	9.65% <57.14%> (+<0.01%)	⬆️	Carriedforward from 8a7b6e0

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

Impacted Files	Coverage Δ
...trajectory_follower/src/mpc_lateral_controller.cpp	39.19% <40.00%> (+5.28%)	⬆️
.../trajectory_follower_nodes/src/controller_node.cpp	48.61% <100.00%> (+6.42%)	⬆️
map/lanelet2_extension/lib/query.cpp	9.52% <0.00%> (ø)
map/lanelet2_extension/lib/utilities.cpp	8.89% <0.00%> (ø)
common/osqp_interface/src/osqp_interface.cpp	34.35% <0.00%> (ø)
map/lanelet2_extension/lib/visualization.cpp	0.00% <0.00%> (ø)
planning/route_handler/src/route_handler.cpp	0.00% <0.00%> (ø)
planning/rtc_interface/src/rtc_interface.cpp	0.00% <0.00%> (ø)
control/trajectory_follower/src/mpc_utils.cpp	62.71% <0.00%> (ø)
map/lanelet2_extension/lib/detection_area.cpp	0.00% <0.00%> (ø)
... and 85 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update b190ea7...1e0e9e2. Read the comment docs.

[tkimura4]

In my environment, the vehicle starts before the steer is converged.
Is it something wrong?

2022-07-06-21-41-42.mp4

[kosuke55]

hmm, I can not watch your video, but did you set following params true?

• enable_keep_stopped_until_steer_convergence
• keep_steer_control_until_converged

[tkimura4]

In parking scenario, it seems to work well.
Is it as your expected?

2022-07-06-22-00-39.mp4

[kosuke55]

In parking scenario, it seems to work well.
Is it as your expected?

yes, I expected it

[tkimura4]

LGTM

[kosuke55]

In my environment, the vehicle starts before the steer is converged.
Is it something wrong?

It takes time to propagate the new trajectory to the output command, but it does not wait for the first trajectory.
So add waiting time for it in 1e0e9e2

steer_converged-2022-07-06_23.31.47.mp4