feat(operation_mode_transition_manager): add package to manage vehicle autonomous mode change

[TakaHoribe]

Description

• Add OperationModeTransitionManager that
  • checks the availability of the Autonomous mode change
  • publish the availability status
  • manage the operation_mode transition (Now only Manual <-> Autonomous is only supported)
  • send control_mode_request when the autonomous mode change is requested and the mode change is permitted.

Related links

Tests performed

Make sure related PRs are already merged in your testing environment:

• feat(simple_planning_simulator): add control_mode server #1061
• feat(trajectory_follower_nodes): add simple trajectory follower #1198
• feat(tier4_vehicle_msgs): add new msgs for engage-on-driving tier4/tier4_autoware_msgs#36
• fix: move IsAutonomousAvailable.msg from vehicle_msgs to system_msgs tier4/tier4_autoware_msgs#41
• feat(vehicle cmd gate): add transition filter #1244
• feat(longitudinal_controller): add disable emergency option #1201
• feat(operator): support autoware operation mode service tier4/tier4_ad_api_adaptor#51

1. run planning simulator, set initial pose and goal.
2. send control mode MANUAL to simulator vehicle

# command 0: MANUAL, 1: AUTO
ros2 service call /control/control_mode_request tier4_vehicle_msgs/srv/ControlModeRequest "{mode: {data: 0}}"

3. check if the control mode is now MANUAL

# report: 1: AUTO, 2: MANUAL
ros2 topic echo /vehicle/status/control_mode 

4. run controller to publish manual driving command and check the vehicle is moving following the trajectory

ros2 launch trajectory_follower_nodes simple_trajectory_follower.launch.xml external_target_vel:=-1.0

5. check if the AUTO mode is available (if the /autoware/engage is false and the vehicle is moving, make sure the AUTO is not available)

ros2 topic echo /control/is_autonomous_available

6. set autoware/engage = true (you can use api or send command directly)

ros2 topic pub /autoware/engage autoware_auto_vehicle_msgs/msg/Engage "{engage: 1}"

7. make sure the is_autonomous_available topic is true. (You can check the detailed info for the prohibit reason in debug topic).

ros2 topic echo /control/operation_mode_transition_manager/debug_info

8. send AUTO mode to vehicle and make sure the mode is changed to AUTO. The vehicle velocity will be changed to follow the reference trajectory speed.

# 1: DRIVER, 2: AUTONOMOUS
ros2 service call /api/autoware/set/operator tier4_external_api_msgs/srv/SetOperator "{mode: {mode: 2}}"

limitation and future work

• write readme (will be in separate PR created soon)
• Implementation
  • Currently implemented as a node, but could be integrated into the vehicle_cmd_gate as a class in the future PR because of the strong connection to the vehicle_cmd_gate
  • add a check function if the filter in the vehicle_cmd_gate is not active when releasing a transition mode
  • gate filter is not working when the mode is transitted from MANUAL to TRANSITION.

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 #1246 (a911633) into main (4542c52) will decrease coverage by 0.17%.
The diff coverage is 0.00%.

@@           Coverage Diff            @@
##            main   #1246      +/-   ##
========================================
- Coverage   9.67%   9.49%   -0.18%     
========================================
  Files       1052    1057       +5     
  Lines      72567   72800     +233     
  Branches   15818   15781      -37     
========================================
- Hits        7019    6913     -106     
- Misses     59227   59869     +642     
+ Partials    6321    6018     -303     

Flag	Coverage Δ		*Carryforward flag
differential	40.74% <0.00%> (?)
total	9.52% <ø> (-0.14%)	⬇️	Carriedforward from 87b4412

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

Impacted Files	Coverage Δ
...include/operation_mode_transition_manager/data.hpp	0.00% <0.00%> (ø)
...tion_manager/operation_mode_transition_manager.hpp	0.00% <0.00%> (ø)
...nclude/operation_mode_transition_manager/state.hpp	0.00% <0.00%> (ø)
...rol/operation_mode_transition_manager/src/data.cpp	0.00% <0.00%> (ø)
..._manager/src/operation_mode_transition_manager.cpp	0.00% <0.00%> (ø)
...ol/operation_mode_transition_manager/src/state.cpp	0.00% <0.00%> (ø)
...ator/nodes/costmap_generator/points_to_costmap.cpp	0.00% <0.00%> (-82.36%)	⬇️
...vehicle_model/sim_model_ideal_steer_acc_geared.cpp	64.15% <0.00%> (-9.44%)	⬇️
...planning_evaluator/src/planning_evaluator_node.cpp	37.11% <0.00%> (-1.04%)	⬇️
map/lanelet2_extension/lib/query.cpp	9.52% <0.00%> (ø)
... and 87 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 4542c52...a911633. Read the comment docs.

[yukkysaito]

LGTM

[tkimura4]

This PR depends on tier4/tier4_ad_api_adaptor#51

[tkimura4]

LGTM

2022-07-07-19-32-46.mp4

[TakaHoribe]

@tkimura4 thank you. I'll merge after tier4/tier4_ad_api_adaptor#51 is merged

[tkimura4]

I confirmed that when the vechile is stopped the engage command is always acceptable if engage_allowed_for_stopped_vehicle is true

2022-07-07-22-55-26.mp4

[TakaHoribe]

Perfectly working as expected.
Although this is a situation where we want to prohibit the engage obviously.

#1246 (comment)