ServiceClient use callback group (ros-navigation#2216)

* change timeout default value

* create and use callback group

* remove generate_internal_node option

* unit test to do a service call in a topic callback

* fix unit test

* set default value to -1

nav2_util/include/nav2_util/lifecycle_service_client.hpp

@@ -22,6 +22,7 @@
 #include "lifecycle_msgs/srv/change_state.hpp"
 #include "lifecycle_msgs/srv/get_state.hpp"
 #include "nav2_util/service_client.hpp"
+#include "nav2_util/node_utils.hpp"
 
 namespace nav2_util
 {

nav2_util/include/nav2_util/service_client.hpp

@@ -17,7 +17,6 @@
 
 #include <string>
 
-#include "nav2_util/node_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 namespace nav2_util
@@ -29,30 +28,25 @@ class ServiceClient
 public:
   explicit ServiceClient(
     const std::string & service_name,
-    const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr())
-  : service_name_(service_name)
+    const rclcpp::Node::SharedPtr & provided_node)
+  : service_name_(service_name), node_(provided_node)
   {
-    if (provided_node) {
-      node_ = provided_node;
-    } else {
-      node_ = generate_internal_node(service_name + "_Node");
-    }
-    client_ = node_->create_client<ServiceT>(service_name);
-  }
-
-  ServiceClient(const std::string & service_name, const std::string & parent_name)
-  : service_name_(service_name)
-  {
-    node_ = generate_internal_node(parent_name + std::string("_") + service_name + "_client");
-    client_ = node_->create_client<ServiceT>(service_name);
+    callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      false);
+    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+    client_ = node_->create_client<ServiceT>(
+      service_name,
+      rmw_qos_profile_services_default,
+      callback_group_);
   }
 
   using RequestType = typename ServiceT::Request;
   using ResponseType = typename ServiceT::Response;
 
   typename ResponseType::SharedPtr invoke(
     typename RequestType::SharedPtr & request,
-    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max())
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1))
   {
     while (!client_->wait_for_service(std::chrono::seconds(1))) {
       if (!rclcpp::ok()) {
@@ -69,7 +63,7 @@ class ServiceClient
       service_name_.c_str());
     auto future_result = client_->async_send_request(request);
 
-    if (rclcpp::spin_until_future_complete(node_, future_result, timeout) !=
+    if (callback_group_executor_.spin_until_future_complete(future_result, timeout) !=
       rclcpp::FutureReturnCode::SUCCESS)
     {
       throw std::runtime_error(service_name_ + " service client: async_send_request failed");
@@ -97,7 +91,7 @@ class ServiceClient
       service_name_.c_str());
     auto future_result = client_->async_send_request(request);
 
-    if (rclcpp::spin_until_future_complete(node_, future_result) !=
+    if (callback_group_executor_.spin_until_future_complete(future_result) !=
       rclcpp::FutureReturnCode::SUCCESS)
     {
       return false;
@@ -122,6 +116,8 @@ class ServiceClient
 protected:
   std::string service_name_;
   rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
   typename rclcpp::Client<ServiceT>::SharedPtr client_;
 };
 

nav2_util/test/test_service_client.cpp

@@ -12,10 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <memory>
 #include <string>
 #include "nav2_util/service_client.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "std_srvs/srv/empty.hpp"
+#include "std_msgs/msg/empty.hpp"
 #include "gtest/gtest.h"
 
 using nav2_util::ServiceClient;
@@ -41,18 +43,46 @@ class TestServiceClient : public ServiceClient<std_srvs::srv::Empty>
   const rclcpp::Node::SharedPtr & getNode() {return node_;}
 };
 
-TEST(ServiceClient, are_non_alphanumerics_removed)
-{
-  TestServiceClient t("/foo/bar");
-  string adjustedPrefix = "_foo_bar_Node_";
-  ASSERT_EQ(t.name().length(), adjustedPrefix.length() + 8);
-  ASSERT_EQ(0, t.name().compare(0, adjustedPrefix.length(), adjustedPrefix));
-}
-
 TEST(ServiceClient, can_ServiceClient_use_passed_in_node)
 {
   auto node = rclcpp::Node::make_shared("test_node");
   TestServiceClient t("bar", node);
   ASSERT_EQ(t.getNode(), node);
   ASSERT_EQ(t.name(), "test_node");
 }
+
+TEST(ServiceClient, can_ServiceClient_invoke_in_callback)
+{
+  int a = 0;
+  auto service_node = rclcpp::Node::make_shared("service_node");
+  auto service = service_node->create_service<std_srvs::srv::Empty>(
+    "empty_srv",
+    [&a](std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr) {
+      a = 1;
+    });
+  auto srv_thread = std::thread([&]() {rclcpp::spin(service_node);});
+
+  auto pub_node = rclcpp::Node::make_shared("pub_node");
+  auto pub = pub_node->create_publisher<std_msgs::msg::Empty>(
+    "empty_topic",
+    rclcpp::QoS(1).transient_local());
+  auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node);});
+
+  auto sub_node = rclcpp::Node::make_shared("sub_node");
+  ServiceClient<std_srvs::srv::Empty> client("empty_srv", sub_node);
+  auto sub = sub_node->create_subscription<std_msgs::msg::Empty>(
+    "empty_topic",
+    rclcpp::QoS(1),
+    [&client](std_msgs::msg::Empty::SharedPtr) {
+      auto req = std::make_shared<std_srvs::srv::Empty::Request>();
+      auto res = client.invoke(req);
+    });
+
+  pub->publish(std_msgs::msg::Empty());
+  rclcpp::spin_some(sub_node);
+
+  rclcpp::shutdown();
+  srv_thread.join();
+  pub_thread.join();
+  ASSERT_EQ(a, 1);
+}