added dummy_Server

Author: BCKSELFDRIVEWORLD

nav2_behavior_tree/test/utils/test_service.hpp

@@ -48,11 +48,22 @@ class TestService : public rclcpp::Node
     const std::shared_ptr<typename ServiceT::Response> response)
   {
     (void)request_header;
-    (void)response;
     current_request_ = request;
+    setSuccessIfExists(response);
   }
 
 private:
+  template<typename T>
+  auto setSuccessIfExists(std::shared_ptr<T> response) -> decltype(response->success, void())
+  {
+    response->success = true;
+  }
+  
+  template<typename T>
+  void setSuccessIfExists(T)
+  {
+  }
+
   typename rclcpp::Service<ServiceT>::SharedPtr server_;
   std::shared_ptr<typename ServiceT::Request> current_request_;
 };

nav2_system_tests/src/behavior_tree/server_handler.cpp

@@ -31,9 +31,9 @@ ServerHandler::ServerHandler()
 {
   node_ = rclcpp::Node::make_shared("behavior_tree_tester");
 
-  clear_local_costmap_server = std::make_unique<DummyService<nav2_msgs::srv::ClearEntireCostmap>>(
+  clear_local_costmap_server = std::make_unique<DummyClearCostmapService>(
     node_, "local_costmap/clear_entirely_local_costmap");
-  clear_global_costmap_server = std::make_unique<DummyService<nav2_msgs::srv::ClearEntireCostmap>>(
+  clear_global_costmap_server = std::make_unique<DummyClearCostmapService>(
     node_, "global_costmap/clear_entirely_global_costmap");
   compute_path_to_pose_server = std::make_unique<DummyComputePathToPoseActionServer>(node_);
   follow_path_server = std::make_unique<DummyFollowPathActionServer>(node_);

nav2_system_tests/src/behavior_tree/server_handler.hpp

@@ -95,6 +95,22 @@ class DummyFollowPathActionServer : public DummyActionServer<nav2_msgs::action::
   }
 };
 
+class DummyClearCostmapService : public DummyService<nav2_msgs::srv::ClearEntireCostmap>
+{
+public:
+  explicit DummyClearCostmapService(
+    const rclcpp::Node::SharedPtr & node,
+    std::string service_name)
+  : DummyService(node, service_name) {}
+
+protected:
+  void fillResponse(
+    const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Request>/*request*/,
+    const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Response> response) override
+  {
+    response->success = true;
+  }
+};
 
 class ServerHandler
 {
@@ -114,8 +130,8 @@ class ServerHandler
   void reset() const;
 
 public:
-  std::unique_ptr<DummyService<nav2_msgs::srv::ClearEntireCostmap>> clear_local_costmap_server;
-  std::unique_ptr<DummyService<nav2_msgs::srv::ClearEntireCostmap>> clear_global_costmap_server;
+  std::unique_ptr<DummyClearCostmapService> clear_local_costmap_server;
+  std::unique_ptr<DummyClearCostmapService> clear_global_costmap_server;
   std::unique_ptr<DummyComputePathToPoseActionServer> compute_path_to_pose_server;
   std::unique_ptr<DummyFollowPathActionServer> follow_path_server;
   std::unique_ptr<DummyActionServer<nav2_msgs::action::Spin>> spin_server;