enable configurable plugin controllers from BT

nav2_behavior_tree/README.md

@@ -15,7 +15,7 @@ The [bt_action_node template](include/nav2_behavior_tree/bt_action_node.hpp) all
 #include "nav2_msgs/action/follow_path.hpp"
 #include "nav2_behavior_tree/bt_action_node.hpp"
 
-class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
+class ComputeControlAction : public BtActionNode<nav2_msgs::action::ComputeControl>
 {
     ...
 };
@@ -28,20 +28,20 @@ BehaviorTreeEngine::BehaviorTreeEngine()
 {
     ...
 
-  factory_.registerNodeType<nav2_behavior_tree::FollowPathAction>("FollowPath");
+  factory_.registerNodeType<nav2_behavior_tree::ComputeControlAction>("ComputeControl");
 
     ...
 }
 ```
 
-Once a new node is registered with the factory, it is now available to the BehaviorTreeEngine and can be used in Behavior Trees. For example, the following simple XML description of a BT shows the FollowPath node in use:
+Once a new node is registered with the factory, it is now available to the BehaviorTreeEngine and can be used in Behavior Trees. For example, the following simple XML description of a BT shows the ComputeControl node in use:
 
 ```XML
 <root main_tree_to_execute="MainTree">
   <BehaviorTree ID="MainTree">
     <Sequence name="root">
       <ComputePathToPose goal="${goal}"/>
-      <FollowPath path="${path}"/>
+      <ComputeControl path="${path}" controller_name="FollowPath"/>
     </Sequence>
   </BehaviorTree>
 </root>
@@ -67,7 +67,7 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a
 |----------|:-------------|------|
 | Backup |  Action | Invokes the BackUp ROS2 action server, which causes the robot to back up to a specific pose. This is used in nav2 Behavior Trees as a recovery behavior. The nav2_recoveries module implements the BackUp action server. | 
 | ComputePathToPose |    Action   | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. |
-| FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the nav2_dwb_controller module. |
+| ComputeControl | Action |Invokes the ComputeControl ROS2 action server, which is implemented by the controller plugin modules loaded. |
 | GoalReached | Condition | Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, otherwise it returns FAILURE. |
 | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. |
 | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. |

nav2_behavior_tree/doc/hierarchy.svg

@@ -131,12 +131,12 @@
        <path fill="none" stroke="rgb(128,128,128)" d="M 8264,9989 C 8084,9989 7904,10169 7904,10349 L 7904,11789 C 7904,11969 8084,12149 8264,12149 L 12117,12149 C 12297,12149 12477,11969 12477,11789 L 12477,10349 C 12477,10169 12297,9989 12117,9989 L 8264,9989 Z"/>
        <path fill="none" stroke="rgb(128,128,128)" d="M 7904,9989 L 7904,9989 Z"/>
        <path fill="none" stroke="rgb(128,128,128)" d="M 12477,12149 L 12477,12149 Z"/>
-       <text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="8865" y="11238"><tspan fill="rgb(128,128,128)" stroke="none">FollowPath</tspan></tspan></tspan></text>
+       <text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="8865" y="11238"><tspan fill="rgb(128,128,128)" stroke="none">ComputeControl</tspan></tspan></tspan></text>
        <path fill="rgb(0,102,204)" stroke="none" d="M 8064,9789 C 7884,9789 7704,9969 7704,10149 L 7704,11589 C 7704,11769 7884,11949 8064,11949 L 11917,11949 C 12097,11949 12277,11769 12277,11589 L 12277,10149 C 12277,9969 12097,9789 11917,9789 L 8064,9789 Z M 7704,9789 L 7704,9789 Z M 12277,11949 L 12277,11949 Z"/>
        <path fill="none" stroke="rgb(52,101,164)" d="M 8064,9789 C 7884,9789 7704,9969 7704,10149 L 7704,11589 C 7704,11769 7884,11949 8064,11949 L 11917,11949 C 12097,11949 12277,11769 12277,11589 L 12277,10149 C 12277,9969 12097,9789 11917,9789 L 8064,9789 Z"/>
        <path fill="none" stroke="rgb(52,101,164)" d="M 7704,9789 L 7704,9789 Z"/>
        <path fill="none" stroke="rgb(52,101,164)" d="M 12277,11949 L 12277,11949 Z"/>
-       <text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="8665" y="11038"><tspan fill="rgb(255,255,255)" stroke="none">FollowPath</tspan></tspan></tspan></text>
+       <text class="TextShape"><tspan class="TextParagraph" font-family="Verdana, sans-serif" font-size="423px" font-weight="700"><tspan class="TextPosition" x="8665" y="11038"><tspan fill="rgb(255,255,255)" stroke="none">ComputeControl</tspan></tspan></tspan></text>
       </g>
      </g>
      <g class="com.sun.star.drawing.LineShape">

nav2_behavior_tree/include/nav2_behavior_tree/follow_path_action.hpp → nav2_behavior_tree/include/nav2_behavior_tree/compute_control_action.hpp

@@ -12,25 +12,25 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_
-#define NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_
+#ifndef NAV2_BEHAVIOR_TREE__COMPUTE_CONTROL_ACTION_HPP_
+#define NAV2_BEHAVIOR_TREE__COMPUTE_CONTROL_ACTION_HPP_
 
 #include <memory>
 #include <string>
 
-#include "nav2_msgs/action/follow_path.hpp"
+#include "nav2_msgs/action/compute_control.hpp"
 #include "nav2_behavior_tree/bt_action_node.hpp"
 
 namespace nav2_behavior_tree
 {
 
-class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
+class ComputeControlAction : public BtActionNode<nav2_msgs::action::ComputeControl>
 {
 public:
-  FollowPathAction(
+  ComputeControlAction(
     const std::string & action_name,
     const BT::NodeConfiguration & conf)
-  : BtActionNode<nav2_msgs::action::FollowPath>(action_name, conf)
+  : BtActionNode<nav2_msgs::action::ComputeControl>(action_name, conf)
   {
     config().blackboard->set("path_updated", false);
   }
@@ -50,6 +50,7 @@ public:
       // Grab the new goal and set the flag so that we send the new goal to
       // the action server on the next loop iteration
       getInput("path", goal_.path);
+      getInput("controller_name", goal_.controller_name);
       goal_updated_ = true;
     }
   }
@@ -58,10 +59,11 @@ public:
   {
     return {
       BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
+      BT::InputPort<std::string>("controller_name", "FollowPath"),
     };
   }
 };
 
 }  // namespace nav2_behavior_tree
 
-#endif  // NAV2_BEHAVIOR_TREE__FOLLOW_PATH_ACTION_HPP_
+#endif  // NAV2_BEHAVIOR_TREE__COMPUTE_CONTROL_ACTION_HPP_

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -20,7 +20,7 @@
 #include "nav2_behavior_tree/back_up_action.hpp"
 #include "nav2_behavior_tree/bt_conversions.hpp"
 #include "nav2_behavior_tree/compute_path_to_pose_action.hpp"
-#include "nav2_behavior_tree/follow_path_action.hpp"
+#include "nav2_behavior_tree/compute_control_action.hpp"
 #include "nav2_behavior_tree/goal_reached_condition.hpp"
 #include "nav2_behavior_tree/is_stuck_condition.hpp"
 #include "nav2_behavior_tree/rate_controller_node.hpp"
@@ -40,7 +40,7 @@ BehaviorTreeEngine::BehaviorTreeEngine()
 {
   // Register our custom action nodes so that they can be included in XML description
   factory_.registerNodeType<nav2_behavior_tree::ComputePathToPoseAction>("ComputePathToPose");
-  factory_.registerNodeType<nav2_behavior_tree::FollowPathAction>("FollowPath");
+  factory_.registerNodeType<nav2_behavior_tree::ComputeControlAction>("ComputeControl");
   factory_.registerNodeType<nav2_behavior_tree::BackUpAction>("BackUp");
   factory_.registerNodeType<nav2_behavior_tree::SpinAction>("Spin");
   factory_.registerNodeType<nav2_behavior_tree::WaitAction>("Wait");

nav2_bt_navigator/README.md

@@ -40,7 +40,7 @@ The BT Navigator package has two sample XML-based descriptions of BTs.  These tr
           <ComputePathToPose goal="${goal}"/>
         </Fallback>
       </RateController>
-      <FollowPath path="${path}"/>
+      <ComputeControl path="${path}" controller_name="FollowPath"/>
     </Sequence>
   </BehaviorTree>
 </root>
@@ -63,7 +63,7 @@ The graphical version of this Behavior Tree:
 <img src="./doc/simple_parallel.png" title="" width="65%" align="middle">
 <br/>
 
-The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `FollowPath` picks it up. Each time a new path is computed, the blackboard gets updated and then `FollowPath` picks up the new goal.
+The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `ComputeControl` picks it up. Each time a new path is computed, the blackboard gets updated and then `ComputeControl` picks up the new goal to compute a control effort for. `controller_name` will specify the type of control effort you'd like to compute such as `FollowPath` `FollowHuman` `Dock`, etc.
 
 ### Recovery Node
 In this section, the recovery node is being introduced to the navigation package.

nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml

@@ -11,7 +11,7 @@
           <ComputePathToPose goal="{goal}"/>
         </Fallback>
       </RateController>
-      <FollowPath path="{path}"/>
+      <ComputeControl path="{path}" controller_name="FollowPath"/>
     </Sequence>
   </BehaviorTree>
 </root>

nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

@@ -16,7 +16,7 @@
           </Fallback>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}"/>
+          <ComputeControl path="{path}" controller_name="FollowPath"/>
           <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
         </RecoveryNode>
       </Sequence>