[RPP] Z offsets cause lookahead point to not match received global plan

[Aposhian]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Humble
• Version or commit hash:
  • 1.1.0
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

Have a significant z offset between the path and the robot (yes, this is an error condition, but the behavior I'm going to describe makes it hard to detect).
This results in the transformed plan in the robot frame (/received_global_plan) having non-zero z values, but the lookahead point in RPP is given a constant z value of 0.01. If there is a slight yaw in the frame as well, this results in an apparent offset between the lookahead point and the received global plan from an orthographic view in rviz, which should seem impossible, since the lookahead point is interpolated between the poses of the received global plan.

Expected behavior

Lookahead point always is in line with received global plan.

Actual behavior

Lookahead point appears to be out of line with the received global plan.

Additional information

I think this could be resolved one of two ways:

• zero out the z values of the received global plan before publishing (we are doing 2D control after all in the local area around the robot)
• publish the lookahead point to have a constant z offset relative to the z values in the received global plan.

Given that the second option would require moving to a spherical interpolation algorithm (or just taking the average z value of the two points, which could result in some edge cases), I propose just zeroing the values of the received global plan in the robot frame, since projecting in that frame is practically what the controller is doing anyway. If there are distortions because of bad transforms, then you should be able to see it in the visualization at that point.