MBF must ensure that the robot is stopped when checking for goal reached

[corot]

That is, when parameter mbf_tolerance_check is true, we also need to check that the robot is almost stopped. Like in the base_local_planner:

  bool stopped(const nav_msgs::Odometry& base_odom, 
      const double& rot_stopped_velocity, const double& trans_stopped_velocity){
    return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity 
      && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
      && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
  }

But wee need to merge #189 to have actual robot speed to check that, so I'll do in a PR after merging that.

[corot]

Actually, I'm rethinking the whole concept of MBF checking for goal reached and having its own tolerance values. Because:

• Every controller I know ignores these tolerance values passed in isGoalReached method, so users will be surprised to see that the values the pass in the ExePAth goal are just ignored. Moreover, it's not easy to change most controllers to use these values, as in all cases I know, the tolerances are not check in isGoalReached, but in computeVelocityCmd.
• If mbf_tolerance_check is true, MBF nevertheless still allows the controller to decide if we have arrived. So it can enforce bigger tolerances, but not smaller than those configured in the controller.
• If MBF decides that we have arrived, it will not do a smooth stop, as the controllers should do.

So I wonder if we should improve it, implementing this and making mbf_tolerance_check completely overrule the controller on deciding if we have reached the goal (#206), or drop the feature entirely.
Thoughts? @spuetz , @Rayman , @reinzor , @dorezyuk ?

[reinzor]

I don't know exactly the history of this issue but it makes sense to give full responsibility to the controller plugin. I like the idea btw of passing goal tolerances in the ExePath goal. We are not using this currently but plan to in the near future. As a work around now we had to use different controllers with the same parameters except for the tolerances.

[Timple]

we also need to check that the robot is almost stopped

If that is desired. Maybe check if the robot almost reached its end velocity (often zero)?

[corot]

I don't know exactly the history of this issue but it makes sense to give full responsibility to the controller plugin. I like the idea btw of passing goal tolerances in the ExePath goal. We are not using this currently but plan to in the near future. As a work around now we had to use different controllers with the same parameters except for the tolerances.

Idea: add a method to the plugin interface to set the tolerances passed in the goal and restore the original values when done. Something like
void updateToleranceValues(double dist_tolerance, double angle_tolerance);
and remove the MBF goal reached check (and the 3 related parameters).

[reinzor]

But what is wrong with the current approach: the isGoalReached method? Why can't we pass the default tolerances or the goal tolerances if specified in the goal? This method still allows to implement custom logic for checking whether the robot has stopped or not (if that is not desired). Or maybe better, remove the isGoalReached method and provide the tolerances in the computeVelocities method and handle the "isGoalReached" via a return type.

[corot]

No, no, nothing wrong with isGoalReached. Problem is that, passing MBF tolerances to isGoalReached makes difficoult to use them in the plugins, as it would require a full refactoring of their code (move the code for checking if goal is reached to isGoalReached, instead of just checking a flag):

• TEB
• BLP

Much easier is to change the internal tolerance values that the controller plugin uses. We can do this in a new updateToleranceValues method, or possible better, as @reinzor suggests, pass in computeVelocities (with default values to NaN if the tolerances are not passed with the goal).

Thinking better, this last option makes the most sense. And indeed is the approach used for the global planner. Only problem I see is if the plugin uses the configured tolerance values outside of the computeVelocities. In that case, you need to rewrite the code, or risk using possibly different tolerance values in different parts of the code. Like DWA, that delegates the check on the latchedStopRotateController_: #L164.

[Rayman]

I've looked at the controllers I've written, and these have custom logic to determine when the goal is reached. My only requirement is that that should always be possible.

On the tolerance values, I'm using the same pattern as the DWA planner, i.e. a LocalPlannerUtil that caches the tolerance values provided in the reconfigure hook. It would be nice to have the latest tolerance values passed into the action, but it would take some work on my part. I'm curios to see what comes out of this discussion.

[doisyg]

With the current state, with a small tolerance on the controller (teb) and a big one on the ExePath, once the goal is reached within the big tolerance, no more cmd vel are published. Potientially, the last published vel can be quite high.

I don't know either what's the best solution. But I am interested in the discussion.

On one hand, I like to have the controller configured with the smallest achievable tolerance and then having MBF overriding them according to the parameters of the action. On the other end, with the current implementation, no deceleration stop velocities are computed. Which is quite dangerous if you have no vel watchdog in your system (but nobody does that right?). This leaves it to your smoother or motor controller to stop the robot with the max configured deceleration after the watchdog expires.

Maybe generating deceleration vels directly from MBF in case the goal is reached ?

[spuetz]

Yeah you're absolutely right, this can be dangerous. We're planning to go in the direction of #206 Then it's up to the user if MBF checks if the goal is reached or not. In combination with the force_stop_at_goal this could be a solution where the user is aware of the result. But I also like the idea of generating declaration velocity commands.

[Rayman]

As an interesting observation: navigation2 has split goal checking
separate from the controller. I think this means that the user has more freedom to configure the behavior that they want.

I'm curious what the implications of that choice are. And I'm not sure where my custom goal checking logic belongs :)

[corot]

By now the solution will be:

• [DONE] merge Do not call isGoalReached on controller when mbf_tolerance_check parameter is true #206 to avoid the confusion of both the controller and MBF checking for goal reached
• write a big warning on mbf_tolerance_check param explaining what @doisyg mentioned, and recommending use it in combination with force_stop_at_goal.

But implementing this issue (check for robot stopped) is much trickier, though. Because if MBF is configured to enforce looser tolerances than the controller (let's say 1m distance), the later won't slow down until closer to its own tolerance (let's say 10 cm). So checking that the robot is stopped won't work. But generating deceleration vels directly from MBF sounds like invading the competences of the controller, and so MBF is not the right place.

So a final, more radical alternative, is to entirely remove this feature! But given it's already in use by some groups, doesn't sound a valid option

[doisyg]

So a final, more radical alternative, is to entirely remove this feature! But given it's already in use by some groups, doesn't sound a valid option

Please don't remove it, we use it too!

But generating deceleration vels directly from MBF sounds like invading the competences of the controller, and so MBF is not the right place.

What would be a proper alternative ? Using a cancel and ensure on the controller side that the robot is smoothly stopped ? Or adding a stop() function in the abstract interface and implementing it on the controller side ?

Or wait for navigation2 to be mature enough for all of us to make the switch ?

[spuetz]

We could also add an outcome success code for that the goal is reached to make it easier for developers. Which is then internally translated to success and finishing the execution. @doisyg We are not removing the MBF check for if the goal is reached.
If we implement a deceleration on the MBF side, then for sure with another parameter to enable it ;)

[corot]

Calling cancel on the controller sounds very reasonable.

…

On Tue, Oct 6, 2020 at 6:39 PM Sebastian Pütz ***@***.***> wrote: We could also add an outcome success code for that the goal is reached to make it easier for developers. Which is then internally translated to success and finishing the execution. @doisyg <https://github.com/doisyg> We are not removing the MBF check for if the goal is reached. If we implement a deceleration on the MBF side, then for sure with another parameter to enable it ;) — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub <#213 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AACOYMX2VLOCOYRTI3GHFI3SJLQVNANCNFSM4O5LHYIA> .