Official Path Point Orientation Guesstimator

[ardabbour]

Feature request

Feature description

Too many a time have I had to find hacks around orientations missing from planners. For Nav2, it seems only the smac planner (and not the 2D version) actually gives orientations. It would be good to have some standardization as to the algorithm producing orientation estimations. Why is this useful? Many controllers can/do make use of this information, such as a docking/parking controller or one based on some sort of trajectory optimization.

Implementation considerations

Something along the lines of the following:

// Warning: untested code!
auto guessPathHeadings(const nav_msgs::msg::Path& path)
{
  auto new_path = path;
  for (auto i = 0; i < path.poses.size() - 1; ++i)
  {
    tf2::Transform tf_curr, tf_next;
    tf2::fromMsg(path.poses.at(i).pose, tf_curr);
    tf2::fromMsg(path.poses.at(i + 1).pose, tf_next);
    const auto quat = tf2::shortestArcQuat(tf_curr.getOrigin(), tf_next.getOrigin());
    new_path.poses.at(i).pose.orientation = tf2::toMsg(quat);
  }
  new_path.poses.back().pose.orientation = path.poses.at(path.poses.size() - 2).pose.orientation;
  return new_path;
}

[SteveMacenski]

To make sure I understand, you mean placing orientations in path point markers? Some planners have these meaningfully, but many (like A* or Dij) don't. I wouldn't argue with adding a nav2_util on it though if someone submitted a PR with it used in some planner. 2D Smac seems like a reasonable enough target. This is also where we could argue semantics about definitions of a path vs plan vs trajectory but I don't think that's really important :wink:

new_path.poses.back() -> wouldn't you want this to be the goal orientation?

What controllers use this?

[ardabbour]

• One (naive) controller that uses this would be based on the one described in the 3rd chapter of Introduction to Autonomous Mobile Robots; a path made up of points that go in perpendicular to the 'default' orientation would cause jerky movement at best.
• Another would be an MPC for delicate parking / docking maneuvers - in such a scenario a plan with orientations would be of great help to the trajectory optimization problem
• You are certainly right about the goal orientation 😅

[SteveMacenski]

Another would be an MPC for delicate parking / docking maneuvers - in such a scenario a plan with orientations would be of great help to the trajectory optimization problem

Arguably, a situation like MPC where it wants to use the orientation as part of the optimization problem, it shouldn't be approximate interpolated values like what you're mentioning. MPC is touchy at best because of the optimization problem under the hood, things really shouldn't be approximated or it could diverge.

But point is taken, its a visually pleasing thing that isn't hard to add and "better is good". Is it strictly useful for a 2D A*? No - or at least not widely - since its not a feasible/smooth trajectory (e.g. guestimation would also then not be smooth), but doesn't mean it couldn't find use.

So adding in a "guesstimator" like what you proposed would be an additional I would merge a PR on if it were added to the 2D SMAC planner so that it had use somewhere in the code. A couple of simple 20 minute unit tests would be nice as well.

I would argue though that the planners that give this information out (state lattice, hybrid-A, RRT on Dubin motion models) aren't approximations so we give those out because they're meaningful - especially if a plan as a "reverse" for part of it to distinguish a forward/backward velocity profile. 2D-grid-search-land could make use of approximations, why not :shrug: I'm all about "better is good".

Keep in mind this information is always there, just not populated if its not an output of the algorithm (e.g. path is just points not poses). Feasible trajectory planners will / should populate those. I would like to add more feasible trajectory planners in the future to augment Hybrid-A* and enable trajectory planners for the minimum viable cross section of robot base types (ackermann, diff, omni, legged) that track orientation to allow for full-range non-circular support. Right now we only support ackermann and legged fully for non-circular support. State Lattice with a couple of generator scripts should cover the other two.

[SteveMacenski]

@ardabbour is this something you'd be interested in contributing? I don't know it should take any more than a couple of hours, but I just don't have the immediate cycles to work on it anytime soon. Its very useful, but so far no one else has claimed interest in working on it.

From your code block, it sounds like you've already made this and it just might need to be upstreamed and applied to NavFn / SmacPlanner2D :wink:

[ardabbour]

I might be able to squeeze it in an upcoming weekend; as you said just needs to be applied it to NavFn / SmacPlanner2D and tested :)

[SteveMacenski]

@ardabbour any progress, I could use it in the #2230 work I'm doing for a generic path smoother :wink:

[SteveMacenski]

For reference, this is my implementation in state_lattice branch to be merged into nav2 soonish

  inline void updateApproximatePathOrientations(nav_msgs::msg::Path & path)
  {
    using namespace nav2_util::geometry_utils;
    double dx, dy, theta;
    for (unsigned int i = 0; i != path.poses.size() - 1; i++) {
      dx = path.poses[i+1].pose.position.x - path.poses[i].pose.position.x;
      dy = path.poses[i+1].pose.position.y - path.poses[i].pose.position.y;
      theta = atan2(dy, dx);
      path.poses[i].pose.orientation = orientationAroundZAxis(theta);
    }
  }