Skip to content

Commit

Permalink
Make BT nodes have configurable wait times (Backport #3960 #4178 #4203)…
Browse files Browse the repository at this point in the history
… (#4409)

* WIP: Make BT nodes have configurable wait times. (#3960)

* Make BT nodes have configurable wait times.

Previous solution provided hardcoded 1s value.
Right now the value can be configured for BT
Action, Cancel and Service nodes.

[#3920]

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* Make BT nodes have configurable wait times.

Previous solution provided hardcoded 1s value.
Right now the value can be configured for BT
Action, Cancel and Service nodes.

[#3920]

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* Fix typos, linting errors and value type from float to int

* Fix extra underscores

* Fix extra underscore

* Update unit tests with blackboard parameter

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* Fix formatting errors

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

* Update system tests to match new parameter

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>

---------

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>
Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* chore(nav2_behavior_tree): log actual wait period in bt_action_node (#4178)

Signed-off-by: Felix <felix@fzeltner.de>
Co-authored-by: Felix <felix@fzeltner.de>
Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* fix missing param declare (#4203)

Signed-off-by: nelson <kaichie.lee@gmail.com>
Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

* Fix error messages (#4411)

Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

---------

Signed-off-by: Adam Galecki <embeddedadam@gmail.com>
Signed-off-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Signed-off-by: Felix <felix@fzeltner.de>
Signed-off-by: nelson <kaichie.lee@gmail.com>
Co-authored-by: Adam Gałecki <73710544+embeddedadam@users.noreply.github.com>
Co-authored-by: bi0ha2ard <bi0ha2ard@users.noreply.github.com>
Co-authored-by: Felix <felix@fzeltner.de>
Co-authored-by: nelson <kaichie.lee@gmail.com>
  • Loading branch information
5 people authored Jun 11, 2024
1 parent cf3dd55 commit 8033471
Show file tree
Hide file tree
Showing 30 changed files with 109 additions and 9 deletions.
12 changes: 9 additions & 3 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,8 @@ class BtActionNode : public BT::ActionNodeBase
server_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
wait_for_service_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");

// Initialize the input and output messages
goal_ = typename ActionT::Goal();
Expand Down Expand Up @@ -93,10 +95,11 @@ class BtActionNode : public BT::ActionNodeBase

// Make sure the server is actually there before continuing
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
if (!action_client_->wait_for_action_server(1s)) {
if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
RCLCPP_ERROR(
node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
action_name.c_str());
node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
action_name.c_str(),
wait_for_service_timeout_.count() / 1000.0);
throw std::runtime_error(
std::string("Action server ") + action_name +
std::string(" not available"));
Expand Down Expand Up @@ -459,6 +462,9 @@ class BtActionNode : public BT::ActionNodeBase
// The timeout value for BT loop execution
std::chrono::milliseconds bt_loop_duration_;

// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;

// To track the action server acknowledgement when a new goal is sent
std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
future_goal_handle_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -232,6 +232,9 @@ class BtActionServer
// Default timeout value while waiting for response from a server
std::chrono::milliseconds default_server_timeout_;

// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;

// User-provided callbacks
OnGoalReceivedCallback on_goal_received_callback_;
OnLoopCallback on_loop_callback_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,9 @@ BtActionServer<ActionT>::BtActionServer(
if (!node->has_parameter("default_server_timeout")) {
node->declare_parameter("default_server_timeout", 20);
}
if (!node->has_parameter("wait_for_service_timeout")) {
node->declare_parameter("wait_for_service_timeout", 1000);
}
}

template<class ActionT>
Expand Down Expand Up @@ -112,6 +115,9 @@ bool BtActionServer<ActionT>::on_configure()
bt_loop_duration_ = std::chrono::milliseconds(timeout);
node->get_parameter("default_server_timeout", timeout);
default_server_timeout_ = std::chrono::milliseconds(timeout);
int wait_for_service_timeout;
node->get_parameter("wait_for_service_timeout", wait_for_service_timeout);
wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout);

// Create the class that registers our custom nodes and executes the BT
bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);
Expand All @@ -123,6 +129,9 @@ bool BtActionServer<ActionT>::on_configure()
blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_); // NOLINT
blackboard_->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_); // NOLINT
blackboard_->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_); // NOLINT
blackboard_->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
wait_for_service_timeout_);

return true;
}
Expand Down Expand Up @@ -190,6 +199,9 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
blackboard->set<rclcpp::Node::SharedPtr>("node", client_node_);
blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
wait_for_service_timeout_);
}
} catch (const std::exception & e) {
RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
server_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
wait_for_service_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");

std::string remapped_action_name;
if (getInput("server_name", remapped_action_name)) {
Expand Down Expand Up @@ -89,10 +91,10 @@ class BtCancelActionNode : public BT::ActionNodeBase

// Make sure the server is actually there before continuing
RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
if (!action_client_->wait_for_action_server(1s)) {
if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) {
RCLCPP_ERROR(
node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
action_name.c_str());
node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs",
action_name.c_str(), wait_for_service_timeout_.count() / 1000.0);
throw std::runtime_error(
std::string("Action server ") + action_name +
std::string(" not available"));
Expand Down Expand Up @@ -168,6 +170,8 @@ class BtCancelActionNode : public BT::ActionNodeBase
// The timeout value while waiting for response from a server when a
// new action goal is canceled
std::chrono::milliseconds server_timeout_;
// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;
};

} // namespace nav2_behavior_tree
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@ class BtServiceNode : public BT::ActionNodeBase
server_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
getInput<std::chrono::milliseconds>("server_timeout", server_timeout_);
wait_for_service_timeout_ =
config().blackboard->template get<std::chrono::milliseconds>("wait_for_service_timeout");

// Now that we have node_ to use, create the service client for this BT service
getInput("service_name", service_name_);
Expand All @@ -77,10 +79,10 @@ class BtServiceNode : public BT::ActionNodeBase
RCLCPP_DEBUG(
node_->get_logger(), "Waiting for \"%s\" service",
service_name_.c_str());
if (!service_client_->wait_for_service(1s)) {
if (!service_client_->wait_for_service(wait_for_service_timeout_)) {
RCLCPP_ERROR(
node_->get_logger(), "\"%s\" service server not available after waiting for 1 s",
service_node_name.c_str());
node_->get_logger(), "\"%s\" service server not available after waiting for %.2fs",
service_name_.c_str(), wait_for_service_timeout_.count() / 1000.0);
throw std::runtime_error(
std::string(
"Service server %s not available",
Expand Down Expand Up @@ -249,6 +251,9 @@ class BtServiceNode : public BT::ActionNodeBase
// The timeout value for BT loop execution
std::chrono::milliseconds bt_loop_duration_;

// The timeout value for waiting for a service to response
std::chrono::milliseconds wait_for_service_timeout_;

// To track the server response when a new request is sent
std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;
bool request_sent_{false};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,9 @@ class AssistedTeleopActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);
config_->blackboard->set<int>("number_recoveries", 0);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,9 @@ class CancelAssistedTeleopActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
client_ = rclcpp_action::create_client<nav2_msgs::action::AssistedTeleop>(
node_, "assisted_teleop");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,9 @@ class BackUpActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);
config_->blackboard->set<int>("number_recoveries", 0);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,9 @@ class CancelBackUpActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
client_ = rclcpp_action::create_client<nav2_msgs::action::BackUp>(
node_, "back_up");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -173,6 +173,7 @@ class BTActionNodeTestFixture : public ::testing::Test
config_->blackboard->set<rclcpp::Node::SharedPtr>("node", node_);
config_->blackboard->set<std::chrono::milliseconds>("server_timeout", 20ms);
config_->blackboard->set<std::chrono::milliseconds>("bt_loop_duration", 10ms);
config_->blackboard->set<std::chrono::milliseconds>("wait_for_service_timeout", 1000ms);
config_->blackboard->set<bool>("initial_pose_received", false);
config_->blackboard->set<bool>("on_cancelled_triggered", false);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,9 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);
config_->blackboard->set<int>("number_recoveries", 0);

Expand Down Expand Up @@ -139,6 +142,9 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);
config_->blackboard->set<int>("number_recoveries", 0);

Expand Down Expand Up @@ -231,6 +237,9 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);
config_->blackboard->set<int>("number_recoveries", 0);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,9 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);

BT::NodeBuilder builder =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,9 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);

BT::NodeBuilder builder =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,9 @@ class CancelControllerActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
client_ = rclcpp_action::create_client<nav2_msgs::action::FollowPath>(
node_, "follow_path");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,9 @@ class DriveOnHeadingActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);

BT::NodeBuilder builder =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,9 @@ class CancelDriveOnHeadingTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
client_ = rclcpp_action::create_client<nav2_msgs::action::DriveOnHeading>(
node_, "drive_on_heading_cancel");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,9 @@ class FollowPathActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);

BT::NodeBuilder builder =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,9 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);
std::vector<geometry_msgs::msg::PoseStamped> poses;
config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,9 @@ class NavigateToPoseActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);

BT::NodeBuilder builder =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,9 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);

factory_->registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,9 @@ class SmoothPathActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);

BT::NodeBuilder builder =
Expand Down
3 changes: 3 additions & 0 deletions nav2_behavior_tree/test/plugins/action/test_spin_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,9 @@ class SpinActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);
config_->blackboard->set<int>("number_recoveries", 0);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,9 @@ class CancelSpinActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
client_ = rclcpp_action::create_client<nav2_msgs::action::Spin>(
node_, "spin");

Expand Down
3 changes: 3 additions & 0 deletions nav2_behavior_tree/test/plugins/action/test_wait_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,9 @@ class WaitActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
config_->blackboard->set<bool>("initial_pose_received", false);
config_->blackboard->set<int>("number_recoveries", 0);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,9 @@ class CancelWaitActionTestFixture : public ::testing::Test
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds(10));
config_->blackboard->set<std::chrono::milliseconds>(
"wait_for_service_timeout",
std::chrono::milliseconds(1000));
client_ = rclcpp_action::create_client<nav2_msgs::action::Wait>(
node_, "wait");

Expand Down
1 change: 1 addition & 0 deletions nav2_bringup/params/nav2_multirobot_params_1.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ bt_navigator:
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
wait_for_service_timeout: 1000
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
Expand Down
1 change: 1 addition & 0 deletions nav2_bringup/params/nav2_multirobot_params_2.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ bt_navigator:
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
wait_for_service_timeout: 1000
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
Expand Down
1 change: 1 addition & 0 deletions nav2_bringup/params/nav2_multirobot_params_all.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ bt_navigator:
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
wait_for_service_timeout: 1000
navigators: ["navigate_to_pose", "navigate_through_poses"]
navigate_to_pose:
plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
Expand Down
1 change: 1 addition & 0 deletions nav2_bringup/params/nav2_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ bt_navigator:
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
wait_for_service_timeout: 1000
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
Expand Down
Loading

0 comments on commit 8033471

Please sign in to comment.