Skip to content

Convert ActionServer to use C++ Classes #766

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
ahcorde merged 5 commits into from
Apr 14, 2021
Merged

Convert ActionServer to use C++ Classes #766

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
310ede3
Convert ActionServer to use C++ Classes
ahcorde Apr 9, 2021
c726671
Fixed Test_action_server
ahcorde Apr 9, 2021
4ba0a50
Merge remote-tracking branch 'origin/post_galactic_freeze' into ahcor…
ahcorde Apr 12, 2021
64b7984
Improved docs
ahcorde Apr 12, 2021
f0d4ef4
Improved docs and minor fixes
ahcorde Apr 14, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions rclpy/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -155,6 +155,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED
src/rclpy/_rclpy_pycapsule.cpp
src/rclpy/action_client.cpp
src/rclpy/action_goal_handle.cpp
src/rclpy/action_server.cpp
src/rclpy/client.cpp
src/rclpy/clock.cpp
src/rclpy/context.cpp
Expand Down
62 changes: 19 additions & 43 deletions rclpy/rclpy/action/server.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,7 @@ def _update_state(self, event):
self._goal.update_goal_state(event)

# Publish state change
_rclpy.rclpy_action_publish_status(self._action_server._handle)
self._action_server._handle.publish_status()

# If it's a terminal state, then also notify the action server
if not self._goal.is_active():
Expand Down Expand Up @@ -140,8 +140,7 @@ def publish_feedback(self, feedback):
feedback_message.feedback = feedback

# Publish
_rclpy.rclpy_action_publish_feedback(
self._action_server._handle, feedback_message)
self._action_server._handle.publish_feedback(feedback_message)

def succeed(self):
self._update_state(_rclpy.GoalEvent.SUCCEED)
Expand Down Expand Up @@ -239,7 +238,7 @@ def __init__(
self._node = node
self._action_type = action_type
with node.handle as node_capsule, node.get_clock().handle:
self._handle = _rclpy.rclpy_action_create_server(
self._handle = _rclpy.ActionServer(
node_capsule,
node.get_clock().handle,
action_type,
Expand Down Expand Up @@ -268,7 +267,7 @@ async def _execute_goal_request(self, request_header_and_message):

# Check if goal ID is already being tracked by this action server
with self._lock:
goal_id_exists = _rclpy.rclpy_action_server_goal_exists(self._handle, goal_info)
goal_id_exists = self._handle.goal_exists(goal_info)

accepted = False
if not goal_id_exists:
Expand Down Expand Up @@ -299,11 +298,7 @@ async def _execute_goal_request(self, request_header_and_message):
response_msg = self._action_type.Impl.SendGoalService.Response()
response_msg.accepted = accepted
response_msg.stamp = goal_info.stamp
_rclpy.rclpy_action_send_goal_response(
self._handle,
request_header,
response_msg,
)
self._handle.send_goal_response(request_header, response_msg)

if not accepted:
self._node.get_logger().debug('New goal rejected: {0}'.format(goal_uuid.uuid))
Expand Down Expand Up @@ -349,8 +344,8 @@ async def _execute_cancel_request(self, request_header_and_message):

with self._lock:
# Get list of goals that are requested to be canceled
cancel_response = _rclpy.rclpy_action_process_cancel_request(
self._handle, cancel_request, self._action_type.Impl.CancelGoalService.Response)
cancel_response = self._handle.process_cancel_request(
cancel_request, self._action_type.Impl.CancelGoalService.Response)

for goal_info in cancel_response.goals_canceling:
goal_uuid = bytes(goal_info.goal_id.uuid)
Expand All @@ -369,11 +364,7 @@ async def _execute_cancel_request(self, request_header_and_message):
# Remove from response
cancel_response.goals_canceling.remove(goal_info)

_rclpy.rclpy_action_send_cancel_response(
self._handle,
request_header,
cancel_response,
)
self._handle.send_cancel_response(request_header, cancel_response)

async def _execute_get_result_request(self, request_header_and_message):
request_header, result_request = request_header_and_message
Expand All @@ -388,11 +379,7 @@ async def _execute_get_result_request(self, request_header_and_message):
'Sending result response for unknown goal ID: {0}'.format(goal_uuid))
result_response = self._action_type.Impl.GetResultService.Response()
result_response.status = GoalStatus.STATUS_UNKNOWN
_rclpy.rclpy_action_send_result_response(
self._handle,
request_header,
result_response,
)
self._handle.send_result_response(request_header, result_response)
return

# There is an accepted goal matching the goal ID, register a callback to send the
Expand All @@ -406,11 +393,7 @@ async def _execute_expire_goals(self, expired_goals):
del self._goal_handles[goal_uuid]

def _send_result_response(self, request_header, future):
_rclpy.rclpy_action_send_result_response(
self._handle,
request_header,
future.result(),
)
self._handle.send_result_response(request_header, future.result())

@property
def action_type(self):
Expand All @@ -420,7 +403,7 @@ def action_type(self):
def is_ready(self, wait_set):
"""Return True if one or more entities are ready in the wait set."""
with self._lock:
ready_entities = _rclpy.rclpy_action_wait_set_is_ready(self._handle, wait_set)
ready_entities = self._handle.is_ready(wait_set)
self._is_goal_request_ready = ready_entities[0]
self._is_cancel_request_ready = ready_entities[1]
self._is_result_request_ready = ready_entities[2]
Expand All @@ -432,8 +415,7 @@ def take_data(self):
data = {}
if self._is_goal_request_ready:
with self._lock:
taken_data = _rclpy.rclpy_action_take_goal_request(
self._handle,
taken_data = self._handle.take_goal_request(
self._action_type.Impl.SendGoalService.Request,
)
# If take fails, then we get (None, None)
Expand All @@ -442,8 +424,7 @@ def take_data(self):

if self._is_cancel_request_ready:
with self._lock:
taken_data = _rclpy.rclpy_action_take_cancel_request(
self._handle,
taken_data = self._handle.take_cancel_request(
self._action_type.Impl.CancelGoalService.Request,
)
# If take fails, then we get (None, None)
Expand All @@ -452,8 +433,7 @@ def take_data(self):

if self._is_result_request_ready:
with self._lock:
taken_data = _rclpy.rclpy_action_take_result_request(
self._handle,
taken_data = self._handle.take_result_request(
self._action_type.Impl.GetResultService.Request,
)
# If take fails, then we get (None, None)
Expand All @@ -462,10 +442,7 @@ def take_data(self):

if self._is_goal_expired:
with self._lock:
data['expired'] = _rclpy.rclpy_action_expire_goals(
self._handle,
len(self._goal_handles),
)
data['expired'] = self._handle.expire_goals(len(self._goal_handles))

return data

Expand All @@ -490,7 +467,7 @@ async def execute(self, taken_data):

def get_num_entities(self):
"""Return number of each type of entity used in the wait set."""
num_entities = _rclpy.rclpy_action_wait_set_get_num_entities(self._handle)
num_entities = self._handle.get_num_entities()
return NumberOfEntities(
num_entities[0],
num_entities[1],
Expand All @@ -501,7 +478,7 @@ def get_num_entities(self):
def add_to_wait_set(self, wait_set):
"""Add entities to wait set."""
with self._lock:
_rclpy.rclpy_action_wait_set_add(self._handle, wait_set)
self._handle.add_to_waitset(wait_set)
# End Waitable API

def notify_execute(self, goal_handle, execute_callback):
Expand All @@ -516,7 +493,7 @@ def notify_execute(self, goal_handle, execute_callback):

def notify_goal_done(self):
with self._lock:
_rclpy.rclpy_action_notify_goal_done(self._handle)
self._handle.notify_goal_done()

def register_handle_accepted_callback(self, handle_accepted_callback):
"""
Expand Down Expand Up @@ -604,8 +581,7 @@ def destroy(self):
for goal_handle in self._goal_handles.values():
goal_handle.destroy()

with self._node.handle as node_capsule:
_rclpy.rclpy_action_destroy_entity(self._handle, node_capsule)
self._handle.destroy_when_not_in_use()
self._node.remove_waitable(self)
self._handle = None

Expand Down
Loading