Skip to content

Commit

Permalink
Allow action servers without execute callback
Browse files Browse the repository at this point in the history
  • Loading branch information
Timple committed Feb 13, 2024
1 parent 06d78fb commit d6f14c9
Show file tree
Hide file tree
Showing 2 changed files with 67 additions and 4 deletions.
21 changes: 18 additions & 3 deletions rclpy/rclpy/action/server.py
Original file line number Diff line number Diff line change
Expand Up @@ -146,9 +146,21 @@ def publish_feedback(self, feedback):
# Publish
self._action_server._handle.publish_feedback(feedback_message)

def succeed(self):
def executing(self):
self._update_state(_rclpy.GoalEvent.EXECUTE)

def succeed(self, response=None):
self._update_state(_rclpy.GoalEvent.SUCCEED)

# Set result
result_response = self._action_server._action_type.Impl.GetResultService.Response()
result_response.status = self.status
if response is not None:
result_response.result = response
else:
result_response.result = self._action_server._action_type.Result()
self._action_server._result_futures[bytes(self.goal_id.uuid)].set_result(result_response)

def abort(self):
self._update_state(_rclpy.GoalEvent.ABORT)

Expand Down Expand Up @@ -186,7 +198,7 @@ def __init__(
node,
action_type,
action_name,
execute_callback,
execute_callback=None,
*,
callback_group=None,
goal_callback=default_goal_callback,
Expand Down Expand Up @@ -233,7 +245,10 @@ def __init__(
self.register_handle_accepted_callback(handle_accepted_callback)
self.register_goal_callback(goal_callback)
self.register_cancel_callback(cancel_callback)
self.register_execute_callback(execute_callback)
if execute_callback:
self.register_execute_callback(execute_callback)
elif handle_accepted_callback is default_handle_accepted_callback:
self._logger.warning("Not handling nor executing the goal, this server will do nothing")

# Import the typesupport for the action module if not already done
check_for_type_support(action_type)
Expand Down
50 changes: 49 additions & 1 deletion rclpy/test/test_action_server.py
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

import rclpy
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.action.server import ServerGoalHandle
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor

Expand Down Expand Up @@ -419,7 +420,7 @@ def execute_callback(gh):
# Execute the goal
server_goal_handle.execute()

# Get the result and exepect it to have canceled status
# Get the result and expect it to have canceled status
get_result_future = self.mock_action_client.get_result(goal_uuid)
rclpy.spin_until_future_complete(self.node, get_result_future, self.executor)
result = get_result_future.result()
Expand Down Expand Up @@ -684,6 +685,53 @@ def execute_with_feedback(goal_handle):
finally:
action_server.destroy()

def test_without_execute_cb(self):
# Just like test_execute_succeed, but without an execute callback
# Goal handle is stored and succeeded from outside the execute callback
stored_goal_handle = None

def handle_accepted_callback(goal_handle: ServerGoalHandle):
goal_handle.executing()
nonlocal stored_goal_handle
stored_goal_handle = goal_handle

executor = MultiThreadedExecutor(context=self.context)

action_server = ActionServer(
self.node,
Fibonacci,
'fibonacci',
handle_accepted_callback=handle_accepted_callback,
)

goal_uuid = UUID(uuid=list(uuid.uuid4().bytes))
goal_msg = Fibonacci.Impl.SendGoalService.Request()
goal_msg.goal_id = goal_uuid
goal_future = self.mock_action_client.send_goal(goal_msg)
rclpy.spin_until_future_complete(self.node, goal_future, executor)
goal_handle = goal_future.result()
self.assertTrue(goal_handle.accepted)

get_result_future = self.mock_action_client.get_result(goal_uuid)
self.assertFalse(get_result_future.done())

result = Fibonacci.Result()
result.sequence.extend([1, 1, 2, 3, 5])
stored_goal_handle.succeed(result)

# Handle all callbacks
for _ in range(5):
rclpy.spin_once(self.node, executor=self.executor, timeout_sec=0.01)
if get_result_future.done():
break
self.assertTrue(get_result_future.done())

result_response = get_result_future.result()

self.assertEqual(result_response.status, GoalStatus.STATUS_SUCCEEDED)
self.assertEqual(result_response.result.sequence.tolist(), [1, 1, 2, 3, 5])
action_server.destroy()


if __name__ == '__main__':
unittest.main()

0 comments on commit d6f14c9

Please sign in to comment.