diff --git a/nav2_bringup/launch/bt_navigator.xml b/nav2_bringup/launch/bt_navigator.xml
index 2f5fe1cd6af4dd50b0ac2b469a9ba29065b9a824..16eb0564d1d511432968e1caf4c4c24d38528ed7 100644
--- a/nav2_bringup/launch/bt_navigator.xml
+++ b/nav2_bringup/launch/bt_navigator.xml
@@ -19,11 +19,12 @@
<BehaviorTree ID="MainTree">
<Sequence name="root">
<RateController hz="1.0">
- <ComputePathToPose goal="${goal}"/>
+ <Fallback>
+ <GoalReached/>
+ <ComputePathToPose goal="${goal}"/>
+ </Fallback>
</RateController>
- <SequenceStar name="RememberFollowPathResult">
- <FollowPath path="${path}"/>
- </SequenceStar>
+ <FollowPath path="${path}"/>
</Sequence>
</BehaviorTree>
</root>
diff --git a/nav2_bt_navigator/behavior_trees/auto_localization.xml b/nav2_bt_navigator/behavior_trees/auto_localization.xml
deleted file mode 100644
index 485c383f6eb73bd4a34dd5a41f652e9fe00cca99..0000000000000000000000000000000000000000
--- a/nav2_bt_navigator/behavior_trees/auto_localization.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-<!--AutoLocalization-->
-<root main_tree_to_execute="MainTree">
- <BehaviorTree ID="MainTree">
- <FallbackStar name="root_AutoLocalization">
- <initialPoseReceived/>
- <SequenceStar name="doSelfLocalization">
- <RetryUntilSuccesful num_attempts="5" name="retry_client_request">
- <globalLocalizationServiceRequest/>
- </RetryUntilSuccesful>
- <RetryUntilSuccesful num_attempts="10" name="retry_localization">
- <Sequence>
- <Fallback>
- <IsLocalized/>
- <SequenceStar>
- <Spin/>
- <BackUp/>
- <Spin/>
- </SequenceStar>
- </Fallback>
- <IsLocalized/>
- </Sequence>
- </RetryUntilSuccesful>
- </SequenceStar>
- </FallbackStar>
- </BehaviorTree>
-</root>
\ No newline at end of file
diff --git a/nav2_bt_navigator/behavior_trees/auto_localization_w_parallel_recovery.xml b/nav2_bt_navigator/behavior_trees/auto_localization_w_parallel_recovery.xml
deleted file mode 100644
index cb6e08948ad1f83d8ff4221f0371cf3a75aa7016..0000000000000000000000000000000000000000
--- a/nav2_bt_navigator/behavior_trees/auto_localization_w_parallel_recovery.xml
+++ /dev/null
@@ -1,58 +0,0 @@
-<!--
- This Behavior Tree first goes into a branch where it will check to see if the robot's initial
- pose is given. If the initial pose is not given it trys to automatically localize the robot.
- After finding the initial pose it will continue with the recovery and navigation branch. The
- AutoLocalization branch is based on 'AutoLocalization.xml' and the navigation branch is based on
- `parallel_w_recovery.xml`.
--->
-<root main_tree_to_execute="MainTree">
- <BehaviorTree ID="MainTree">
- <Sequence name="root">
- <FallbackStar name="AutoLocalization">
- <initialPoseReceived/>
- <SequenceStar name="doSelfLocalization">
- <RetryUntilSuccesful num_attempts="5" name="retry_client_request">
- <globalLocalizationServiceRequest/>
- </RetryUntilSuccesful>
- <RetryUntilSuccesful num_attempts="10" name="retry_localization">
- <Sequence>
- <Fallback>
- <IsLocalized/>
- <SequenceStar>
- <Spin/>
- <BackUp/>
- <Spin/>
- </SequenceStar>
- </Fallback>
- <IsLocalized/>
- </Sequence>
- </RetryUntilSuccesful>
- </SequenceStar>
- </FallbackStar>
- <FallbackStar name="check_motion">
- <Inverter name="is_not_stuck">
- <IsStuck/>
- </Inverter>
- <SequenceStar name="stuck_recovery">
- <Stop/>
- <BackUp/>
- <Spin/>
- </SequenceStar>
- </FallbackStar>
- <SequenceStar name="navigate">
- <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
- <ParallelNode threshold="1">
- <FollowPath path="${path}"/>
- <Sequence>
- <RateController hz="2">
- <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
- </RateController>
- <UpdatePath/>
- </Sequence>
- </ParallelNode>
- </SequenceStar>
- </Sequence>
- </BehaviorTree>
-</root>
-
-
diff --git a/nav2_bt_navigator/behavior_trees/auto_localization_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/auto_localization_w_replanning_and_recovery.xml
new file mode 100644
index 0000000000000000000000000000000000000000..34e21a5476ca2767e7cad4fb0627d34755fc36d9
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/auto_localization_w_replanning_and_recovery.xml
@@ -0,0 +1,54 @@
+<!--
+ This Behavior Tree first checks to see if the robot's initial pose is known. If the initial pose
+ is not known, it trys to automatically localize the robot. After finding the initial pose, the
+ navigate with replanning and retry branch gets executed.
+-->
+<root main_tree_to_execute="MainTree">
+ <BehaviorTree ID="MainTree">
+ <SequenceStar name="root">
+ <FallbackStar name="AutoLocalization">
+ <initialPoseReceived/>
+ <SequenceStar name="doSelfLocalization">
+ <RetryUntilSuccesful num_attempts="5" name="retry_client_request">
+ <globalLocalizationServiceRequest/>
+ </RetryUntilSuccesful>
+ <RetryUntilSuccesful num_attempts="10" name="retry_localization">
+ <Sequence>
+ <Fallback>
+ <IsLocalized/>
+ <SequenceStar>
+ <Spin/>
+ <BackUp/>
+ <Spin/>
+ </SequenceStar>
+ </Fallback>
+ <IsLocalized/>
+ </Sequence>
+ </RetryUntilSuccesful>
+ </SequenceStar>
+ </FallbackStar>
+ <RetryUntilSuccesful name="navigate_w_replanning_and_retry" num_attempts="6">
+ <Fallback>
+ <Sequence>
+ <RateController hz="1.0">
+ <Fallback>
+ <GoalReached/>
+ <ComputePathToPose goal="${goal}" path="${path}"/>
+ </Fallback>
+ </RateController>
+ <FollowPath path="${path}"/>
+ </Sequence>
+ <ForceFailure>
+ <SequenceStar name="recovery">
+ <clearEntirelyCostmapServiceRequest service_name="/local_costmap/clear_entirely_local_costmap"/>
+ <clearEntirelyCostmapServiceRequest service_name="/global_costmap/clear_entirely_global_costmap"/>
+ <Spin/>
+ </SequenceStar>
+ </ForceFailure>
+ </Fallback>
+ </RetryUntilSuccesful>
+ </SequenceStar>
+ </BehaviorTree>
+</root>
+
+
diff --git a/nav2_bt_navigator/behavior_trees/bt_navigator_params.yaml b/nav2_bt_navigator/behavior_trees/bt_navigator_params.yaml
deleted file mode 100644
index de5e05469a2ccccbee0f4b65af87594346863705..0000000000000000000000000000000000000000
--- a/nav2_bt_navigator/behavior_trees/bt_navigator_params.yaml
+++ /dev/null
@@ -1,5 +0,0 @@
-# Provide the path to the xml file with the behavior tree description to use
-
-BtNavigator:
- ros__parameters:
- bt_xml_filename: "~/ros2_overlay_ws/src/navigation2/nav2_bt_navigator/behavior_trees/simple_sequential.xml"
\ No newline at end of file
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0a09dfc913a55bf3b7f5ee77e4909b946f708b62
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
@@ -0,0 +1,17 @@
+<!--
+ This Behavior Tree replans the global path periodically at 1 Hz.
+-->
+
+<root main_tree_to_execute="MainTree">
+ <BehaviorTree ID="MainTree">
+ <Sequence name="root">
+ <RateController hz="1.0">
+ <Fallback>
+ <GoalReached/>
+ <ComputePathToPose goal="${goal}"/>
+ </Fallback>
+ </RateController>
+ <FollowPath path="${path}"/>
+ </Sequence>
+ </BehaviorTree>
+</root>
\ No newline at end of file
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_retry.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
similarity index 64%
rename from nav2_bt_navigator/behavior_trees/navigate_w_recovery_retry.xml
rename to nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
index f1e8e14bf3fa6c23fbbc9d66594387c02c9327b7..6a4b0bdda8784d5c715a1156415dab809b47100c 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_retry.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
@@ -6,21 +6,15 @@
<BehaviorTree ID="MainTree">
<RetryUntilSuccesful name="retry_navigate" num_attempts="6">
<Fallback>
- <SequenceStar>
- <ComputePathToPose goal="${goal}" path="${path}"/>
- <Sequence>
+ <Sequence>
+ <RateController hz="1.0">
<Fallback>
<GoalReached/>
- <RateController hz="1.0">
- <Sequence>
- <ComputePathToPose goal="${goal}" path="${path}"/>
- <UpdatePath/>
- </Sequence>
- </RateController>
+ <ComputePathToPose goal="${goal}" path="${path}"/>
</Fallback>
- <FollowPath path="${path}"/>
- </Sequence>
- </SequenceStar>
+ </RateController>
+ <FollowPath path="${path}"/>
+ </Sequence>
<ForceFailure>
<SequenceStar name="recovery">
<clearEntirelyCostmapServiceRequest service_name="/local_costmap/clear_entirely_local_costmap"/>
diff --git a/nav2_bt_navigator/behavior_trees/parallel.xml b/nav2_bt_navigator/behavior_trees/parallel.xml
deleted file mode 100644
index 4bb567a833c6bdbb5d644164a945034cde5ce026..0000000000000000000000000000000000000000
--- a/nav2_bt_navigator/behavior_trees/parallel.xml
+++ /dev/null
@@ -1,23 +0,0 @@
-<!--
- This Behavior Tree replans the global path periodically at 1 Hz.
--->
-
-<root main_tree_to_execute="MainTree">
- <BehaviorTree ID="MainTree">
- <SequenceStar name="root">
- <ComputePathToPose goal="${goal}" path="${path}"/>
- <Sequence>
- <Fallback>
- <GoalReached/>
- <RateController hz="1.0">
- <Sequence>
- <ComputePathToPose goal="${goal}" path="${path}"/>
- <UpdatePath/>
- </Sequence>
- </RateController>
- </Fallback>
- <FollowPath path="${path}"/>
- </Sequence>
- </SequenceStar>
- </BehaviorTree>
-</root>
\ No newline at end of file
diff --git a/nav2_bt_navigator/behavior_trees/parallel_w_recovery.xml b/nav2_bt_navigator/behavior_trees/parallel_w_recovery.xml
deleted file mode 100644
index 76055671ecfc7e41a42f973f72a93711d1644cc6..0000000000000000000000000000000000000000
--- a/nav2_bt_navigator/behavior_trees/parallel_w_recovery.xml
+++ /dev/null
@@ -1,34 +0,0 @@
-<!--
- This Behavior Tree first goes into a branch where it will check if the robot is stuck.
- If that is the case, it will attempt to recover. Otherwise it will continue with
- the navigation branch. In this case the navigation branch is based on `parallel.xml`.
--->
-
-<root main_tree_to_execute="MainTree">
- <BehaviorTree ID="MainTree">
- <Sequence name="root">
- <FallbackStar name="check_motion">
- <Inverter name="is_not_stuck">
- <IsStuck/>
- </Inverter>
- <SequenceStar name="stuck_recovery">
- <Stop/>
- <BackUp/>
- <Spin/>
- </SequenceStar>
- </FallbackStar>
- <SequenceStar name="navigate">
- <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
- <ParallelNode threshold="1">
- <FollowPath path="${path}"/>
- <Sequence>
- <RateController hz="2">
- <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
- </RateController>
- <UpdatePath/>
- </Sequence>
- </ParallelNode>
- </SequenceStar>
- </Sequence>
- </BehaviorTree>
-</root>
diff --git a/nav2_bt_navigator/behavior_trees/simple_sequential.xml b/nav2_bt_navigator/behavior_trees/simple_sequential.xml
deleted file mode 100644
index 4a0844c88f2683a78fc9a88c8ddd1fd34d28ef2e..0000000000000000000000000000000000000000
--- a/nav2_bt_navigator/behavior_trees/simple_sequential.xml
+++ /dev/null
@@ -1,13 +0,0 @@
-<!--
- This Behavior Tree does the minimum - it invokes ComputePathToPose once to compute a path
- using the global planner. It then invokes the local planner (FollowPath) with this path.
--->
-
-<root main_tree_to_execute="MainTree">
- <BehaviorTree ID="MainTree">
- <SequenceStar name="root">
- <ComputePathToPose goal="${goal}"/>
- <FollowPath path="${path}"/>
- </SequenceStar>
- </BehaviorTree>
-</root>
diff --git a/nav2_bt_navigator/behavior_trees/simple_sequential_w_recovery.xml b/nav2_bt_navigator/behavior_trees/simple_sequential_w_recovery.xml
deleted file mode 100644
index eb44e54ec01d87199e2ca91823bca58bedd394e0..0000000000000000000000000000000000000000
--- a/nav2_bt_navigator/behavior_trees/simple_sequential_w_recovery.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-<!--
- This Behavior Tree first goes into a branch where it will check if the robot is stuck.
- If that is the case, it will attempt to recover. Otherwise it will continue with
- the navigation branch. In this case the navigation branch is based on `simple_sequential.xml`.
--->
-
-<root main_tree_to_execute="MainTree">
- <BehaviorTree ID="MainTree">
- <Sequence name="root">
- <FallbackStar name="check_motion">
- <Inverter name="is_not_stuck">
- <IsStuck/>
- </Inverter>
- <SequenceStar name="stuck_recovery">
- <Stop/>
- <BackUp/>
- <Spin/>
- </SequenceStar>
- </FallbackStar>
- <SequenceStar name="navigate">
- <ComputePathToPose endpoints="${endpoints}" path="${path}"/>
- <FollowPath path="${path}"/>
- </SequenceStar>
- </Sequence>
- </BehaviorTree>
-</root>
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index 7f01b74fe633e56491577cffda7c4ac44e31c691..d5a47537950a44cfeb016ca1b2e9c48e7604cbd0 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -72,6 +72,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
// Put items on the blackboard
blackboard_->set<geometry_msgs::msg::PoseStamped::SharedPtr>("goal", goal_); // NOLINT
blackboard_->set<nav2_msgs::msg::Path::SharedPtr>("path", path_); // NOLINT
+ blackboard_->set<rclcpp::Node::SharedPtr>("node", client_node_); // NOLINT
blackboard_->set<std::chrono::milliseconds>("node_loop_timeout", std::chrono::milliseconds(10)); // NOLINT
blackboard_->set<bool>("path_updated", false); // NOLINT
blackboard_->set<bool>("initial_pose_received", false); // NOLINT
diff --git a/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp b/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
index 7049f1af6ffba701bcbdd603e8c0c0e3bfd44a7e..485fe9c2f3433f10476aeabe28950e848a30538c 100644
--- a/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
+++ b/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
@@ -45,29 +45,24 @@ public:
BT::NodeStatus tick() override
{
if (!initialized_) {
- // Get the required items from the blackboard
- node_ = blackboard()->template get<nav2_lifecycle::LifecycleNode::SharedPtr>("node");
-
- node_->get_parameter_or<double>("is_localized_condition.x_tol", x_tol_, 0.25);
- node_->get_parameter_or<double>("is_localized_condition.y_tol", y_tol_, 0.25);
- node_->get_parameter_or<double>("is_localized_condition.rot_tol", rot_tol_, M_PI / 4);
-
- robot_ = std::make_unique<nav2_robot::Robot>(
- node_->get_node_base_interface(),
- node_->get_node_topics_interface(),
- node_->get_node_logging_interface(),
- true);
-
- initialized_ = true;
+ initialize();
}
return isLocalized() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
}
-protected:
- void cleanup()
+ void initialize()
{
- robot_.reset();
+ node_ = blackboard()->template get<rclcpp::Node::SharedPtr>("node");
+ node_->get_parameter_or<double>("is_localized_condition.x_tol", x_tol_, 0.25);
+ node_->get_parameter_or<double>("is_localized_condition.y_tol", y_tol_, 0.25);
+ node_->get_parameter_or<double>("is_localized_condition.rot_tol", rot_tol_, M_PI / 4);
+ robot_ = std::make_unique<nav2_robot::Robot>(
+ node_->get_node_base_interface(),
+ node_->get_node_topics_interface(),
+ node_->get_node_logging_interface(),
+ true);
+ initialized_ = true;
}
bool isLocalized()
@@ -93,11 +88,18 @@ protected:
return false;
}
+protected:
+ void cleanup()
+ {
+ robot_.reset();
+ }
+
+private:
static const int cov_x_ = 0;
static const int cov_y_ = 7;
static const int cov_a_ = 35;
- nav2_lifecycle::LifecycleNode::SharedPtr node_;
+ rclcpp::Node::SharedPtr node_;
std::unique_ptr<nav2_robot::Robot> robot_;
bool initialized_;