Skip to content

Commit e3fac7c

Browse files
SteveMacenskiSteveMacenski
committed
adding unit tests
1 parent c12ded8 commit e3fac7c

File tree

7 files changed

+547
-4
lines changed

7 files changed

+547
-4
lines changed

‎nav2_coverage/README.md

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -65,9 +65,12 @@ Walk through
6565
- RQT
6666

6767
Future
68-
- Test BT nodes / XML / Navigator // unit tests
69-
- Create simulation to testing / demo
68+
- Use setup with BT nodes / XML / Navigator. Simulator demo altogether. Demo video.
69+
- README
7070

71+
72+
- Python3 API from tester.py
73+
- Rename packages / repo?
7174
- A couple of utilities for the BT nodes to iterate through the swath-turn combos (optional)
7275

7376

‎nav2_coverage_bt/test/CMakeLists.txt

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
# Cancel test
2+
ament_add_gtest(test_cancel_complete_coverage test_cancel_complete_coverage.cpp)
3+
target_link_libraries(test_cancel_complete_coverage nav2_cancel_complete_coverage_action_bt_node)
4+
ament_target_dependencies(test_cancel_complete_coverage ${dependencies})
5+
6+
# Make command test
7+
ament_add_gtest(test_compute_coverage_path test_compute_coverage_path.cpp)
8+
target_link_libraries(test_compute_coverage_path nav2_compute_complete_coverage_action_bt_node)
9+
ament_target_dependencies(test_compute_coverage_path ${dependencies})

‎nav2_coverage_bt/test/test_cancel_complete_coverage.cpp

Lines changed: 176 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,176 @@
1+
// Copyright (c) 2023 Open Navigation LLC
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <gtest/gtest.h>
16+
#include <memory>
17+
#include <set>
18+
#include <string>
19+
20+
#include "behaviortree_cpp_v3/bt_factory.h"
21+
22+
#include "nav2_behavior_tree/utils/test_action_server.hpp"
23+
#include "nav2_coverage_bt/cancel_complete_coverage_path.hpp"
24+
#include "lifecycle_msgs/srv/change_state.hpp"
25+
26+
class CancelCoverageServer
27+
: public TestActionServer<nav2_complete_coverage_msgs::action::ComputeCoveragePath>
28+
{
29+
public:
30+
CancelCoverageServer()
31+
: TestActionServer("compute_coverage_path")
32+
{}
33+
34+
protected:
35+
void execute(
36+
const typename std::shared_ptr<
37+
rclcpp_action::ServerGoalHandle<nav2_complete_coverage_msgs::action::ComputeCoveragePath>>
38+
goal_handle)
39+
{
40+
while (!goal_handle->is_canceling()) {
41+
// Coverage here until goal cancels
42+
std::this_thread::sleep_for(std::chrono::milliseconds(15));
43+
}
44+
}
45+
};
46+
47+
class CancelCoverageActionTestFixture : public ::testing::Test
48+
{
49+
public:
50+
static void SetUpTestCase()
51+
{
52+
node_ = std::make_shared<rclcpp::Node>("cancel_compute_coverage_path_test_fixture");
53+
factory_ = std::make_shared<BT::BehaviorTreeFactory>();
54+
55+
config_ = new BT::NodeConfiguration();
56+
57+
// Create the blackboard that will be shared by all of the nodes in the tree
58+
config_->blackboard = BT::Blackboard::create();
59+
// Put items on the blackboard
60+
config_->blackboard->set<rclcpp::Node::SharedPtr>(
61+
"node",
62+
node_);
63+
config_->blackboard->set<std::chrono::milliseconds>(
64+
"server_timeout",
65+
std::chrono::milliseconds(20));
66+
config_->blackboard->set<std::chrono::milliseconds>(
67+
"bt_loop_duration",
68+
std::chrono::milliseconds(10));
69+
client_ =
70+
rclcpp_action::create_client<nav2_complete_coverage_msgs::action::ComputeCoveragePath>(
71+
node_, "compute_coverage_path");
72+
73+
BT::NodeBuilder builder =
74+
[](const std::string & name, const BT::NodeConfiguration & config)
75+
{
76+
return std::make_unique<nav2_coverage_bt::CoverageCancel>(
77+
name, "compute_coverage_path", config);
78+
};
79+
80+
factory_->registerBuilder<nav2_coverage_bt::CoverageCancel>(
81+
"CancelCoverage", builder);
82+
}
83+
84+
static void TearDownTestCase()
85+
{
86+
delete config_;
87+
config_ = nullptr;
88+
node_.reset();
89+
action_server_.reset();
90+
client_.reset();
91+
factory_.reset();
92+
}
93+
94+
void TearDown() override
95+
{
96+
tree_.reset();
97+
}
98+
99+
static std::shared_ptr<CancelCoverageServer> action_server_;
100+
static std::shared_ptr<
101+
rclcpp_action::Client<nav2_complete_coverage_msgs::action::ComputeCoveragePath>> client_;
102+
103+
protected:
104+
static rclcpp::Node::SharedPtr node_;
105+
static BT::NodeConfiguration * config_;
106+
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
107+
static std::shared_ptr<BT::Tree> tree_;
108+
};
109+
110+
rclcpp::Node::SharedPtr CancelCoverageActionTestFixture::node_ = nullptr;
111+
std::shared_ptr<CancelCoverageServer>
112+
CancelCoverageActionTestFixture::action_server_ = nullptr;
113+
std::shared_ptr<rclcpp_action::Client<nav2_complete_coverage_msgs::action::ComputeCoveragePath>>
114+
CancelCoverageActionTestFixture::client_ = nullptr;
115+
116+
BT::NodeConfiguration * CancelCoverageActionTestFixture::config_ = nullptr;
117+
std::shared_ptr<BT::BehaviorTreeFactory>
118+
CancelCoverageActionTestFixture::factory_ = nullptr;
119+
std::shared_ptr<BT::Tree> CancelCoverageActionTestFixture::tree_ = nullptr;
120+
121+
TEST_F(CancelCoverageActionTestFixture, test_ports)
122+
{
123+
std::string xml_txt =
124+
R"(
125+
<root main_tree_to_execute = "MainTree" >
126+
<BehaviorTree ID="MainTree">
127+
<CancelCoverage name="CoverageCancel"/>
128+
</BehaviorTree>
129+
</root>)";
130+
131+
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
132+
auto send_goal_options = rclcpp_action::Client<
133+
nav2_complete_coverage_msgs::action::ComputeCoveragePath>::SendGoalOptions();
134+
135+
// Creating a dummy goal_msg
136+
auto goal_msg = nav2_complete_coverage_msgs::action::ComputeCoveragePath::Goal();
137+
138+
// BackUping for server and sending a goal
139+
client_->wait_for_action_server();
140+
client_->async_send_goal(goal_msg, send_goal_options);
141+
142+
// Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
143+
std::this_thread::sleep_for(std::chrono::milliseconds(15));
144+
145+
// Executing tick
146+
tree_->rootNode()->executeTick();
147+
148+
// BT node should return success, once when the goal is cancelled
149+
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
150+
151+
// Adding another test case to check if the goal is infact cancelling
152+
EXPECT_EQ(action_server_->isGoalCancelled(), true);
153+
}
154+
155+
int main(int argc, char ** argv)
156+
{
157+
::testing::InitGoogleTest(&argc, argv);
158+
159+
// initialize ROS
160+
rclcpp::init(argc, argv);
161+
162+
// initialize action server and back_up on new thread
163+
CancelCoverageActionTestFixture::action_server_ =
164+
std::make_shared<CancelCoverageServer>();
165+
std::thread server_thread([]() {
166+
rclcpp::spin(CancelCoverageActionTestFixture::action_server_);
167+
});
168+
169+
int all_successful = RUN_ALL_TESTS();
170+
171+
// shutdown ROS
172+
rclcpp::shutdown();
173+
server_thread.join();
174+
175+
return all_successful;
176+
}

‎nav2_coverage_bt/test/test_compute_coverage_path.cpp

Lines changed: 173 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,173 @@
1+
// Copyright (c) 2023 Open Navigation LLC
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <gtest/gtest.h>
16+
#include <memory>
17+
#include <set>
18+
#include <string>
19+
20+
#include "nav_msgs/msg/path.hpp"
21+
#include "geometry_msgs/msg/pose_stamped.hpp"
22+
23+
#include "behaviortree_cpp_v3/bt_factory.h"
24+
25+
#include "nav2_behavior_tree/utils/test_action_server.hpp"
26+
#include "nav2_coverage_bt/compute_complete_coverage_path.hpp"
27+
28+
class ComputeCompleteCoveragePathActionServer
29+
: public TestActionServer<nav2_complete_coverage_msgs::action::ComputeCoveragePath>
30+
{
31+
public:
32+
ComputeCompleteCoveragePathActionServer()
33+
: TestActionServer("compute_coverage_path")
34+
{}
35+
36+
protected:
37+
void execute(
38+
const typename std::shared_ptr<
39+
rclcpp_action::ServerGoalHandle<nav2_complete_coverage_msgs::action::ComputeCoveragePath>>
40+
goal_handle)
41+
override
42+
{
43+
const auto goal = goal_handle->get_goal();
44+
auto result =
45+
std::make_shared<nav2_complete_coverage_msgs::action::ComputeCoveragePath::Result>();
46+
result->nav_path.poses.resize(2);
47+
result->nav_path.poses[1].pose.position.x = 1.0;
48+
result->nav_path.poses[0].pose.position.x = 0.0;
49+
goal_handle->succeed(result);
50+
}
51+
};
52+
53+
class ComputeCoveragePathActionTestFixture : public ::testing::Test
54+
{
55+
public:
56+
static void SetUpTestCase()
57+
{
58+
node_ = std::make_shared<rclcpp::Node>("compute_coverage_path_action_test_fixture");
59+
factory_ = std::make_shared<BT::BehaviorTreeFactory>();
60+
61+
config_ = new BT::NodeConfiguration();
62+
63+
// Create the blackboard that will be shared by all of the nodes in the tree
64+
config_->blackboard = BT::Blackboard::create();
65+
// Put items on the blackboard
66+
config_->blackboard->set<rclcpp::Node::SharedPtr>(
67+
"node",
68+
node_);
69+
config_->blackboard->set<std::chrono::milliseconds>(
70+
"server_timeout",
71+
std::chrono::milliseconds(20));
72+
config_->blackboard->set<std::chrono::milliseconds>(
73+
"bt_loop_duration",
74+
std::chrono::milliseconds(10));
75+
config_->blackboard->set<bool>("initial_pose_received", false);
76+
77+
BT::NodeBuilder builder =
78+
[](const std::string & name, const BT::NodeConfiguration & config)
79+
{
80+
return std::make_unique<nav2_coverage_bt::ComputeCoveragePathAction>(
81+
name, "compute_coverage_path", config);
82+
};
83+
84+
factory_->registerBuilder<nav2_coverage_bt::ComputeCoveragePathAction>(
85+
"ComputeCoveragePath", builder);
86+
}
87+
88+
static void TearDownTestCase()
89+
{
90+
delete config_;
91+
config_ = nullptr;
92+
node_.reset();
93+
action_server_.reset();
94+
factory_.reset();
95+
}
96+
97+
void TearDown() override
98+
{
99+
tree_.reset();
100+
}
101+
102+
static std::shared_ptr<ComputeCompleteCoveragePathActionServer> action_server_;
103+
104+
protected:
105+
static rclcpp::Node::SharedPtr node_;
106+
static BT::NodeConfiguration * config_;
107+
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
108+
static std::shared_ptr<BT::Tree> tree_;
109+
};
110+
111+
rclcpp::Node::SharedPtr ComputeCoveragePathActionTestFixture::node_ = nullptr;
112+
std::shared_ptr<ComputeCompleteCoveragePathActionServer>
113+
ComputeCoveragePathActionTestFixture::action_server_ = nullptr;
114+
BT::NodeConfiguration * ComputeCoveragePathActionTestFixture::config_ = nullptr;
115+
std::shared_ptr<BT::BehaviorTreeFactory> ComputeCoveragePathActionTestFixture::factory_ = nullptr;
116+
std::shared_ptr<BT::Tree> ComputeCoveragePathActionTestFixture::tree_ = nullptr;
117+
118+
TEST_F(ComputeCoveragePathActionTestFixture, test_tick)
119+
{
120+
// create tree
121+
std::string xml_txt =
122+
R"(
123+
<root main_tree_to_execute = "MainTree" >
124+
<BehaviorTree ID="MainTree">
125+
<ComputeCoveragePath nav_path="{path}"/>
126+
</BehaviorTree>
127+
</root>)";
128+
129+
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
130+
131+
// tick until node succeeds
132+
while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
133+
tree_->rootNode()->executeTick();
134+
}
135+
136+
// the goal should have reached our server
137+
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
138+
139+
// check if returned path is correct
140+
nav_msgs::msg::Path path;
141+
config_->blackboard->get<nav_msgs::msg::Path>("path", path);
142+
EXPECT_EQ(path.poses.size(), 2u);
143+
EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
144+
EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
145+
146+
// halt node so another goal can be sent
147+
tree_->rootNode()->halt();
148+
EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
149+
}
150+
151+
int main(int argc, char ** argv)
152+
{
153+
::testing::InitGoogleTest(&argc, argv);
154+
155+
// initialize ROS
156+
rclcpp::init(argc, argv);
157+
158+
// initialize action server and spin on new thread
159+
ComputeCoveragePathActionTestFixture::action_server_ =
160+
std::make_shared<ComputeCompleteCoveragePathActionServer>();
161+
162+
std::thread server_thread([]() {
163+
rclcpp::spin(ComputeCoveragePathActionTestFixture::action_server_);
164+
});
165+
166+
int all_successful = RUN_ALL_TESTS();
167+
168+
// shutdown ROS
169+
rclcpp::shutdown();
170+
server_thread.join();
171+
172+
return all_successful;
173+
}

‎nav2_coverage_navigator/src/coverage_navigator.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -65,12 +65,12 @@ CoverageNavigator::getDefaultBTFilepath(
6565
std::string pkg_share_dir =
6666
ament_index_cpp::get_package_share_directory("nav2_coverage_bt");
6767
node->declare_parameter<std::string>(
68-
"default_nav_to_pose_bt_xml",
68+
"default_coverage_bt_xml",
6969
pkg_share_dir +
7070
"/behavior_trees/navigate_w_basic_complete_coverage.xml");
7171
}
7272

73-
node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename);
73+
node->get_parameter("default_coverage_bt_xml", default_bt_xml_filename);
7474

7575
return default_bt_xml_filename;
7676
}

‎nav2_coverage_navigator/test/CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
ament_add_gtest(test_coverage_navigator test_coverage_navigator.cpp)
2+
target_link_libraries(test_coverage_navigator nav2_coverage_navigator)
3+
ament_target_dependencies(test_coverage_navigator ${dependencies})

0 commit comments

Comments
 (0)