Proposal for handling goal modification

[kosuke55]

BackGround

There are cases where the goal is dynamically modified by planning modules.
ex) modify the goal to an available parking space when there is an obstacle near the goal in pull_over.
Pull Over Goal Search

For such cases, the following processings should be taken.

• ArrivedGoal judgment is made considering the modified goal
• Rerouting is performed for the modified goal while driving

Proposal

We would like to take a step-by-step approach, so "Initial Step" shows what we would like to implement at this time, and "Final Step" shows an image of what we would like to implement at final.

Initial Step (only modify goal)

• Send route from mission_planner to behavior_path_planner.
• Modify the goal in behavior_path and send it back to mission_planner.
• Make ArrivedGoal judgment for the modified goal.
  • Until the modified goal is returned from behavior_path, the goal of the original route is used for ArrivedGoal judgment.

(/tmp needs to be determined later)

Add new msg

Create a new type to add uuid to route and modified goal.

The uuid is needed to determine whether to use the route sent from AD API to MissionPlanner or modified goal.
For example, when a route is sent from AD API to Mission Planner and the modified goal has not been received yet, it is necessary to judge whether the modified goal is for the current route or the previous one.

msg change target and usage (red frame)

• update by adding uuid to existing type
  • HADMapRoute
• create new custom msg
  • msg with uuid added to PoseStamped

modified goal stamp

Use the stamp of the timing when behavior_path_planner is triggered for modified goal. It is the same as path_with_lane_id's one.

modified goal publish timing

Only when the goal is modified and do not publish in a module that does not have a modification function.

Final Step (modify goal and reroute while driving)

If the goal position is significantly revised, it is assumed that the route will deviate greatly from the original route.

• Send route from mission_planner to behavior_path_planner.
• Modify the goal in behavior_path and send it back to mission_planner.
• Make ArrivedGoal judgment for the modified goal.
  • Until the modified goal is returned from behavior_path, the goal of the original route is used for ArrivedGoal judgment.
• Reroute with the modified goal and send it to other nodes as a modified route
  • Consideration should be given to avoiding abrupt path changes, and it should be possible to do so while driving.

The information to be sent from Mission Planner to AD API, which is todo, needs to be determined later considering Apps. At a minimum, the original route and modified route seem enough.

[kosuke55]

cc @yukkysaito @mitsudome-r @isamu-takagi

[kosuke55]

@isamu-takagi
Yes, I would prefer to have the header at the top, given that we can use MessageFilter, and I think it would be easier to understand with the same structure as the other msgs.
The only drawback is that it requires some conversion, but I don't think it would be a big problem.

before

pose_stamped_with_uuid.pose_stamped = pose_stamped;

after

pose_stamped_with_uuid.pose = pose_stamped.pose;
pose_stamped_with_uuid.header =  pose_stamped.header;

[isamu-takagi]

I got it. It looks good.

[kosuke55]

thanks for your comment.

@isamu-takagi @yukkysaito @xmfcx

• change the message
  • feat(autoware_planning_msgs): move header to the top in PoseWithUuidStamped autoware_msgs#50
• updated modified goal
  • feat(behavior_path_planner): modified goal with uuid autoware.universe#2602
• add rviz plugin
  • feat(tier4_planning_rviz_plugin): visualize pose_with_uuid_stamped autoware.universe#2662

[kosuke55]

I have merged the above PRs.
@isamu-takagi
May I ask you to support uuid in mission_planner?:pray:

[isamu-takagi]

I added id to mission planner. It's random id instead of uuid for now.
autowarefoundation/autoware.universe#2746