fix(autoware_behavior_velocity_planner_common): add node clock, fix use sim time

[dmoszynski]

Description

The behavior_velocity_planner module has a bug that causes it to work incorrectly when use_sim_time == True - it should use the time published on /clock, however, it does not do this.

Related links

--

How was this PR tested?

I used tier4 evaluator scenario scenario for local testing. The tests included comparison of behavior for the same setup but different values of the use_sim_time parameter (either True or False).

• The result of the scenario is Passed for use_sim_time == False.

• The result of the scenario is Failure for use_sim_time == True, as the Ego, after stopping before the pedestrian crosswalk (and reaching a linear speed equal to 0.0000), immediately started moving again by reaching the linear small speed (less than 0.1000) and stopped again.

• In the scenario, such behavior is not allowed, so therefore it caused Failure and helped detect this issue.

• To get Passed I've relaxed the condition that checks the stopped state from speed<0.0001 to speed<0.1000.

After this change, the scenario seemed to run identically for both cases - after Ego stopped before the crosswalk, it gently moved and stopped again (reaching a speed less than 0.1000, so the conditions were met and got Passed).

• The change in Ego's behavior is related to the behavior_velocity_crosswalk_module - which, after the vehicle stops, transitions to IGNORE state - this transition does not occur in the case of use_sim_time == False.

• Below I've attached the video before the changes:

  • Ego stops when the crosswalk module is in YIELD state,
  • the crosswalk module transitions to IGNORE state,
  • Ego starts again and reaches linear velocity <0.1 (plot),
  • the crosswalk module transitions to YIELD state,
  • Ego stops again.
• issue.mp4

Reason

The reason by which the behavior_velocity_crosswalk_module transitions to the IGNORE state is a bug in the velocity buffer, when the parameter use_sim_time == True.

• To be more precise, the behavior_velocity_crosswalk_module behaves as if the velocity buffer is off and the planner_data_->isVehicleStopped() call (here) returns True as immediately as the linear velocity of Ego reaches 0.0.
• This is because the TwistStamped objects in the velocity buffer queue contain timestamps that are consistent with the simulation time (starts at zero) while in the isVehicleStopped() method they are compared with the system time (epoch) - it can be seen here.
• This is because the timestamp “now” is acquired using the static method rclcpp::Clock().now() instead of using clock_->now() - clock that is assigned to the node and takes into account the node parameter use_sim_time. (I realize that in the case of using the static rclcpp::Clock().now() method, the time from the topic /clock should be used, but this is not happening, even though the publication /clock starts before the start of the module and publishes continuously)

Fix

I’ve managed to fix this bug by making sure that module behavior_velocity_crosswalk_module uses the clock assigned to the node (takes into account use_sim_time) and now timestamps are correctly compared, velocity buffer works as expected. The changes can be seen in this PR. Below I've attached the video after the changes.

fixed.mp4

Notes for reviewers

Note: Analysis and tests were performed on the tier4/v.0.26.1 version.

Interface changes

None.

Effects on system behavior

The module behavior_velocity_crosswalk_module uses the clock assigned to the node (takes into account use_sim_time).

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[xmfcx]

@takayuki5168 san could you assign someone to review this? It's a very simple one.

[codecov]

Codecov Report

Attention: Patch coverage is 66.66667% with 1 line in your changes missing coverage. Please review.

Project coverage is 28.01%. Comparing base (9f7ba86) to head (0e98a7a).
Report is 53 commits behind head on main.

Files with missing lines	Patch %	Lines
.../behavior_velocity_planner_common/planner_data.hpp	66.66%	1 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #8876      +/-   ##
==========================================
- Coverage   28.02%   28.01%   -0.02%     
==========================================
  Files        1316     1320       +4     
  Lines       98601    98649      +48     
  Branches    39771    39773       +2     
==========================================
+ Hits        27636    27637       +1     
- Misses      70819    70866      +47     
  Partials      146      146              

Flag	Coverage Δ		*Carryforward flag
differential	20.56% <66.66%> (?)
total	28.02% <ø> (-0.01%)	⬇️	Carriedforward from 9f7ba86

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

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[takayuki5168]

LGTM
Thank you!

[takayuki5168]

@rej55 @soblin @shmpwk
Please approve the PR.