ros2action: add SIGINT handler to manage cancel request. (#956)

Signed-off-by: Tomoya Fujita <[email protected]>

ros2action/ros2action/verb/send_goal.py

@@ -12,6 +12,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import signal
+
 from action_msgs.msg import GoalStatus
 import rclpy
 from rclpy.action import ActionClient
@@ -121,6 +123,32 @@ def send_goal(action_name, action_type, goal_values, feedback_callback):
 
         goal_handle = goal_future.result()
 
+        # install signal handler to cancel the goal on SIGINT
+        def _sigint_cancel_handler(sig, frame):
+            nonlocal goal_handle
+            # Cancel the goal if it's still active
+            if (goal_handle is not None and
+                (GoalStatus.STATUS_ACCEPTED == goal_handle.status or
+                 GoalStatus.STATUS_EXECUTING == goal_handle.status)):
+                print('Canceling goal...')
+                cancel_future = goal_handle.cancel_goal_async()
+                rclpy.spin_until_future_complete(node, cancel_future)
+
+                cancel_response = cancel_future.result()
+
+                if cancel_response is None:
+                    raise RuntimeError(
+                        'Exception while canceling goal: {!r}'.format(cancel_future.exception()))
+
+                if len(cancel_response.goals_canceling) == 0:
+                    raise RuntimeError('Failed to cancel goal')
+                if len(cancel_response.goals_canceling) > 1:
+                    raise RuntimeError('More than one goal canceled')
+                if cancel_response.goals_canceling[0].goal_id != goal_handle.goal_id:
+                    raise RuntimeError('Canceled goal with incorrect goal ID')
+                print('Goal canceled.')
+        signal.signal(signal.SIGINT, _sigint_cancel_handler)
+
         if goal_handle is None:
             raise RuntimeError(
                 'Exception while sending goal: {!r}'.format(goal_future.exception()))
@@ -148,28 +176,6 @@ def send_goal(action_name, action_type, goal_values, feedback_callback):
         print('Result:\n    {}'.format(message_to_yaml(result.result)))
         print('Goal finished with status: {}'.format(_goal_status_to_string(result.status)))
     finally:
-        # Cancel the goal if it's still active
-        if (goal_handle is not None and
-            (GoalStatus.STATUS_ACCEPTED == goal_handle.status or
-             GoalStatus.STATUS_EXECUTING == goal_handle.status)):
-            print('Canceling goal...')
-            cancel_future = goal_handle.cancel_goal_async()
-            rclpy.spin_until_future_complete(node, cancel_future)
-
-            cancel_response = cancel_future.result()
-
-            if cancel_response is None:
-                raise RuntimeError(
-                    'Exception while canceling goal: {!r}'.format(cancel_future.exception()))
-
-            if len(cancel_response.goals_canceling) == 0:
-                raise RuntimeError('Failed to cancel goal')
-            if len(cancel_response.goals_canceling) > 1:
-                raise RuntimeError('More than one goal canceled')
-            if cancel_response.goals_canceling[0].goal_id != goal_handle.goal_id:
-                raise RuntimeError('Canceled goal with incorrect goal ID')
-            print('Goal canceled.')
-
         if action_client is not None:
             action_client.destroy()
         if node is not None: