diff --git a/nav2_bringup/bringup/launch/bringup_launch.py b/nav2_bringup/bringup/launch/bringup_launch.py index 9904b5bc7f1..61a048d0da9 100644 --- a/nav2_bringup/bringup/launch/bringup_launch.py +++ b/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()), ]) diff --git a/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py b/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py index d544528ab17..4612223f189 100644 --- a/nav2_bringup/bringup/launch/multi_tb3_simulation_launch.py +++ b/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]), diff --git a/nav2_bringup/bringup/launch/navigation_launch.py b/nav2_bringup/bringup/launch/navigation_launch.py index ba43546a674..b837ac4d4eb 100644 --- a/nav2_bringup/bringup/launch/navigation_launch.py +++ b/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'), diff --git a/nav2_bringup/bringup/launch/tb3_simulation_launch.py b/nav2_bringup/bringup/launch/tb3_simulation_launch.py index f3fe02b7e73..ce843a8adbe 100644 --- a/nav2_bringup/bringup/launch/tb3_simulation_launch.py +++ b/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 diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index cb17f396a7f..6ce8e32634b 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/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 diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 726ce779e5d..efa093a6fb7 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/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 diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index c2f9c0a2397..cb123400a50 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/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 diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 0c2f7c18909..76e63c7d5da 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/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::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 bt_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 2cc2d768cb5..d69227017fb 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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("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(xml_file), std::istreambuf_iterator()); 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,6 +274,19 @@ BtNavigator::navigateToPose() action_server_->publish_feedback(feedback_msg); }; + 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(); + return; + } + // 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 diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index d047dad8661..792bee20ad1 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -1,5 +1,6 @@ #goal definition geometry_msgs/PoseStamped pose +string behavior_tree --- #result definition std_msgs/Empty result