Skip to content
Merged
Changes from all commits
Commits
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
27 changes: 15 additions & 12 deletions rclpy/rclpy/action/server.py
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,9 @@ class CancelResponse(Enum):
ACCEPT = 2


GoalEvent = _rclpy.GoalEvent


class ServerGoalHandle:
"""Goal handle for working with Action Servers."""

Expand All @@ -59,7 +62,7 @@ def __init__(self, action_server, goal_info, goal_request):
:param goal_info: GoalInfo message.
:param goal_request: The user defined goal request message from an ActionClient.
"""
self._goal = _rclpy.ActionGoalHandle(action_server._handle, goal_info)
self._goal_handle = _rclpy.ActionGoalHandle(action_server._handle, goal_info)
self._action_server = action_server
self._goal_info = goal_info
self._goal_request = goal_request
Expand All @@ -85,9 +88,9 @@ def goal_id(self):
@property
def is_active(self):
with self._lock:
if self._goal is None:
if self._goal_handle is None:
return False
return self._goal.is_active()
return self._goal_handle.is_active()

@property
def is_cancel_requested(self):
Expand All @@ -96,24 +99,24 @@ def is_cancel_requested(self):
@property
def status(self):
with self._lock:
if self._goal is None:
if self._goal_handle is None:
return GoalStatus.STATUS_UNKNOWN
return self._goal.get_status()
return self._goal_handle.get_status()

def _update_state(self, event):
with self._lock:
# Ignore updates for already destructed goal handles
if self._goal is None:
if self._goal_handle is None:
return

# Update state
self._goal.update_goal_state(event)
self._goal_handle.update_goal_state(event)

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

# If it's a terminal state, then also notify the action server
if not self._goal.is_active():
if not self._goal_handle.is_active():
self._action_server.notify_goal_done()

def execute(self, execute_callback=None):
Expand All @@ -130,7 +133,7 @@ def publish_feedback(self, feedback):

with self._lock:
# Ignore for already destructed goal handles
if self._goal is None:
if self._goal_handle is None:
return

# Populate the feedback message with metadata about this goal
Expand All @@ -153,10 +156,10 @@ def canceled(self):

def destroy(self):
with self._lock:
if self._goal is None:
if self._goal_handle is None:
return
self._goal.destroy_when_not_in_use()
self._goal = None
self._goal_handle.destroy_when_not_in_use()
self._goal_handle = None

self._action_server.remove_future(self._result_future)

Expand Down