Add unit tests for clear entire costmap and reinitialize global localization BT service nodes

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

@@ -0,0 +1,38 @@
+// Copyright (c) 2019 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
+
+#include <string>
+
+#include "nav2_behavior_tree/bt_service_node.hpp"
+#include "nav2_msgs/srv/clear_entire_costmap.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
+{
+public:
+  ClearEntireCostmapService(
+    const std::string & service_node_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_

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

@@ -0,0 +1,36 @@
+// Copyright (c) 2019 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
+
+#include <string>
+
+#include "nav2_behavior_tree/bt_service_node.hpp"
+#include "std_srvs/srv/empty.hpp"
+
+namespace nav2_behavior_tree
+{
+
+class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
+{
+public:
+  ReinitializeGlobalLocalizationService(
+    const std::string & service_node_name,
+    const BT::NodeConfiguration & conf);
+};
+
+}  // namespace nav2_behavior_tree
+
+#endif  // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_

nav2_behavior_tree/plugins/action/clear_costmap_service.cpp

@@ -12,34 +12,25 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_
-#define NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_
-
 #include <string>
 #include <memory>
-#include <cmath>
 
-#include "nav2_behavior_tree/bt_service_node.hpp"
-#include "nav2_msgs/srv/clear_entire_costmap.hpp"
+#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
 
 namespace nav2_behavior_tree
 {
 
-class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
+ClearEntireCostmapService::ClearEntireCostmapService(
+  const std::string & service_node_name,
+  const BT::NodeConfiguration & conf)
+: BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, conf)
 {
-public:
-  ClearEntireCostmapService(
-    const std::string & service_node_name,
-    const BT::NodeConfiguration & conf)
-  : BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>(service_node_name, conf)
-  {
-  }
+}
 
-  void on_tick() override
-  {
-    increment_recovery_count();
-  }
-};
+void ClearEntireCostmapService::on_tick()
+{
+  increment_recovery_count();
+}
 
 }  // namespace nav2_behavior_tree
 
@@ -48,5 +39,3 @@ BT_REGISTER_NODES(factory)
 {
   factory.registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap");
 }
-
-#endif  // NAV2_BEHAVIOR_TREE__CLEAR_COSTMAP_SERVICE_HPP_

nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp

@@ -12,29 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
-#define NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
-
 #include <string>
-#include <memory>
-#include <cmath>
-
-#include "nav2_behavior_tree/bt_service_node.hpp"
-#include "std_srvs/srv/empty.hpp"
+#include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp"
 
 namespace nav2_behavior_tree
 {
 
-class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
-{
-public:
-  ReinitializeGlobalLocalizationService(
-    const std::string & service_node_name,
-    const BT::NodeConfiguration & conf)
-  : BtServiceNode<std_srvs::srv::Empty>(service_node_name, conf)
-  {
-  }
-};
+ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService(
+  const std::string & service_node_name,
+  const BT::NodeConfiguration & conf)
+: BtServiceNode<std_srvs::srv::Empty>(service_node_name, conf)
+{}
 
 }  // namespace nav2_behavior_tree
 
@@ -44,5 +32,3 @@ BT_REGISTER_NODES(factory)
   factory.registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(
     "ReinitializeGlobalLocalization");
 }
-
-#endif  // NAV2_BEHAVIOR_TREE__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -10,6 +10,14 @@ ament_add_gtest(test_action_wait_action test_wait_action.cpp)
 target_link_libraries(test_action_wait_action nav2_wait_action_bt_node)
 ament_target_dependencies(test_action_wait_action ${dependencies})
 
+ament_add_gtest(test_action_clear_costmap_service test_clear_costmap_service.cpp)
+target_link_libraries(test_action_clear_costmap_service nav2_clear_costmap_service_bt_node)
+ament_target_dependencies(test_action_clear_costmap_service ${dependencies})
+
+ament_add_gtest(test_action_reinitialize_global_localization_service test_reinitialize_global_localization_service.cpp)
+target_link_libraries(test_action_reinitialize_global_localization_service nav2_reinitialize_global_localization_service_bt_node)
+ament_target_dependencies(test_action_reinitialize_global_localization_service ${dependencies})
+
 ament_add_gtest(test_action_compute_path_to_pose_action test_compute_path_to_pose_action.cpp)
 target_link_libraries(test_action_compute_path_to_pose_action nav2_compute_path_to_pose_action_bt_node)
 ament_target_dependencies(test_action_compute_path_to_pose_action ${dependencies})

nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp

@@ -0,0 +1,130 @@
+// Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2020 Sarthak Mittal
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <set>
+#include <string>
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "../../test_service.hpp"
+#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
+
+class ClearEntireCostmapService : public TestService<nav2_msgs::srv::ClearEntireCostmap>
+{
+public:
+  ClearEntireCostmapService()
+  : TestService("clear_entire_costmap")
+  {}
+};
+
+class ClearEntireCostmapServiceTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = std::make_shared<rclcpp::Node>("clear_entire_costmap_test_fixture");
+    factory_ = std::make_shared<BT::BehaviorTreeFactory>();
+
+    config_ = new BT::NodeConfiguration();
+
+    // Create the blackboard that will be shared by all of the nodes in the tree
+    config_->blackboard = BT::Blackboard::create();
+    // Put items on the blackboard
+    config_->blackboard->set<rclcpp::Node::SharedPtr>(
+      "node",
+      node_);
+    config_->blackboard->set<std::chrono::milliseconds>(
+      "server_timeout",
+      std::chrono::milliseconds(10));
+    config_->blackboard->set<bool>("path_updated", false);
+    config_->blackboard->set<bool>("initial_pose_received", false);
+    config_->blackboard->set<int>("number_recoveries", 0);
+
+    factory_->registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>("ClearEntireCostmap");
+  }
+
+  static void TearDownTestCase()
+  {
+    delete config_;
+    config_ = nullptr;
+    node_.reset();
+    server_.reset();
+    factory_.reset();
+  }
+
+  void SetUp() override
+  {
+    config_->blackboard->set("number_recoveries", 0);
+  }
+
+  void TearDown() override
+  {
+    tree_.reset();
+  }
+
+  static std::shared_ptr<ClearEntireCostmapService> server_;
+
+protected:
+  static rclcpp::Node::SharedPtr node_;
+  static BT::NodeConfiguration * config_;
+  static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
+  static std::shared_ptr<BT::Tree> tree_;
+};
+
+rclcpp::Node::SharedPtr ClearEntireCostmapServiceTestFixture::node_ = nullptr;
+std::shared_ptr<ClearEntireCostmapService> ClearEntireCostmapServiceTestFixture::server_ = nullptr;
+BT::NodeConfiguration * ClearEntireCostmapServiceTestFixture::config_ = nullptr;
+std::shared_ptr<BT::BehaviorTreeFactory> ClearEntireCostmapServiceTestFixture::factory_ = nullptr;
+std::shared_ptr<BT::Tree> ClearEntireCostmapServiceTestFixture::tree_ = nullptr;
+
+TEST_F(ClearEntireCostmapServiceTestFixture, test_tick)
+{
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <ClearEntireCostmap service_name="clear_entire_costmap"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 0);
+  EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(config_->blackboard->get<int>("number_recoveries"), 1);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+
+  // initialize ROS
+  rclcpp::init(argc, argv);
+
+  // initialize service and spin on new thread
+  ClearEntireCostmapServiceTestFixture::server_ = std::make_shared<ClearEntireCostmapService>();
+  std::thread server_thread([]() {
+      rclcpp::spin(ClearEntireCostmapServiceTestFixture::server_);
+    });
+
+  int all_successful = RUN_ALL_TESTS();
+
+  // shutdown ROS
+  rclcpp::shutdown();
+  server_thread.join();
+
+  return all_successful;
+}