Start/Goal frame is ignored in planner server

[PGotzmann]

Bug report

I recently found that while nav2_planner PlannerServer accepts goals in frames other than its global frame, this information is subsequently ignored in planners such as navfn_planneror smac_planner. This leads to strange errors as goal and start frame are silently treated as if they were in the same frame.

I personally would not expect such a behavior for a service taking a PoseStamped as an argument. Is there an obvious reason for this limitation that I am missing?

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • Foxy binaries
• Version or commit hash:
  • navigation2 main sources (d3fc28a)
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

• ensure a valid tf tree exists e.g. world->map->base_link
• initialize navigation2 with global_frame set to map
• call compute_path_to_pose action with a pose in world frame and navfn_planner

Expected behavior

Generally, if a PoseStamped is used in ROS, one silently assumes that the frame_id is handled by the underlying function and transformed to the required frame implicitly as long as the frame is part of a valid tf tree. At least I would expect an error message saying that the frame does not match the expected one as for example it is done in the planner plugin tutorial from nav2 docs.

Actual behavior

Both navfn_planner and smac_planner assume that the goal frame matches the global_frame and blindly ignore the frame_id.

[SteveMacenski]

PRs are always appreciated to implement your requested behavior.

We make the reasonable assumption that goal requests are coming in the same frame at the costmap https://github.com/ros-planning/navigation2/blob/main/nav2_navfn_planner/src/navfn_planner.cpp#L68 so if that's not the case, there's some clean up that could be done to support that.

[PGotzmann]

I totally agree that the goal request generally will be in the same frame as the costmap. My point was just that that is not what the interface suggests and that a goal request in another frame should be fine as long as its transformable in this frame and the results lies within costmap boundaries.

I can think of some cases where a goal in a different frame might be beneficial.
For example in my case there is a static transform world->map to align a graph map recorded with slam_toolbox with a global origin. Goals are generally given with respect to this global origin not the actual map frame.

Such a check/transformation could be implemented at two different levels, namely in PlannerServer::computePlan (https://github.com/ros-planning/navigation2/blob/main/nav2_planner/src/planner_server.cpp#L209) or in the implementation of createPlan of the respective planner plugin (https://github.com/ros-planning/navigation2/blob/main/nav2_navfn_planner/src/navfn_planner.cpp#L118).

Is this a feature you would like to see implemented, and if so, would you prefer is at a planner server or planner plugin level?

[SteveMacenski]

Is this a feature you would like to see implemented, and if so, would you prefer is at a planner server or planner plugin level?

Planner server level. By the time it gets to the plugins, they should only be working with costmap coordinates. So I'd recommend checking the frame_id and transforming it to the costmap's global frame ID if different. Or at minimum doing what you suggest and issuing an error if its wrong and returning.

[SteveMacenski]

Merging imminent