diff --git a/nav2_bringup/launch/bt_navigator.xml b/nav2_bringup/launch/bt_navigator.xml
index 036824d36910644a6a353ba470d53692cfa4098b..2f5fe1cd6af4dd50b0ac2b469a9ba29065b9a824 100644
--- a/nav2_bringup/launch/bt_navigator.xml
+++ b/nav2_bringup/launch/bt_navigator.xml
@@ -19,10 +19,7 @@
<BehaviorTree ID="MainTree">
<Sequence name="root">
<RateController hz="1.0">
- <Fallback>
- <GoalReached/>
- <ComputePathToPose goal="${goal}"/>
- </Fallback>
+ <ComputePathToPose goal="${goal}"/>
</RateController>
<SequenceStar name="RememberFollowPathResult">
<FollowPath path="${path}"/>
diff --git a/nav2_bt_navigator/behavior_trees/auto_localization.xml b/nav2_bt_navigator/behavior_trees/auto_localization.xml
new file mode 100644
index 0000000000000000000000000000000000000000..485c383f6eb73bd4a34dd5a41f652e9fe00cca99
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/auto_localization.xml
@@ -0,0 +1,26 @@
+<!--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
new file mode 100644
index 0000000000000000000000000000000000000000..cb6e08948ad1f83d8ff4221f0371cf3a75aa7016
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/auto_localization_w_parallel_recovery.xml
@@ -0,0 +1,58 @@
+<!--
+ 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
deleted file mode 100644
index 34e21a5476ca2767e7cad4fb0627d34755fc36d9..0000000000000000000000000000000000000000
--- a/nav2_bt_navigator/behavior_trees/auto_localization_w_replanning_and_recovery.xml
+++ /dev/null
@@ -1,54 +0,0 @@
-<!--
- 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
new file mode 100644
index 0000000000000000000000000000000000000000..de5e05469a2ccccbee0f4b65af87594346863705
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/bt_navigator_params.yaml
@@ -0,0 +1,5 @@
+# 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_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_retry.xml
similarity index 64%
rename from nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
rename to nav2_bt_navigator/behavior_trees/navigate_w_recovery_retry.xml
index 6a4b0bdda8784d5c715a1156415dab809b47100c..f1e8e14bf3fa6c23fbbc9d66594387c02c9327b7 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_retry.xml
@@ -6,15 +6,21 @@
<BehaviorTree ID="MainTree">
<RetryUntilSuccesful name="retry_navigate" num_attempts="6">
<Fallback>
- <Sequence>
- <RateController hz="1.0">
+ <SequenceStar>
+ <ComputePathToPose goal="${goal}" path="${path}"/>
+ <Sequence>
<Fallback>
<GoalReached/>
- <ComputePathToPose goal="${goal}" path="${path}"/>
+ <RateController hz="1.0">
+ <Sequence>
+ <ComputePathToPose goal="${goal}" path="${path}"/>
+ <UpdatePath/>
+ </Sequence>
+ </RateController>
</Fallback>
- </RateController>
- <FollowPath path="${path}"/>
- </Sequence>
+ <FollowPath path="${path}"/>
+ </Sequence>
+ </SequenceStar>
<ForceFailure>
<SequenceStar name="recovery">
<clearEntirelyCostmapServiceRequest service_name="/local_costmap/clear_entirely_local_costmap"/>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
deleted file mode 100644
index 0a09dfc913a55bf3b7f5ee77e4909b946f708b62..0000000000000000000000000000000000000000
--- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-<!--
- 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/parallel.xml b/nav2_bt_navigator/behavior_trees/parallel.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4bb567a833c6bdbb5d644164a945034cde5ce026
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/parallel.xml
@@ -0,0 +1,23 @@
+<!--
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..76055671ecfc7e41a42f973f72a93711d1644cc6
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/parallel_w_recovery.xml
@@ -0,0 +1,34 @@
+<!--
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..4a0844c88f2683a78fc9a88c8ddd1fd34d28ef2e
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/simple_sequential.xml
@@ -0,0 +1,13 @@
+<!--
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..eb44e54ec01d87199e2ca91823bca58bedd394e0
--- /dev/null
+++ b/nav2_bt_navigator/behavior_trees/simple_sequential_w_recovery.xml
@@ -0,0 +1,26 @@
+<!--
+ 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_tasks/include/nav2_tasks/is_localized_condition.hpp b/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
index 485fe9c2f3433f10476aeabe28950e848a30538c..7049f1af6ffba701bcbdd603e8c0c0e3bfd44a7e 100644
--- a/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
+++ b/nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
@@ -45,24 +45,29 @@ public:
BT::NodeStatus tick() override
{
if (!initialized_) {
- initialize();
+ // 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;
}
return isLocalized() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
}
- void initialize()
+protected:
+ void cleanup()
{
- 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;
+ robot_.reset();
}
bool isLocalized()
@@ -88,18 +93,11 @@ public:
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;
- rclcpp::Node::SharedPtr node_;
+ nav2_lifecycle::LifecycleNode::SharedPtr node_;
std::unique_ptr<nav2_robot::Robot> robot_;
bool initialized_;