Select behavior Tree in NavigateToPoseAction

nav2_bringup/bringup/launch/bringup_launch.py

@@ -37,7 +37,7 @@ def generate_launch_description():
     map_yaml_file = LaunchConfiguration('map')
     use_sim_time = LaunchConfiguration('use_sim_time')
     params_file = LaunchConfiguration('params_file')
-    bt_xml_file = LaunchConfiguration('bt_xml_file')
+    default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
     autostart = LaunchConfiguration('autostart')
 
     stdout_linebuf_envvar = SetEnvironmentVariable(
@@ -73,7 +73,7 @@ def generate_launch_description():
         description='Full path to the ROS2 parameters file to use for all launched nodes')
 
     declare_bt_xml_cmd = DeclareLaunchArgument(
-        'bt_xml_file',
+        'default_bt_xml_filename',
         default_value=os.path.join(
             get_package_share_directory('nav2_bt_navigator'),
             'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
@@ -114,7 +114,7 @@ def generate_launch_description():
                               'use_sim_time': use_sim_time,
                               'autostart': autostart,
                               'params_file': params_file,
-                              'bt_xml_file': bt_xml_file,
+                              'default_bt_xml_filename': default_bt_xml_filename,
                               'use_lifecycle_mgr': 'false',
                               'map_subscribe_transient_local': 'true'}.items()),
     ])

nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py

@@ -49,7 +49,7 @@ def generate_launch_description():
     # On this example all robots are launched with the same settings
     map_yaml_file = LaunchConfiguration('map')
 
-    bt_xml_file = LaunchConfiguration('bt_xml_file')
+    default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
     autostart = LaunchConfiguration('autostart')
     rviz_config_file = LaunchConfiguration('rviz_config')
     use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
@@ -83,7 +83,7 @@ def generate_launch_description():
         description='Full path to the ROS2 parameters file to use for robot2 launched nodes')
 
     declare_bt_xml_cmd = DeclareLaunchArgument(
-        'bt_xml_file',
+        'default_bt_xml_filename',
         default_value=os.path.join(
             get_package_share_directory('nav2_bt_navigator'),
             'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
@@ -152,7 +152,7 @@ def generate_launch_description():
                                   'map': map_yaml_file,
                                   'use_sim_time': 'True',
                                   'params_file': params_file,
-                                  'bt_xml_file': bt_xml_file,
+                                  'default_bt_xml_filename': default_bt_xml_filename,
                                   'autostart': autostart,
                                   'use_rviz': 'False',
                                   'use_simulator': 'False',
@@ -170,7 +170,7 @@ def generate_launch_description():
                 msg=[robot['name'], ' params yaml: ', params_file]),
             LogInfo(
                 condition=IfCondition(log_settings),
-                msg=[robot['name'], ' behavior tree xml: ', bt_xml_file]),
+                msg=[robot['name'], ' behavior tree xml: ', default_bt_xml_filename]),
             LogInfo(
                 condition=IfCondition(log_settings),
                 msg=[robot['name'], ' rviz config file: ', rviz_config_file]),

nav2_bringup/bringup/launch/navigation_launch.py

@@ -31,7 +31,7 @@ def generate_launch_description():
     use_sim_time = LaunchConfiguration('use_sim_time')
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
-    bt_xml_file = LaunchConfiguration('bt_xml_file')
+    default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
     map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
 
     lifecycle_nodes = ['controller_server',
@@ -52,7 +52,7 @@ def generate_launch_description():
     # Create our own temporary YAML files that include substitutions
     param_substitutions = {
         'use_sim_time': use_sim_time,
-        'bt_xml_filename': bt_xml_file,
+        'default_bt_xml_filename': default_bt_xml_filename,
         'autostart': autostart,
         'map_subscribe_transient_local': map_subscribe_transient_local}
 
@@ -84,7 +84,7 @@ def generate_launch_description():
             description='Full path to the ROS2 parameters file to use'),
 
         DeclareLaunchArgument(
-            'bt_xml_file',
+            'default_bt_xml_filename',
             default_value=os.path.join(
                 get_package_share_directory('nav2_bt_navigator'),
                 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),

nav2_bringup/bringup/launch/tb3_simulation_launch.py

@@ -38,7 +38,7 @@ def generate_launch_description():
     map_yaml_file = LaunchConfiguration('map')
     use_sim_time = LaunchConfiguration('use_sim_time')
     params_file = LaunchConfiguration('params_file')
-    bt_xml_file = LaunchConfiguration('bt_xml_file')
+    default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
     autostart = LaunchConfiguration('autostart')
 
     # Launch configuration variables specific to simulation
@@ -90,7 +90,7 @@ def generate_launch_description():
         description='Full path to the ROS2 parameters file to use for all launched nodes')
 
     declare_bt_xml_cmd = DeclareLaunchArgument(
-        'bt_xml_file',
+        'default_bt_xml_filename',
         default_value=os.path.join(
             get_package_share_directory('nav2_bt_navigator'),
             'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
@@ -173,7 +173,7 @@ def generate_launch_description():
                           'map': map_yaml_file,
                           'use_sim_time': use_sim_time,
                           'params_file': params_file,
-                          'bt_xml_file': bt_xml_file,
+                          'default_bt_xml_filename': default_bt_xml_filename,
                           'autostart': autostart}.items())
 
     # Create the launch description and populate

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -51,7 +51,7 @@ bt_navigator:
     use_sim_time: True
     global_frame: map
     robot_base_frame: base_link
-    bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_follow_path_action_bt_node

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -51,7 +51,7 @@ bt_navigator:
     use_sim_time: True
     global_frame: map
     robot_base_frame: base_link
-    bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_follow_path_action_bt_node

nav2_bringup/bringup/params/nav2_params.yaml

@@ -51,7 +51,7 @@ bt_navigator:
     use_sim_time: True
     global_frame: map
     robot_base_frame: base_link
-    bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_follow_path_action_bt_node

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -112,13 +112,22 @@ class BtNavigator : public nav2_util::LifecycleNode
   void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose);
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
 
+  /**
+   * @brief Replace current BT with another one
+   * @param bt_xml_filename The file containing the new BT
+   * @return true if the resulting BT correspond to the one in bt_xml_filename. false
+   * if something went wrong, and previous BT is mantained
+   */
+  bool loadBehaviorTree(const std::string & bt_id);
+
   BT::Tree tree_;
 
   // The blackboard shared by all of the nodes in the tree
   BT::Blackboard::Ptr blackboard_;
 
-  // The XML string that defines the Behavior Tree to create
-  std::string xml_string_;
+  // The XML fiñe that cointains the Behavior Tree to create
+  std::string current_bt_xml_filename_;
+  std::string default_bt_xml_filename_;
 
   // The wrapper class for the BT functionality
   std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;

nav2_bt_navigator/src/bt_navigator.cpp

@@ -59,7 +59,7 @@ BtNavigator::BtNavigator()
   };
 
   // Declare this node's parameters
-  declare_parameter("bt_xml_filename");
+  declare_parameter("default_bt_xml_filename");
   declare_parameter("plugin_lib_names", plugin_libs);
   declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1));
   declare_parameter("global_frame", std::string("map"));
@@ -127,28 +127,44 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   blackboard_->set<int>("number_recoveries", 0);  // NOLINT
 
   // Get the BT filename to use from the node parameter
-  std::string bt_xml_filename;
-  get_parameter("bt_xml_filename", bt_xml_filename);
+  get_parameter("default_bt_xml_filename", default_bt_xml_filename_);
+
+  if (!loadBehaviorTree(default_bt_xml_filename_)) {
+    RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str());
+    return nav2_util::CallbackReturn::FAILURE;
+  }
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+bool
+BtNavigator::loadBehaviorTree(const std::string & bt_xml_filename)
+{
+  // Use previous BT if it is the existing one
+  if (current_bt_xml_filename_ == bt_xml_filename) {
+    return true;
+  }
 
   // Read the input BT XML from the specified file into a string
   std::ifstream xml_file(bt_xml_filename);
 
   if (!xml_file.good()) {
     RCLCPP_ERROR(get_logger(), "Couldn't open input XML file: %s", bt_xml_filename.c_str());
-    return nav2_util::CallbackReturn::FAILURE;
+    return false;
   }
 
-  xml_string_ = std::string(
+  auto xml_string = std::string(
     std::istreambuf_iterator<char>(xml_file),
     std::istreambuf_iterator<char>());
 
   RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str());
-  RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string_.c_str());
+  RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string.c_str());
 
   // Create the Behavior Tree from the XML input
-  tree_ = bt_->buildTreeFromText(xml_string_, blackboard_);
+  tree_ = bt_->buildTreeFromText(xml_string, blackboard_);
+  current_bt_xml_filename_ = bt_xml_filename;
 
-  return nav2_util::CallbackReturn::SUCCESS;
+  return true;
 }
 
 nav2_util::CallbackReturn
@@ -188,7 +204,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 
   action_server_.reset();
   plugin_lib_names_.clear();
-  xml_string_.clear();
   blackboard_.reset();
   bt_->haltAllActions(tree_.rootNode());
   bt_.reset();
@@ -259,30 +274,42 @@ BtNavigator::navigateToPose()
       action_server_->publish_feedback(feedback_msg);
     };
 
-  // Execute the BT that was previously created in the configure step
-  nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling);
-  // Make sure that the Bt is not in a running state from a previous execution
-  // note: if all the ControlNodes are implemented correctly, this is not needed.
-  bt_->haltAllActions(tree_.rootNode());
-
-  switch (rc) {
-    case nav2_behavior_tree::BtStatus::SUCCEEDED:
-      RCLCPP_INFO(get_logger(), "Navigation succeeded");
-      action_server_->succeeded_current();
-      break;
-
-    case nav2_behavior_tree::BtStatus::FAILED:
-      RCLCPP_ERROR(get_logger(), "Navigation failed");
-      action_server_->terminate_current();
-      break;
-
-    case nav2_behavior_tree::BtStatus::CANCELED:
-      RCLCPP_INFO(get_logger(), "Navigation canceled");
-      action_server_->terminate_all();
-      break;
-
-    default:
-      throw std::logic_error("Invalid status return from BT");
+  auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree;
+
+  // Empty id in request is default for backward compatibility
+  bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename;
+
+  if (!loadBehaviorTree(bt_xml_filename)) {
+    RCLCPP_ERROR(
+      get_logger(), "BT file not found: %s. Navigation canceled",
+      bt_xml_filename.c_str(), current_bt_xml_filename_.c_str());
+    action_server_->terminate_current();
+  } else {
+    // Execute the BT that was previously created in the configure step
+    nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling);
+    // Make sure that the Bt is not in a running state from a previous execution
+    // note: if all the ControlNodes are implemented correctly, this is not needed.
+    bt_->haltAllActions(tree_.rootNode());
+
+    switch (rc) {
+      case nav2_behavior_tree::BtStatus::SUCCEEDED:
+        RCLCPP_INFO(get_logger(), "Navigation succeeded");
+        action_server_->succeeded_current();
+        break;
+
+      case nav2_behavior_tree::BtStatus::FAILED:
+        RCLCPP_ERROR(get_logger(), "Navigation failed");
+        action_server_->terminate_current();
+        break;
+
+      case nav2_behavior_tree::BtStatus::CANCELED:
+        RCLCPP_INFO(get_logger(), "Navigation canceled");
+        action_server_->terminate_all();
+        break;
+
+      default:
+        throw std::logic_error("Invalid status return from BT");
+    }
   }
 }
 

nav2_msgs/action/NavigateToPose.action

@@ -1,5 +1,6 @@
 #goal definition
 geometry_msgs/PoseStamped pose
+string behavior_tree
 ---
 #result definition
 std_msgs/Empty result