make the costmaps used for planning and controlling configurable

Hey,

I have a proposal to extend the current mbf-costmap-nav and to allow the users to specify on which map they want to run their planners/controllers.

Running a planner on the local costmap allows the users to "bridge the gap" between planners and controllers. In particular, this might help if in one costmap the robot configuration appears free, while being blocked on the other - something which happens if both costmaps have largely different resolutions or run at different update frequencies.

I don't have a clear application for the other use-case (running the controller on the global costmap) but implemented it just for completeness.

The default behavior does not change. The user may specify an optional "costmap" tag, taking either "global" or "local" arguments.

Example:


planners:
    - {name: global_planner_global_1,    type: global_planner/GlobalPlanner} # runs on the global costmap    
    - {name: global_planner_global_2,    type: global_planner/GlobalPlanner, costmap: "global"} # explicitly on the global costmap              
    - {name: global_planner_local_1,      type: global_planner/GlobalPlanner, costmap: "local"} # on the local costmap

We are currently evaluating this setup for our replanning. The pseudo code for the replanning might look something like the code below (note: this won't run, just demonstrates the idea)


from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
from mbf_msgs.msg import GetPathGoal, GetPathAction, ExePathAction, ExePathGoal
from actionlib import SimpleActionClient

get_path_ac = SimpleActionClient("/move_base_flex/get_path", GetPathAction)
exe_path_ac = SimpleActionClient("/move_base_flex/exe_path", ExePathAction)

def advance_along_path(global_path: Path, robot_pose: PoseStamped, distance: float) -> PoseStamped:
    """Return the pose in path which has the distance to the robot_pose."""
    return PoseStamped()

def current_robot_pose() -> PoseStamped:
    """Return the current robot pose."""
    return PoseStamped()

def get_local_path(global_path) -> Path():
    """Returns the local path."""
    if not global_path:
        raise RuntimeError("No Path")

    # Dummy value for the local costmap size.
    local_costmap_size: float = 10.

    goal = GetPathGoal()

    # Advance along the global path until we reach the last valid pose within the local costmap
    goal.target_pose = advance_along_path(global_path, current_robot_pose(), local_costmap_size)
    goal.planner = "global_planner_local_1"

    # Call move_base_flex/get_path
    get_path_ac.send_goal_and_wait(goal)
    return get_path_ac.get_result()

def get_global_path(target_pose: PoseStamped) -> Path():
    """Calls a planner running on the global costmap."""

    goal = GetPathGoal()
    goal.tagret_pose = target_pose
    goal.planner = "global_planner_global_1"

    # ... Call the move_base_flex/get_path and return a result. If there is no
    # plan, we return an empty path.
    return Path()

def navigate(goal: PoseStamped) -> None:
    global_path = Path()
    local_path = Path()

    # Checks if the robot has arrived.
    while not is_arrived(goal):
        try:
            # Throws is the global_path is empty.
            local_path = get_local_path(global_path)
        except RuntimeError:
            # Regenerate the global_path
            global_path = get_global_path(goal)
            continue

        # Call the controller with the current local_path
        exe_goal = ExePathGoal(path=local_path)
        exe_path_ac.send_goal(exe_goal, ...)

        # Sleep before replanning.
        rospy.sleep(0.5)

magazino / move_base_flex

make the costmaps used for planning and controlling configurable #278