Wait for service and improve cancellation logic

RobotWebTools · Nov 1, 2023 · 76debda · 76debda
1 parent 8dd96ea
commit 76debda
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 13 deletions.
diff --git a/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py b/rosbridge_library/src/rosbridge_library/capabilities/send_action_goal.py
@@ -143,16 +143,6 @@ def cancel_action_goal(self, message):
             if client_handler.send_goal_helper is not None:
                 client_handler.send_goal_helper.cancel_goal()
 
-                outgoing_message = {
-                    "op": "action_result",
-                    "action": action,
-                    "result": False,
-                }
-                if cid is not None:
-                    outgoing_message["id"] = cid
-                # TODO: fragmentation, compression
-                self.protocol.send(outgoing_message)
-
     def _success(self, cid, action, fragment_size, compression, message):
         outgoing_message = {
             "op": "action_result",

diff --git a/rosbridge_library/src/rosbridge_library/internal/actions.py b/rosbridge_library/src/rosbridge_library/internal/actions.py
@@ -128,9 +128,11 @@ def args_to_action_goal_instance(action, inst, args):
 class SendGoal:
     """Helper class to send action goals."""
 
-    def __init__(self, sleep_time=0.001):
+    def __init__(self, server_timeout_time=1.0, sleep_time=0.001):
+        self.server_timeout_time = server_timeout_time
         self.sleep_time = sleep_time
         self.goal_handle = None
+        self.goal_canceled = False
 
     def get_result_cb(self, future):
         self.result = future.result()
@@ -153,10 +155,11 @@ def send_goal(self, node_handle, action, action_type, args=None, feedback_cb=Non
 
         self.result = None
         client = ActionClient(node_handle, action_class, action_name)
+        client.wait_for_server(timeout_sec=self.server_timeout_time)
         send_goal_future = client.send_goal_async(inst, feedback_callback=feedback_cb)
         send_goal_future.add_done_callback(self.goal_response_cb)
 
-        while self.result is None:
+        while self.result is None and not self.goal_canceled:
             time.sleep(self.sleep_time)
 
         client.destroy()
@@ -175,3 +178,4 @@ def cancel_goal(self):
         cancel_goal_future = self.goal_handle.cancel_goal_async()
         while not cancel_goal_future.done():
             time.sleep(self.sleep_time)
+        self.goal_canceled = True
diff --git a/rosbridge_library/src/rosbridge_library/internal/services.py b/rosbridge_library/src/rosbridge_library/internal/services.py
@@ -101,7 +101,7 @@ def args_to_service_request_instance(service, inst, args):
     populate_instance(msg, inst)
 
 
-def call_service(node_handle, service, args=None, sleep_time=0.001):
+def call_service(node_handle, service, args=None, server_timeout_time=1.0, sleep_time=0.001):
     # Given the service name, fetch the type and class of the service,
     # and a request instance
     service = expand_topic_name(service, node_handle.get_name(), node_handle.get_namespace())
@@ -124,6 +124,7 @@ def call_service(node_handle, service, args=None, sleep_time=0.001):
     client = node_handle.create_client(
         service_class, service, callback_group=ReentrantCallbackGroup()
     )
+    client.wait_for_service(server_timeout_time)
 
     future = client.call_async(inst)
     while rclpy.ok() and not future.done():