Skip to content

Commit 647b53b

Browse files
naiveHobonaiveHobo
authored
Add unit tests for clear entire costmap and reinitialize global localization BT service nodes (#1845)
* Add clear entire costmap service unit tests Signed-off-by: Sarthak Mittal <[email protected]> * Add reinitialize global localization service unit test Signed-off-by: Sarthak Mittal <[email protected]>
1 parent e610645 commit 647b53b

File tree

8 files changed

+414
-41
lines changed

8 files changed

+414
-41
lines changed

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/clear_costmap_service.hpp‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
// Copyright (c) 2019 Samsung Research America
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+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
16+
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
17+
18+
#include <string>
19+
20+
#include "nav2_behavior_tree/bt_service_node.hpp"
21+
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
22+
23+
namespace nav2_behavior_tree
24+
{
25+
26+
class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
27+
{
28+
public:
29+
ClearEntireCostmapService(
30+
const std::string & service_node_name,
31+
const BT::NodeConfiguration & conf);
32+
33+
void on_tick() override;
34+
};
35+
36+
} // namespace nav2_behavior_tree
37+
38+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_

‎nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
// Copyright (c) 2019 Samsung Research America
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+
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
16+
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
17+
18+
#include <string>
19+
20+
#include "nav2_behavior_tree/bt_service_node.hpp"
21+
#include "std_srvs/srv/empty.hpp"
22+
23+
namespace nav2_behavior_tree
24+
{
25+
26+
class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
27+
{
28+
public:
29+
ReinitializeGlobalLocalizationService(
30+
const std::string & service_node_name,
31+
const BT::NodeConfiguration & conf);
32+
};
33+
34+
} // namespace nav2_behavior_tree
35+
36+
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_

‎nav2_behavior_tree/plugins/action/clear_costmap_service.cpp‎

Lines changed: 10 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -12,34 +12,25 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_
16-
#define NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_
17-
1815
#include <string>
1916
#include <memory>
20-
#include <cmath>
2117

22-
#include "nav2_behavior_tree/bt_service_node.hpp"
23-
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
18+
#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
2419

2520
namespace nav2_behavior_tree
2621
{
2722

28-
class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
23+
ClearEntireCostmapService::ClearEntireCostmapService(
24+
const std::string & service_node_name,
25+
const BT::NodeConfiguration & conf)
26+
: BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, conf)
2927
{
30-
public:
31-
ClearEntireCostmapService(
32-
const std::string & service_node_name,
33-
const BT::NodeConfiguration & conf)
34-
: BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, conf)
35-
{
36-
}
28+
}
3729

38-
void on_tick() override
39-
{
40-
increment_recovery_count();
41-
}
42-
};
30+
void ClearEntireCostmapService::on_tick()
31+
{
32+
increment_recovery_count();
33+
}
4334

4435
} // namespace nav2_behavior_tree
4536

@@ -48,5 +39,3 @@ BT_REGISTER_NODES(factory)
4839
{
4940
factory.registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap");
5041
}
51-
52-
#endif // NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_

‎nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp‎

Lines changed: 6 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -12,29 +12,17 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
16-
#define NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
17-
1815
#include <string>
19-
#include <memory>
20-
#include <cmath>
21-
22-
#include "nav2_behavior_tree/bt_service_node.hpp"
23-
#include "std_srvs/srv/empty.hpp"
16+
#include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp"
2417

2518
namespace nav2_behavior_tree
2619
{
2720

28-
class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
29-
{
30-
public:
31-
ReinitializeGlobalLocalizationService(
32-
const std::string & service_node_name,
33-
const BT::NodeConfiguration & conf)
34-
: BtServiceNode<std_srvs::srv::Empty>(service_node_name, conf)
35-
{
36-
}
37-
};
21+
ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService(
22+
const std::string & service_node_name,
23+
const BT::NodeConfiguration & conf)
24+
: BtServiceNode<std_srvs::srv::Empty>(service_node_name, conf)
25+
{}
3826

3927
} // namespace nav2_behavior_tree
4028

@@ -44,5 +32,3 @@ BT_REGISTER_NODES(factory)
4432
factory.registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(
4533
"ReinitializeGlobalLocalization");
4634
}
47-
48-
#endif // NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_

‎nav2_behavior_tree/test/plugins/action/CMakeLists.txt‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,14 @@ ament_add_gtest(test_action_wait_action test_wait_action.cpp)
1010
target_link_libraries(test_action_wait_action nav2_wait_action_bt_node)
1111
ament_target_dependencies(test_action_wait_action ${dependencies})
1212

13+
ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp)
14+
target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node)
15+
ament_target_dependencies(test_action_clear_costmap_service ${dependencies})
16+
17+
ament_add_gtest(test_action_reinitialize_global_localization_service test_reinitialize_global_localization_service.cpp)
18+
target_link_libraries(test_action_reinitialize_global_localization_service nav2_reinitialize_global_localization_service_bt_node)
19+
ament_target_dependencies(test_action_reinitialize_global_localization_service ${dependencies})
20+
1321
ament_add_gtest(test_action_compute_path_to_pose_action test_compute_path_to_pose_action.cpp)
1422
target_link_libraries(test_action_compute_path_to_pose_action nav2_compute_path_to_pose_action_bt_node)
1523
ament_target_dependencies(test_action_compute_path_to_pose_action ${dependencies})

‎nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp‎

Lines changed: 130 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,130 @@
1+
// Copyright (c) 2018 Intel Corporation
2+
// Copyright (c) 2020 Sarthak Mittal
3+
//
4+
// Licensed under the Apache License, Version 2.0 (the "License");
5+
// you may not use this file except in compliance with the License.
6+
// You may obtain a copy of the License at
7+
//
8+
// http://www.apache.org/licenses/LICENSE-2.0
9+
//
10+
// Unless required by applicable law or agreed to in writing, software
11+
// distributed under the License is distributed on an "AS IS" BASIS,
12+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
// See the License for the specific language governing permissions and
14+
// limitations under the License.
15+
16+
#include <gtest/gtest.h>
17+
#include <memory>
18+
#include <set>
19+
#include <string>
20+
21+
#include "behaviortree_cpp_v3/bt_factory.h"
22+
23+
#include "../../test_service.hpp"
24+
#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
25+
26+
class ClearEntireCostmapService : public TestService<nav2_msgs::srv::ClearEntireCostmap>
27+
{
28+
public:
29+
ClearEntireCostmapService()
30+
: TestService("clear_entire_costmap")
31+
{}
32+
};
33+
34+
class ClearEntireCostmapServiceTestFixture : public ::testing::Test
35+
{
36+
public:
37+
static void SetUpTestCase()
38+
{
39+
node_ = std::make_shared<rclcpp::Node>("clear_entire_costmap_test_fixture");
40+
factory_ = std::make_shared<BT::BehaviorTreeFactory>();
41+
42+
config_ = new BT::NodeConfiguration();
43+
44+
// Create the blackboard that will be shared by all of the nodes in the tree
45+
config_->blackboard = BT::Blackboard::create();
46+
// Put items on the blackboard
47+
config_->blackboard->set<rclcpp::Node::SharedPtr>(
48+
"node",
49+
node_);
50+
config_->blackboard->set<std::chrono::milliseconds>(
51+
"server_timeout",
52+
std::chrono::milliseconds(10));
53+
config_->blackboard->set<bool>("path_updated", false);
54+
config_->blackboard->set<bool>("initial_pose_received", false);
55+
config_->blackboard->set<int>("number_recoveries", 0);
56+
57+
factory_->registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap");
58+
}
59+
60+
static void TearDownTestCase()
61+
{
62+
delete config_;
63+
config_ = nullptr;
64+
node_.reset();
65+
server_.reset();
66+
factory_.reset();
67+
}
68+
69+
void SetUp() override
70+
{
71+
config_->blackboard->set("number_recoveries", 0);
72+
}
73+
74+
void TearDown() override
75+
{
76+
tree_.reset();
77+
}
78+
79+
static std::shared_ptr<ClearEntireCostmapService> server_;
80+
81+
protected:
82+
static rclcpp::Node::SharedPtr node_;
83+
static BT::NodeConfiguration * config_;
84+
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
85+
static std::shared_ptr<BT::Tree> tree_;
86+
};
87+
88+
rclcpp::Node::SharedPtr ClearEntireCostmapServiceTestFixture::node_ = nullptr;
89+
std::shared_ptr<ClearEntireCostmapService> ClearEntireCostmapServiceTestFixture::server_ = nullptr;
90+
BT::NodeConfiguration * ClearEntireCostmapServiceTestFixture::config_ = nullptr;
91+
std::shared_ptr<BT::BehaviorTreeFactory> ClearEntireCostmapServiceTestFixture::factory_ = nullptr;
92+
std::shared_ptr<BT::Tree> ClearEntireCostmapServiceTestFixture::tree_ = nullptr;
93+
94+
TEST_F(ClearEntireCostmapServiceTestFixture, test_tick)
95+
{
96+
std::string xml_txt =
97+
R"(
98+
<root main_tree_to_execute = "MainTree" >
99+
<BehaviorTree ID="MainTree">
100+
<ClearEntireCostmap service_name="clear_entire_costmap"/>
101+
</BehaviorTree>
102+
</root>)";
103+
104+
tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
105+
EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
106+
EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
107+
EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
108+
}
109+
110+
int main(int argc, char ** argv)
111+
{
112+
::testing::InitGoogleTest(&argc, argv);
113+
114+
// initialize ROS
115+
rclcpp::init(argc, argv);
116+
117+
// initialize service and spin on new thread
118+
ClearEntireCostmapServiceTestFixture::server_ = std::make_shared<ClearEntireCostmapService>();
119+
std::thread server_thread([]() {
120+
rclcpp::spin(ClearEntireCostmapServiceTestFixture::server_);
121+
});
122+
123+
int all_successful = RUN_ALL_TESTS();
124+
125+
// shutdown ROS
126+
rclcpp::shutdown();
127+
server_thread.join();
128+
129+
return all_successful;
130+
}

0 commit comments

Comments
 (0)