Skip to content
Snippets Groups Projects
Commit ef0b2aaf authored by Mohammad Haghighipanah's avatar Mohammad Haghighipanah Committed by Carl Delsey
Browse files

Revert "Modified all the Behaivor Tree's to work with lifecycle branch (#772)" (#776) 

This reverts commit e4452eb04002e71d801c5b87abb1e7866bdd550c.
parent 7d1cb08b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 216 additions and 30 deletions
nav2_bringup/launch/bt_navigator.xml
+ 1
− 4
View file @ ef0b2aaf
......@@ -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}"/>
......
nav2_bt_navigator/behavior_trees/auto_localization.xml 0 → 100644
+ 26
− 0
View file @ ef0b2aaf
<!--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
nav2_bt_navigator/behavior_trees/auto_localization_w_replanning_and_recovery.xml → nav2_bt_navigator/behavior_trees/auto_localization_w_parallel_recovery.xml
+ 58
− 0
View file @ ef0b2aaf
<!--
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.
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">
<SequenceStar name="root">
<Sequence name="root">
<FallbackStar name="AutoLocalization">
<initialPoseReceived/>
<SequenceStar name="doSelfLocalization">
......@@ -27,27 +29,29 @@
</RetryUntilSuccesful>
</SequenceStar>
</FallbackStar>
<RetryUntilSuccesful name="navigate_w_replanning_and_retry" num_attempts="6">
<Fallback>
<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="1.0">
<Fallback>
<GoalReached/>
<ComputePathToPose goal="${goal}" path="${path}"/>
</Fallback>
<RateController hz="2">
<ComputePathToPose endpoints="${endpoints}" path="${path}"/>
</RateController>
<FollowPath path="${path}"/>
<UpdatePath/>
</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>
</ParallelNode>
</SequenceStar>
</Sequence>
</BehaviorTree>
</root>
......
nav2_bt_navigator/behavior_trees/bt_navigator_params.yaml 0 → 100644
+ 5
− 0
View file @ ef0b2aaf
# 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
nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml → nav2_bt_navigator/behavior_trees/navigate_w_recovery_retry.xml
+ 12
− 6
View file @ ef0b2aaf
......@@ -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"/>
......
nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml → nav2_bt_navigator/behavior_trees/parallel.xml
+ 23
− 0
View file @ ef0b2aaf
......@@ -4,14 +4,20 @@
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<RateController hz="1.0">
<SequenceStar name="root">
<ComputePathToPose goal="${goal}" path="${path}"/>
<Sequence>
<Fallback>
<GoalReached/>
<ComputePathToPose goal="${goal}"/>
<RateController hz="1.0">
<Sequence>
<ComputePathToPose goal="${goal}" path="${path}"/>
<UpdatePath/>
</Sequence>
</RateController>
</Fallback>
</RateController>
<FollowPath path="${path}"/>
</Sequence>
<FollowPath path="${path}"/>
</Sequence>
</SequenceStar>
</BehaviorTree>
</root>
\ No newline at end of file
nav2_bt_navigator/behavior_trees/parallel_w_recovery.xml 0 → 100644
+ 34
− 0
View file @ ef0b2aaf
<!--
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>
nav2_bt_navigator/behavior_trees/simple_sequential.xml 0 → 100644
+ 13
− 0
View file @ ef0b2aaf
<!--
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>
nav2_bt_navigator/behavior_trees/simple_sequential_w_recovery.xml 0 → 100644
+ 26
− 0
View file @ ef0b2aaf
<!--
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>
nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
+ 18
− 20
View file @ ef0b2aaf
......@@ -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_;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment