Planner selector concept: Design and implementation questions

[pabloinigoblasco]

I show here some questions I have about implementing a convenient PlannerSelector that could be potentially integrated into the navigation stack.

Goal: the planer selector/goal selector is a "component" that listen to the ROS network (via an action server or a topic) and is used in the Behavior Tree to store the desired planner or controller. External task-level ros nodes may decide what is the best planner to use and use this mechanism.

Note: This same design principles can be applied to the concept of GoalSelector if the FollowPath action request message is extended with some new goal_id additional field.

In the following code it is shown an example about how the planner selector could be integrated in a navigation-bt.

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
          <PipelineSequence name="NavigateWithReplanning">
          <PlannerSelector default_controller_id="FollowPath" default_planner_id="GridBased"/> 
             <RateController hz="0.1">
              <ComputePathToPose goal="{goal}" path="{path}"/>
              </RateController>
            <FollowPath path="{path}"/>
        </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

Description

• The PlannerSelector listen to the ROS network to get the "selected planner and controller".
• The PlannerSelector is "updated" at the beginning of the "navigation cycle" and writes into the blackboard the selected planner and controller (the implementation could use input and output ports but using the blackboard is a more integrated approach).
• Then the ComputePathToPose and the FollowPath BTNodes reads from the blackboard to fill with that information the action request messages (foor the planner_server and the controller_server)

Previous experiments
I tried to implement the concept explained above. The ROS network communication mechanism used was "node parameters". The PlannerSelector had a PlannerSelector::onParametersUpdated callback and checked if the "PlannerSelector.planner_id" and/or "PlannerSelector.controler_id" where updated.

However I got some weird behavior. It looked like many of the parameter updates "were lost".
Why it did not work? This was my interpretation about what happened (Please correct me if you think I am wrong): The lifetime of the PlannerSelector is short. Because of that the PlannerSelector::onParametersUpdated was not called. (Can someone tell me if this hypothesis of the short-lifetime is correct?)

Successful workaround
Since that first approach failed I used a Decorator instead of a regular BTNode.

Already exist a PlannerSelector experiment in the following fork (PlannerSelector.hpp and PlannerSelector.cpp)

The resulting BT is like the following shown below:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PlannerSelector default_controller_id="FollowPath" default_planner_id="GridBased">
          <PipelineSequence name="NavigateWithReplanning">
              <RateController hz="0.1">
              <ComputePathToPose goal="{goal}" path="{path}"/>
              </RateController>
            <FollowPath path="{path}"/>
        </PipelineSequence>
      </PlannerSelector>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

Results and usage

This solution (fork https://github.com/pabloinigoblasco/navigation2): works properly using parameters updates from the terminal like it is shown below:

ros2 parameter set /bt_navigator PlannerSelector.controller_id "MyController"

or

ros2 parameter set /bt_navigator PlannerSelector.planner_id "MyPlanner"

Open issues
There is some criticism related with the usage of parameters. for me it is okay also to use any another mechanism (topics, actions or services). This approach was originally selected mimicking the concept of dynamic_reconfigure server of the move_base node (ROS 1)

** Other related issues and PRs **
PR

[SteveMacenski]

https://github.com/ros-planning/navigation2/blob/main/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp the goal checker is very similar to what we're talking about here (topic in -> change some output port -> input port to another), this was tested and utilized in the follow_path.xml dynamic object following demos @fmrico conducted!

https://github.com/ros-planning/navigation2/blob/main/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp#L34 the algorithm Bt nodes use ports already for selecting IDs so using the blackboard variable ports seems like a very simple solution to the issue of conveying information between the BT nodes (that was conveyed from a topic/service via ROS2)

I think we've covered some of the other important topics here on the slack thread - any additional things we should cover as part of this?

[pabloinigoblasco]

That is the correct reference example. Thanks.

• So I will introduce the following changes to the initial experiment:
  • using i/o ports instead hardcoded-propagation via blackboard
  • replacing the usage of properties with (maybe services or actions instead of topics - see below)
  • It is needed to update the FollowPath action request with a new additional field goalchecker_id

Additional issues

• Is a topic subscription a good solution for the planner_selector and the goalchecker_selector?

It may be good to respond some feedback to the requester. For example in the case the planner, controller or goalchecker is unknown or it is not registered.

• Is it convenient conceptually separating planner_selector and controller_selector? Right now planner_selector aggregates the concept of (planner_selector and controller_selector)

• An minor priority question. It is just curiosity. It looks like this is a design pattern (planner_selector, goalchecker_selector, maybe controller_selector). Shouldn't we use a template base class to let the user implement their own inputs into the nav-tree?

• Possible details of the base class:

  • c++ template parameters: TOutput (ie: std_msgs::msg::String, geometry_msgs::msg::PoseStamped), optional TInput
  • output port name string
  • optional input port name string

Maybe that is overengineering, but may be interesting to know what you think.

Regards.

[SteveMacenski]

Is a topic subscription a good solution for the planner_selector and the goalchecker_selector?

Topic seems reasonable

Is it convenient conceptually separating planner_selector and controller_selector? Right now planner_selector aggregates the concept of (planner_selector and controller_selector)

I think so, since if we have other BT nodes like coverage planning servers, or route planning servers, that 1 node would really start to blow up. Separating them makes them more easily testable and scale as the project gets more complex.

[pabloinigoblasco]

BTNode design
I am implementing around the planner_selector, controller_selector and goal_selector.
I am totally miming the goal_updater. That means that we will inherit:

• usage of decorator node
• configurable input topic name

Anything else to say about this?

Naming decisions
BTW, I guess it is also a good moment to also check if the naming is good. Now they are being implemented as "PlannerSelector", "ControllerSelector" and "GoalSelector".

I keep this previous plan, but:
Everything okay with that? Maybe PlannerSwitcher? PlannerConfiguration?

Also the name of the output ports. Right now:

• selected_planner
• selected_controller
• selected_goal_checker

Finally about names of the topic names

• /planner_selector
• /controller_selector
• /goal_checker_selector

what about them?

Default values feature
One last thing. For the initial case when not any topic message was received. We have the option of setting a default planner/controller/goal_checker name as a "static-value input port". Should that functionality be there?
if not the alternatives would be:

• access to the parameter server to get the default values for planner/controller/goal_checker
• or providing empty outputs

[SteveMacenski]

I'm happy with all of those, "Selector" name, topic names, port names. Yes, I think a default_planner_id just as the compute path to pose and follow path have would be good. That way it returns those values when none is set and the planner_id={selected_planner_id} still would enable some default behavior. From that actually, we could add that to the default BTs if you liked when these selector nodes are done (since there's still a default, just set in another location, but also then allows selections)

[pabloinigoblasco]

I created an initial PR for the planner_selector before applying PRs for others (controller_selector and goal_checker_selector)

ros-navigation/navigation2#2249

[SteveMacenski]

Closing, see Nav2 ticket / PR set

[Lannister-Xiaolin]

Any one can share a xml configure that works for galactic version? I setted as below, it's not working!!!

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <RecoveryNode number_of_retries="6" name="NavigateRecovery">
            <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased"
                             topic_name="planner_selector">
                <PipelineSequence name="NavigateWithReplanning">
                    <RateController hz="1.0">
                        <RecoveryNode number_of_retries="1" name="ComputePathToPose">
                            <ComputePathToPose goal="{goal}" path="{path}"/>
                            <ReactiveFallback name="ComputePathToPoseRecoveryFallback">
                                <GoalUpdated/>
                                <ClearEntireCostmap name="ClearGlobalCostmap-Context"
                                                    service_name="global_costmap/clear_entirely_global_costmap"/>
                            </ReactiveFallback>
                        </RecoveryNode>
                    </RateController>
                    <RecoveryNode number_of_retries="1" name="FollowPath">
                        <FollowPath path="{path}" controller_id="FollowPath"/>
                        <ReactiveFallback name="FollowPathRecoveryFallback">
                            <GoalUpdated/>
                            <ClearEntireCostmap name="ClearLocalCostmap-Context"
                                                service_name="local_costmap/clear_entirely_local_costmap"/>
                        </ReactiveFallback>
                    </RecoveryNode>
                </PipelineSequence>
            </PlannerSelector>
            <ReactiveFallback name="RecoveryFallback">
                <GoalUpdated/>
                <RoundRobin name="RecoveryActions">
                    <Sequence name="ClearingActions">
                        <ClearEntireCostmap name="ClearLocalCostmap-Subtree"
                                            service_name="local_costmap/clear_entirely_local_costmap"/>
                        <ClearEntireCostmap name="ClearGlobalCostmap-Subtree"
                                            service_name="global_costmap/clear_entirely_global_costmap"/>
                    </Sequence>
                    <Spin spin_dist="1.57"/>
                    <Wait wait_duration="5"/>
                    <BackUp backup_dist="0.15" backup_speed="0.025"/>
                </RoundRobin>
            </ReactiveFallback>
        </RecoveryNode>
    </BehaviorTree>
</root>

Log of bt_navigator show immediately succucess!!!