Goal pose is not aligned to the map

[takayuki5168]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I'm convinced that this is not my fault but a bug.

Description

When the goal pose is given to Autoware, usually by "2D Goal Pose" rviz plugin, mission_planner recognized that the goal pose's z is 0, and publishes the goal pose marker whose z is 0.
This may cause some misleading to any calculation using the goal pose or visualization.

Expected behavior

For "2D Goal Pose", since it does not set a z value as the name suggests, mission planner should calculate the proper z value based on the lanelet so that the goal pose will be aligned to the lanelet.

Actual behavior

The goal pose's z is 0.

Steps to reproduce

Just set the goal pose with "2D Goal Pose" rviz plugin.

Versions

No response

Possible causes

No response

Additional context

No response

[takayuki5168]

@yukkysaito @mitsudome-r What do you think.
If the goal pose given to mission_planner may contain the z value in the future, we have to discuss more.

[takayuki5168]

cc @TakaHoribe @kosuke55

[takayuki5168]

@yukkysaito @mitsudome-r kindly ping.

[xmfcx]

We can republish the 2D Goal Pose from the mission planner with correct Z coordinates.

@angry-crab would like to work on this.

[mitsudome-r]

Additional description to @xmfcx's comment:

Right now, the mission_planner(global planner) is not capable of handling z value in the given pose/goal properly to search for closest lane in z direction so I don't think it's worth creating another rviz plugin. (I also feel uncomfortable to create a plugin that depends on a map message)

If it is just a visualization issue, we can make the mission_planner to realign the given goal pose with the lane height and republish the marker.