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

Modified all the Behaivor Tree's to work with lifecycle branch (#784) 

* updated the bt-navigator to stop path planning after reaching the goal

* updated navigate w recovery tree to work with updated ratecontroller

* changed parallel to replanning

* renammed recovery tree

* deleted unnecessary trees

* modified auto-localization tree

* fixed a bug in isLocalized

* deleted auto-localization tree

* fixed uncrustify

* fixed the rebasing problem
parent ef0b2aaf
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 103 additions and 138 deletions
nav2_bringup/launch/bt_navigator.xml
+ 5
− 4
View file @ 612bb2e6
......@@ -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>
nav2_bt_navigator/behavior_trees/auto_localization.xml deleted 100644 → 0
+ 0
− 26
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_parallel_recovery.xml → nav2_bt_navigator/behavior_trees/auto_localization_w_replanning_and_recovery.xml
+ 54
− 0
View file @ 612bb2e6
<!--
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`.
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">
<Sequence name="root">
<SequenceStar name="root">
<FallbackStar name="AutoLocalization">
<initialPoseReceived/>
<SequenceStar name="doSelfLocalization">
......@@ -29,29 +27,27 @@
</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}"/>
<RetryUntilSuccesful name="navigate_w_replanning_and_retry" num_attempts="6">
<Fallback>
<Sequence>
<RateController hz="2">
<ComputePathToPose endpoints="${endpoints}" path="${path}"/>
<RateController hz="1.0">
<Fallback>
<GoalReached/>
<ComputePathToPose goal="${goal}" path="${path}"/>
</Fallback>
</RateController>
<UpdatePath/>
<FollowPath path="${path}"/>
</Sequence>
</ParallelNode>
</SequenceStar>
</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>
......
nav2_bt_navigator/behavior_trees/bt_navigator_params.yaml deleted 100644 → 0
+ 0
− 5
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/parallel.xml → nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml
+ 17
− 0
View file @ 612bb2e6
......@@ -4,20 +4,14 @@
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<SequenceStar name="root">
<ComputePathToPose goal="${goal}" path="${path}"/>
<Sequence>
<Sequence name="root">
<RateController hz="1.0">
<Fallback>
<GoalReached/>
<RateController hz="1.0">
<Sequence>
<ComputePathToPose goal="${goal}" path="${path}"/>
<UpdatePath/>
</Sequence>
</RateController>
<ComputePathToPose goal="${goal}"/>
</Fallback>
<FollowPath path="${path}"/>
</Sequence>
</SequenceStar>
</RateController>
<FollowPath path="${path}"/>
</Sequence>
</BehaviorTree>
</root>
\ No newline at end of file
nav2_bt_navigator/behavior_trees/navigate_w_recovery_retry.xml → nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml
+ 6
− 12
View file @ 612bb2e6
......@@ -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"/>
......
nav2_bt_navigator/behavior_trees/parallel_w_recovery.xml deleted 100644 → 0
+ 0
− 34
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 deleted 100644 → 0
+ 0
− 13
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 deleted 100644 → 0
+ 0
− 26
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_bt_navigator/src/bt_navigator.cpp
+ 1
− 0
View file @ 612bb2e6
......@@ -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
......
nav2_tasks/include/nav2_tasks/is_localized_condition.hpp
+ 20
− 18
View file @ 612bb2e6
......@@ -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_;
......
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