Controller::distToSegmentSquared() ignores z-component half the time

[lewie-donckers]

While working on unit tests for Controller::distToSegmentSquared() I noticed that the z-component of the inputs is ignored in some calculations but not in all.

I doubt that this is intended; it looks like a bug but perhaps someone can shed some light on this:

  const double l2 = distSquared(pose_v, pose_w); // z used for l2
  if (l2 == 0) {
    return pose_w; // <-- z included in result in this case. not in the other return
  }

  tf2::Transform result;

  const double t = std::clamp(
    ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) *
       (pose_w.getOrigin().x() - pose_v.getOrigin().x()) +
     (pose_p.getOrigin().y() - pose_v.getOrigin().y()) *
       (pose_w.getOrigin().y() - pose_v.getOrigin().y())) /
      l2,
    0.0, 1.0); // <-- z used in l2 but not in rest of calculation of t. which in turn is used for x and y of result.
  result.setOrigin(tf2::Vector3(
    pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()),
    pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); // <-- z forced to 0 in result

  const auto yaw = estimate_pose_angle ? atan2(
                                           pose_w.getOrigin().y() - pose_v.getOrigin().y(),
                                           pose_w.getOrigin().x() - pose_v.getOrigin().x())
                                       : tf2::getYaw(pose_v.getRotation());
  tf2::Quaternion pose_quaternion;
  pose_quaternion.setRPY(0.0, 0.0, yaw);
  result.setRotation(pose_quaternion);

  return result;

[rokusottervanger]

@cesar-lopez-mar Could you shine your light on this?

[cesar-lopez-mar]

We definitely should not use z components, unless we explicitly support also commands outside of the plane, which we do not want imo.

[Timple]

I agree with @cesar-lopez-mar, this planner is 2D only.

But as odometry input might be 3 dimensional, a z (or roll/pitch) component might be present.

So a test might actually be that the controller input is the same regardless of z value. (This sounds trivial if you strip all z-related stuff from the repository. But we do transformations which might take this into account under the hood.