Don't loop forever on registering a nonexistent service; also each service call is in its own thread.

Author: LaurieCheers-unity

ros_tcp_endpoint/client.py

@@ -158,18 +158,22 @@ def send_ros_service_request(self, srv_id, destination, data):
             # TODO: send a response to Unity anyway?
             return
         else:
-            ros_communicator = self.tcp_server.source_destination_dict[
+            ros_communicator = self.tcp_server.source_destination_dict[destination]
+            service_thread = threading.Thread(target=self.service_call_thread, args=(srv_id, destination, data, ros_communicator))
+            service_thread.daemon = True
+            service_thread.start()
+
+    def service_call_thread(self, srv_id, destination, data, ros_communicator):
+        response = ros_communicator.send(data)
+        
+        if not response:
+            error_msg = "No response data from service '{}'!".format(
                 destination
-            ]
-            response = ros_communicator.send(data)
-            if not response:
-                error_msg = "No response data from service '{}'!".format(
-                    destination
-                )
-                self.tcp_server.send_unity_error(error_msg)
-                self.logerr(error_msg)
-                # TODO: send a response to Unity anyway?
-                return
+            )
+            self.tcp_server.send_unity_error(error_msg)
+            self.logerr(error_msg)
+            # TODO: send a response to Unity anyway?
+            return
 
         self.tcp_server.unity_tcp_sender.send_ros_service_response(srv_id, destination, response)
 

ros_tcp_endpoint/service.py

@@ -34,9 +34,8 @@ def __init__(self, service, service_class):
         node_name = f'{strippedService}_RosService'
         RosSender.__init__(self, node_name)
 
+        self.service_topic = service
         self.cli = self.create_client(service_class, service)
-        while not self.cli.wait_for_service(timeout_sec=1.0):
-            self.get_logger().info(f'{service} not available, waiting again...')
         self.req = service_class.Request()
 
 
@@ -55,6 +54,10 @@ def send(self, data):
         message_type = type(self.req)
         message = deserialize_message(data, message_type)
 
+        if not self.cli.service_is_ready():
+            self.get_logger().error('Ignoring service call to {} - service is not ready.'.format(self.service_topic))
+            return None
+            
         self.future = self.cli.call_async(message)
 
         while rclpy.ok():