Handle infeasible global planner result when periodically replanning

[remco-r]

While a goal is active, and a new goal is given, say through rviz, and this new goal is infeasible, then mbf aborts the active goal (good behavior). While a goal is active, and through the planner_frequency > 0 this goal is replanned and no feasible path is found, nothing happens (bad behavior). There will be no active global path, and the local planner will continue on its old path.

I connected the replanning goal call with the same callback as the "manual" goal call. so that the current goal will be canceled if no feasible path is found upon replanning.

[corot]

Not 100% sure this is a good idea :thinking: Replanning can fail sporadically for whatever reason, but if the robot can keep moving towards the goal, it's better not to interrupt him, I guess.

[remco-r]

It's indeed a matter of strategy.

1. If your robot currently has only one goal, perhaps it should get as close as possible to it, even when sporadically no feasible global path can be found, or when there is actually no feasible path.

2. However, when going over a list of goals you may want to continue with the next one if you know that the current goal is infeasible, in stead of getting closer until the local planner finds out that its infeasible.

The question is: When replanning active, do you want to respond to the latest global planner run, or do you want to follow the last feasible path?

I must say i agree with you and lean towards 1. a bit more. Another option would be to create a parameter for this, something like handle_infeasible_replanning_outcome. I leave it up to you.

[corot]

I must say i agree with you and lean towards 1. a bit more. Another option would be to create a parameter for this, something like handle_infeasible_replanning_outcome. I leave it up to you.

yup, I was thinking about having the change under a parameter; I propose a more straightforward name, like stop_on_replan_failure, or stop_when_replan_fails

[remco-r]

implemented the stop_when_replan_fails as dynamic reconfigure param. However I'm having issues showing it in my rqt_reconfigure, so i only tested it by hard coding.

Also, i noticed replanningThread is a separate thread, which does access some member variables. I would be willing to make those atomic.

[remco-r]

That's fine. explanation link doesn't work though.

Great to know about the cfg recompilation issues. I was struggling with that.

[corot]

That's fine. explanation link doesn't work though.

Great to know about the cfg recompilation issues. I was struggling with that.

Link is to this file: mbf_abstract_nav/src/mbf_abstract_nav/__init__.py, more precisely this comment:

# Generic set of parameters required by any MBF-based navigation framework
# To use:
#
#  from mbf_abstract_nav import add_mbf_abstract_nav_params
#  gen = ParameterGenerator()
#  add_mbf_abstract_nav_params(gen)
#  ...
# WARN: parameters added here must be copied on the specific MBF implementation reconfigure callback, e.g. in:
#       https://github.com/magazino/move_base_flex/blob/master/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp#L130
# Also, you need to touch https://github.com/magazino/move_base_flex/blob/master/mbf_costmap_nav/cfg/MoveBaseFlex.cfg
# when recompiling to ensure configuration code is regenerated with the new parameters