NavigateToPose distance_remaining wrong values

[Juancams]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Humble source
• Version or commit hash:
  • ros-humble-navigation2 1.1.12-1jammy.20240126.081240
• DDS implementation:
  • Cyclone DDS

Steps to reproduce issue

I'm using the feedback from the NavigateToPose action, showing how much distance I have left to reach the end

send_goal_options.feedback_callback =
      [this](rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::SharedPtr,
      const std::shared_ptr<const nav2_msgs::action::NavigateToPose::Feedback> feedback) {
        RCLCPP_INFO(node_->get_logger(), "Distance remaining: %f", feedback->distance_remaining);
      };

Expected behavior

I expect for the total distance from the current pose at the end. Visual example:

[INFO] [1707130943.860846039] [moveto_test]: Received feedback: 2.407172
[INFO] [1707130943.960828232] [moveto_test]: Received feedback: 2.373995
[INFO] [1707130944.060919236] [moveto_test]: Received feedback: 2.373995
[INFO] [1707130944.160828423] [moveto_test]: Received feedback: 2.373995
[INFO] [1707130944.260844053] [moveto_test]: Received feedback: 2.348995
[INFO] [1707130944.360855709] [moveto_test]: Received feedback: 2.348995
[INFO] [1707130944.760797423] [moveto_test]: Received feedback: 2.173995
[INFO] [1707130944.860845067] [moveto_test]: Received feedback: 2.123995
[INFO] [1707130944.960829884] [moveto_test]: Received feedback: 2.098995
[INFO] [1707130945.060839497] [moveto_test]: Received feedback: 2.048995
[INFO] [1707130945.160823729] [moveto_test]: Received feedback: 1.973995
[INFO] [1707130945.260834466] [moveto_test]: Received feedback: 1.923995

Actual behavior

At first it gives me some small values, for example 0.1, and then the total distance, for example 5.0, and it decreases until 0.0~. Visual example:

[INFO] [1707130943.060844887] [moveto_test]: Received feedback: 0.283800
[INFO] [1707130943.160948636] [moveto_test]: Received feedback: 0.283800
[INFO] [1707130943.260824600] [moveto_test]: Received feedback: 0.283800
[INFO] [1707130943.760881441] [moveto_test]: Received feedback: 0.283800
[INFO] [1707130943.860846039] [moveto_test]: Received feedback: 2.407172
[INFO] [1707130943.960828232] [moveto_test]: Received feedback: 2.373995
[INFO] [1707130944.060919236] [moveto_test]: Received feedback: 2.373995
[INFO] [1707130944.160828423] [moveto_test]: Received feedback: 2.373995
[INFO] [1707130944.260844053] [moveto_test]: Received feedback: 2.348995
[INFO] [1707130944.360855709] [moveto_test]: Received feedback: 2.348995
[INFO] [1707130944.760797423] [moveto_test]: Received feedback: 2.173995
[INFO] [1707130944.860845067] [moveto_test]: Received feedback: 2.123995
[INFO] [1707130944.960829884] [moveto_test]: Received feedback: 2.098995
[INFO] [1707130945.060839497] [moveto_test]: Received feedback: 2.048995
[INFO] [1707130945.160823729] [moveto_test]: Received feedback: 1.973995
[INFO] [1707130945.260834466] [moveto_test]: Received feedback: 1.923995

[SteveMacenski]

Its based on integrated paths distances, not simply L2 distance to the goal. So when replanning, the distances might jump when it needs to then go around something that the last path did not.

If you believe from the context of your testing that there is a problem, please debug a bit with your context (the segment of software is here) and let us know! Your example didn't give us much to work on, so please do a little sleuthing and let us know what you find or provide detail to reproduce. Are those changes in value after a replan?

[Juancams]

I was reviewing the code you mention, and I understand that when we call the action, several intermediate poses will be generated that the robot tries to reach until it reaches the end. This is true?

If so, I understand that when I receive the feedback it gives me the distance left to the closest pose. This is true?

If both are true, shouldn't it give the distance to the end point from the current one?

[SteveMacenski]

I don't understand what you said. Nav2 has global path planning which generates global dense routes to the goal. This is taking the path and finding the integrated distance along it from the robot's current pose to the goal pose.

[Juancams]

What I am trying to explain (and sorry, I thought that with the previous example I would be able to explain it), is that the distance obtained appears to have an error. I'll try to explain myself better:

The logical thing would be that when we command a navigation action to our robot, if we check the distance left, it will give us the length of the path. This is true? I understand that this length may vary, since it may be the case that you have to replan, and therefore, have to take another route that increases the distance.

This being how I think it works, when I obtain the distance I have left to reach my goal, I receive incorrect values. Imagine that my robot has to follow a path of 5 meters, the feedback gives me values of 0.1 - 0.5 meters for the first few seconds, which are not real, and then it gives me 5 meters and decreases. Shouldn't I give myself 5 meters from the beginning?

If you still don't understand my case, I can try to record a video in simulation, although my messages per terminal will be identical to the ones I attached previously.

[SteveMacenski]

if we check the distance left, it will give us the length of the path. This is true?

Indeed!

the feedback gives me values of 0.1 - 0.5 meters for the first few seconds, which are not real, and then it gives me 5 meters and decreases

Is this possibly the feedback given by the tree (which ticks at ~100hz) before the path is actually computed? Some timestamps of the feedback you have and the path computation would be valuable. Is this only the first few feedback messages for a new action or is this happening regularly?

Curious if you change blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path); to:

if (!blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path)) {
  return;
}

If that resolves the issue. If we don't have real path data yet, the ETA / distance remaining metrics are meaningless anyway. I'm not sure why you'd see small numbers and non-zero, but one thing at a time, I suppose :-)

Do you have an example path for me to look at when this happens? Does it look over itself?

[SteveMacenski]

Any update or thoughts?

[Juancams]

Hi,

Sorry for the delay in responding, I was running several tests on this.

I have carried out tests with the change you mention, but the wrong distance continues to appear at first. A test that I have added is to do it on a real robot, and it seems that it works correctly without applying the change you mention. I understand that it is a simulation problem, but I don't know the reason.

[SteveMacenski]

Does this happen with the basic nav2 tb3 demo launch files in nav2_bringup or only on your application? I need something I can reproduce to look into it myself. If it doesn't happen on our sample but does on your application, that might point in another direction than NavigateToPose having an error.

[Juancams]

I am testing directly with the navigation used in this simulator and with this robot's own navigation package

[SteveMacenski]

Can you test with our demo to see if it persists?

[Juancams]

Screencast.from.02-20-2024.09.21.12.PM.webm

As you can see, sometimes values appear that are 0 or incorrect for the remaining distance.

[SteveMacenski]

I can't seem to load the video, try uploading to youtube

[Juancams]

Link

[SteveMacenski]

Thank you, that's more clear. Is it always just the first messages? That could be easily fixed by the patch I linked to before, for the first time you call the action. I think you mentioned you didn't think that was true?

if (!blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path)) {
  return;
}

After the first time, then the path is still on the blackboard from the last run, so it passes with the last path's value. We can fix that by unsetting it (https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/include/behaviortree_cpp/blackboard.h#L211). That feature only exists in BT.CPPv4 which we're migrating to currently, so that will need to wait a couple of weeks before its available to fix that portion.

But in the meantime, can you re-test the patch above and tell me if it works for the first time you call the action (so that there's no opportunity for it to be set yet from a previous run)?

[Juancams]

I have tried the patch you mention, and I have recorded the test in the tb3 simulator, but the same thing keeps happening :(

I leave you here the link to the video