Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

(WIP) Recoveries always return success, regardless if they're able to complete their required tasks or not #1855

Merged
Merged
Show file tree
Hide file tree
Changes from 64 commits
Commits
Show all changes
67 commits
Select commit Hold shift + click to select a range
a9a6caf
modified such that if the recovery is aborted due to potential collis…
vinnnyr Jul 4, 2020
6aaf8f4
Changed from SequenceStar to RoundRobin to mask any failure that migh…
vinnnyr Jul 11, 2020
cc27817
Change Backup recovery test such that the aborted recovery is expecte…
vinnnyr Jul 12, 2020
57ef361
fixing linting error
vinnnyr Jul 12, 2020
dc351d6
Change depricated argument for backup recovery test
vinnnyr Jul 12, 2020
714c7ad
added backup to the recovery actions, using defaults from the BackUp …
vinnnyr Jul 19, 2020
fb950b2
Update nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_rec…
vinnnyr Jul 21, 2020
d48915f
Merge branch 'main' of https://github.com/ros-planning/navigation2 in…
vinnnyr Aug 13, 2020
d257edd
Reverting changes to BT. Seq. Star control node is in line to what we…
vinnnyr Aug 16, 2020
d7a6572
sync with main
vinnnyr Sep 26, 2020
7af1f82
Merge branch 'main' into 1779-00-change-recovery-behavior
vinnnyr Oct 11, 2020
bd4929b
checkpoitn commit
vinnnyr Oct 11, 2020
b37f357
Changed collision failure critera as backup collision abortion occurs…
vinnnyr Oct 11, 2020
12edd2c
bool to control whether fake costmap should be generated
vinnnyr Oct 11, 2020
e7ad19f
Merge branch '1779-00-change-recovery-behavior' into scratch/spintest
vinnnyr Oct 11, 2020
ed25c93
added fake tester nodes
vinnnyr Oct 11, 2020
0b55041
Fake costmap passed through environmental variable now
vinnnyr Oct 11, 2020
0151db2
Fake Footprint being sent out
vinnnyr Oct 12, 2020
1aba980
breadcrumbs of fake spin tests
vinnnyr Oct 12, 2020
89ea257
fake odom
vinnnyr Oct 12, 2020
bd746a8
launch file was messed up
vinnnyr Oct 12, 2020
0b31314
still troubleshooting spin
vinnnyr Oct 13, 2020
538d13a
trying to fake spin, goal keeps on getting rejected
vinnnyr Oct 13, 2020
3bd1e3d
goal is now being accepted
vinnnyr Oct 13, 2020
d571bd2
Send odom again during default test, not able to get spin recovery ac…
vinnnyr Oct 13, 2020
458bd05
Fake spin not working well when asking for PI radians out
vinnnyr Oct 22, 2020
fd7b2e2
Merge branch 'main' into 1779-00-change-recovery-behavior
vinnnyr Oct 22, 2020
dab14f0
Merge branch 'main' into 1779-00-change-recovery-behavior
vinnnyr Oct 27, 2020
68438aa
playing around with timing wehn publishing fake transforms
vinnnyr Oct 27, 2020
e61a4c6
Merge branch 'main' into 1779-00-change-recovery-behavior
vinnnyr Nov 1, 2020
ab48111
BT changes, testing TBD
vinnnyr Nov 1, 2020
f5ea383
[WIP] Fix CI build issues (#2076)
SteveMacenski Nov 5, 2020
18d6137
Patch for PhotoAtWaypoint plugin (#2067)
SteveMacenski Nov 6, 2020
ca34f00
Fix for double free issue in map server testcases (#2078)
AlexeyMerzlyakov Nov 6, 2020
5736ce6
Support in keepout filter for mask and costmap published in different…
AlexeyMerzlyakov Nov 6, 2020
b866701
Loop fix (#2068)
Nov 7, 2020
01926c9
test if action server failures correctly propagate into BT context
vinnnyr Nov 9, 2020
8222bf2
Merge branch 'main' into 1779-00-change-recovery-behavior
vinnnyr Nov 9, 2020
164f55b
change recovery subtree so that RoundRobin replaces SequenceStar for …
vinnnyr Nov 9, 2020
0fce77d
uncrustify and line nav2_behavior_tree changes
vinnnyr Nov 9, 2020
e0b42da
uncrustify and linting
vinnnyr Nov 10, 2020
87ff83f
adding clearing actions to own subtree
vinnnyr Nov 10, 2020
360ba8f
more linting
vinnnyr Nov 10, 2020
21fe80a
reduced testing value to see if that will stop the robot from abortin…
vinnnyr Nov 10, 2020
b8c3c8e
tighten up tolerance on backup since the requested backup is so small
vinnnyr Nov 10, 2020
c38ae23
delint
vinnnyr Nov 10, 2020
eb0ce17
cleaned up spin recovery such that we simulate the robot slowly spinn…
vinnnyr Nov 10, 2020
767fbb4
linting
vinnnyr Nov 10, 2020
46b6391
change order of Spin and Wait to match original
vinnnyr Nov 10, 2020
d12addd
fake spin failing due to potential collision even when empty costmap
vinnnyr Nov 10, 2020
049f5f3
lint
vinnnyr Nov 18, 2020
6f90283
more lint
vinnnyr Nov 18, 2020
09a9311
can get all the spins to pass now
vinnnyr Nov 18, 2020
0119136
spin test strangely passing all cases even though costmap is populated
vinnnyr Nov 18, 2020
112955e
spin recovery
vinnnyr Nov 22, 2020
7f2a1c0
commmit before I try to visualize this
vinnnyr Nov 22, 2020
4b057eb
now I know that there are two things publishing to the same footprint
vinnnyr Nov 23, 2020
bb99b20
1) figured out conflicting pubs to footprint, 2) why can't I get cost…
vinnnyr Nov 23, 2020
931f1c3
Fake Spin Test shows failure correctly for angles greater than pi / 2…
vinnnyr Nov 24, 2020
045f5cd
fake launch lint
vinnnyr Nov 25, 2020
fae4429
check in with linting to show the structure
vinnnyr Dec 7, 2020
269e078
Merge branch 'main' into 1779-00-change-recovery-behavior
naiveHobo Feb 7, 2021
85e1c81
Add overall behavior tree system tests
naiveHobo Feb 9, 2021
dd34c89
Add RecoverySubtreeGoalUpdated BT test
naiveHobo Feb 9, 2021
0929255
Update readme and docs
naiveHobo Feb 10, 2021
00286f3
Add tests to check if BT XML files are well-formed
naiveHobo Feb 12, 2021
e1fe0c3
Uncomment tests that were commented out
naiveHobo Feb 12, 2021
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 40 additions & 2 deletions nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,19 @@ class BackUpActionServer : public TestActionServer<nav2_msgs::action::BackUp>

protected:
void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>)
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>
goal_handle)
override
{}
{
nav2_msgs::action::BackUp::Result::SharedPtr result =
std::make_shared<nav2_msgs::action::BackUp::Result>();
bool return_success = getReturnSuccess();
if (return_success) {
goal_handle->succeed(result);
} else {
goal_handle->abort(result);
}
}
};

class BackUpActionTestFixture : public ::testing::Test
Expand Down Expand Up @@ -154,6 +164,34 @@ TEST_F(BackUpActionTestFixture, test_tick)
EXPECT_EQ(goal->speed, 0.26f);
}

TEST_F(BackUpActionTestFixture, test_failure)
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<BackUp backup_dist="2" backup_speed="0.26" />
</BehaviorTree>
</root>)";

tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
action_server_->setReturnSuccess(false);
EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);

while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
{
tree_->rootNode()->executeTick();
}

EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);

auto goal = action_server_->getCurrentGoal();
EXPECT_EQ(goal->target.x, 2.0);
EXPECT_EQ(goal->speed, 0.26f);
}

int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);
Expand Down
41 changes: 39 additions & 2 deletions nav2_behavior_tree/test/plugins/action/test_spin_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,19 @@ class SpinActionServer : public TestActionServer<nav2_msgs::action::Spin>

protected:
void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>)
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>
goal_handle)
override
{}
{
nav2_msgs::action::Spin::Result::SharedPtr result =
std::make_shared<nav2_msgs::action::Spin::Result>();
bool return_success = getReturnSuccess();
if (return_success) {
goal_handle->succeed(result);
} else {
goal_handle->abort(result);
}
}
};

class SpinActionTestFixture : public ::testing::Test
Expand Down Expand Up @@ -149,6 +159,33 @@ TEST_F(SpinActionTestFixture, test_tick)
EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f);
}

TEST_F(SpinActionTestFixture, test_failure)
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Spin spin_dist="3.14" />
</BehaviorTree>
</root>)";

tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
action_server_->setReturnSuccess(false);

EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS &&
tree_->rootNode()->status() != BT::NodeStatus::FAILURE)
{
tree_->rootNode()->executeTick();
}

std::cout << tree_->rootNode()->status();

EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE);
EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
EXPECT_EQ(action_server_->getCurrentGoal()->target_yaw, 3.14f);
}

int main(int argc, char ** argv)
{
::testing::InitGoogleTest(&argc, argv);
Expand Down
11 changes: 11 additions & 0 deletions nav2_behavior_tree/test/test_action_server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,16 @@ class TestActionServer : public rclcpp::Node
return current_goal_;
}

void setReturnSuccess(bool return_success)
{
return_success_ = return_success;
}

bool getReturnSuccess(void)
{
return return_success_;
}

protected:
virtual rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID &,
Expand Down Expand Up @@ -77,6 +87,7 @@ class TestActionServer : public rclcpp::Node
private:
typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
std::shared_ptr<const typename ActionT::Goal> current_goal_;
bool return_success_ = true;
};

#endif // TEST_ACTION_SERVER_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,15 @@
</PipelineSequence>
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
<ReactiveFallback name="RecoveryFallback">
<GoalUpdated/>
<SequenceStar name="RecoveryActions">
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
<RoundRobin name="RecoveryActions">
<Sequence name="ClearingActions">
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<Spin spin_dist="1.57"/>
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
<Wait wait_duration="5"/>
vinnnyr marked this conversation as resolved.
Show resolved Hide resolved
</SequenceStar>
<BackUp backup_dist="0.15" backup_speed="0.025"/>
</RoundRobin>
</ReactiveFallback>
</RecoveryNode>
</BehaviorTree>
Expand Down
2 changes: 1 addition & 1 deletion nav2_recoveries/plugins/back_up.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ Status BackUp::onCycleUpdate()
if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) {
stopRobot();
RCLCPP_WARN(logger_, "Collision Ahead - Exiting BackUp");
return Status::SUCCEEDED;
return Status::FAILED;
}

vel_pub_->publish(std::move(cmd_vel));
Expand Down
8 changes: 6 additions & 2 deletions nav2_recoveries/plugins/spin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -119,9 +119,13 @@ Status Spin::onCycleUpdate()
action_server_->publish_feedback(feedback_);

double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_);
if (remaining_yaw <= 0) {
if (remaining_yaw < 1e-6) {
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
stopRobot();
return Status::SUCCEEDED;
} else {
RCLCPP_INFO(
logger_, "Remaining Yaw: %lf",
naiveHobo marked this conversation as resolved.
Show resolved Hide resolved
remaining_yaw);
}

double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw);
Expand All @@ -138,7 +142,7 @@ Status Spin::onCycleUpdate()
if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) {
stopRobot();
RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin");
return Status::SUCCEEDED;
return Status::FAILED;
}

vel_pub_->publish(std::move(cmd_vel));
Expand Down
5 changes: 5 additions & 0 deletions nav2_system_tests/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(nav2_util REQUIRED)
find_package(nav2_map_server REQUIRED)
find_package(nav2_behavior_tree REQUIRED)
find_package(nav2_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)
Expand All @@ -21,6 +22,7 @@ find_package(nav2_navfn_planner REQUIRED)
find_package(nav2_planner REQUIRED)
find_package(navigation2)
find_package(angles REQUIRED)
find_package(behaviortree_cpp_v3 REQUIRED)

nav2_package()

Expand All @@ -33,6 +35,7 @@ set(dependencies
visualization_msgs
nav2_amcl
nav2_lifecycle_manager
nav2_behavior_tree
gazebo_ros_pkgs
geometry_msgs
std_msgs
Expand All @@ -41,6 +44,7 @@ set(dependencies
nav2_planner
nav2_navfn_planner
angles
behaviortree_cpp_v3
)

if(BUILD_TESTING)
Expand All @@ -50,6 +54,7 @@ if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_pytest REQUIRED)

add_subdirectory(src/behavior_tree)
add_subdirectory(src/planning)
add_subdirectory(src/localization)
add_subdirectory(src/system)
Expand Down
2 changes: 2 additions & 0 deletions nav2_system_tests/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
<build_depend>nav2_msgs</build_depend>
<build_depend>nav2_lifecycle_manager</build_depend>
<build_depend>nav2_navfn_planner</build_depend>
<build_depend>nav2_behavior_tree</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>nav2_amcl</build_depend>
Expand All @@ -40,6 +41,7 @@
<exec_depend>nav2_msgs</exec_depend>
<exec_depend>nav2_lifecycle_manager</exec_depend>
<exec_depend>nav2_navfn_planner</exec_depend>
<exec_depend>nav2_behavior_tree</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
Expand Down
8 changes: 8 additions & 0 deletions nav2_system_tests/src/behavior_tree/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
ament_add_gtest(test_behavior_tree_node
test_behavior_tree_node.cpp
server_handler.cpp
)

ament_target_dependencies(test_behavior_tree_node
${dependencies}
)
Loading