Skip to content
Closed
Show file tree
Hide file tree
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
5 changes: 2 additions & 3 deletions rclpy/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -148,13 +148,11 @@ install(TARGETS rclpy_common

# Split from main extension and converted to pybind11
pybind11_add_module(_rclpy_pybind11 SHARED
src/rclpy/_rclpy_action.cpp
src/rclpy/_rclpy_handle.cpp
src/rclpy/_rclpy_logging.cpp
src/rclpy/_rclpy_pybind11.cpp
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 All @@ -163,6 +161,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED
src/rclpy/graph.cpp
src/rclpy/guard_condition.cpp
src/rclpy/handle.cpp
src/rclpy/init.cpp
src/rclpy/logging.cpp
src/rclpy/names.cpp
src/rclpy/node.cpp
Expand Down
55 changes: 32 additions & 23 deletions rclpy/rclpy/action/client.py
Original file line number Diff line number Diff line change
Expand Up @@ -149,9 +149,9 @@ def __init__(
self._node = node
self._action_type = action_type
self._action_name = action_name
with node.handle:
self._client_handle = _rclpy.ActionClient(
node.handle,
with node.handle as node_capsule:
self._client_handle = _rclpy.rclpy_action_create_client(
node_capsule,
action_type,
action_name,
goal_service_qos_profile.get_c_qos_profile(),
Expand Down Expand Up @@ -220,7 +220,9 @@ def _remove_pending_result_request(self, future):
# Start Waitable API
def is_ready(self, wait_set):
"""Return True if one or more entities are ready in the wait set."""
ready_entities = self._client_handle.is_ready(wait_set)
ready_entities = _rclpy.rclpy_action_wait_set_is_ready(
self._client_handle,
wait_set)
self._is_feedback_ready = ready_entities[0]
self._is_status_ready = ready_entities[1]
self._is_goal_response_ready = ready_entities[2]
Expand All @@ -232,36 +234,36 @@ def take_data(self):
"""Take stuff from lower level so the wait set doesn't immediately wake again."""
data = {}
if self._is_goal_response_ready:
taken_data = self._client_handle.take_goal_response(
self._action_type.Impl.SendGoalService.Response)
taken_data = _rclpy.rclpy_action_take_goal_response(
self._client_handle, self._action_type.Impl.SendGoalService.Response)
# If take fails, then we get (None, None)
if all(taken_data):
data['goal'] = taken_data

if self._is_cancel_response_ready:
taken_data = self._client_handle.take_cancel_response(
self._action_type.Impl.CancelGoalService.Response)
taken_data = _rclpy.rclpy_action_take_cancel_response(
self._client_handle, self._action_type.Impl.CancelGoalService.Response)
# If take fails, then we get (None, None)
if all(taken_data):
data['cancel'] = taken_data

if self._is_result_response_ready:
taken_data = self._client_handle.take_result_response(
self._action_type.Impl.GetResultService.Response)
taken_data = _rclpy.rclpy_action_take_result_response(
self._client_handle, self._action_type.Impl.GetResultService.Response)
# If take fails, then we get (None, None)
if all(taken_data):
data['result'] = taken_data

if self._is_feedback_ready:
taken_data = self._client_handle.take_feedback(
self._action_type.Impl.FeedbackMessage)
taken_data = _rclpy.rclpy_action_take_feedback(
self._client_handle, self._action_type.Impl.FeedbackMessage)
# If take fails, then we get None
if taken_data is not None:
data['feedback'] = taken_data

if self._is_status_ready:
taken_data = self._client_handle.take_status(
self._action_type.Impl.GoalStatusMessage)
taken_data = _rclpy.rclpy_action_take_status(
self._client_handle, self._action_type.Impl.GoalStatusMessage)
# If take fails, then we get None
if taken_data is not None:
data['status'] = taken_data
Expand Down Expand Up @@ -345,12 +347,12 @@ 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 = self._client_handle.get_num_entities()
num_entities = _rclpy.rclpy_action_wait_set_get_num_entities(self._client_handle)
return NumberOfEntities(*num_entities)

def add_to_wait_set(self, wait_set):
"""Add entities to wait set."""
self._client_handle.add_to_waitset(wait_set)
_rclpy.rclpy_action_wait_set_add(self._client_handle, wait_set)
# End Waitable API

def send_goal(self, goal, **kwargs):
Expand Down Expand Up @@ -421,7 +423,8 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None):
request = self._action_type.Impl.SendGoalService.Request()
request.goal_id = self._generate_random_uuid() if goal_uuid is None else goal_uuid
request.goal = goal
sequence_number = self._client_handle.send_goal_request(request)
sequence_number = _rclpy.rclpy_action_send_goal_request(
self._client_handle, request)
if sequence_number in self._pending_goal_requests:
raise RuntimeError(
'Sequence ({}) conflicts with pending goal request'.format(sequence_number))
Expand Down Expand Up @@ -479,7 +482,9 @@ def _cancel_goal_async(self, goal_handle):

cancel_request = CancelGoal.Request()
cancel_request.goal_info.goal_id = goal_handle.goal_id
sequence_number = self._client_handle.send_cancel_request(cancel_request)
sequence_number = _rclpy.rclpy_action_send_cancel_request(
self._client_handle,
cancel_request)
if sequence_number in self._pending_cancel_requests:
raise RuntimeError(
'Sequence ({}) conflicts with pending cancel request'.format(sequence_number))
Expand Down Expand Up @@ -531,7 +536,9 @@ def _get_result_async(self, goal_handle):

result_request = self._action_type.Impl.GetResultService.Request()
result_request.goal_id = goal_handle.goal_id
sequence_number = self._client_handle.send_result_request(result_request)
sequence_number = _rclpy.rclpy_action_send_result_request(
self._client_handle,
result_request)
if sequence_number in self._pending_result_requests:
raise RuntimeError(
'Sequence ({}) conflicts with pending result request'.format(sequence_number))
Expand All @@ -550,8 +557,10 @@ def server_is_ready(self):

:return: True if an action server is ready, False otherwise.
"""
with self._node.handle:
return self._client_handle.is_action_server_available()
with self._node.handle as node_capsule:
return _rclpy.rclpy_action_server_is_available(
node_capsule,
self._client_handle)

def wait_for_server(self, timeout_sec=None):
"""
Expand All @@ -578,8 +587,8 @@ def destroy(self):
"""Destroy the underlying action client handle."""
if self._client_handle is None:
return
with self._node.handle:
self._client_handle.destroy_when_not_in_use()
with self._node.handle as node_capsule:
_rclpy.rclpy_action_destroy_entity(self._client_handle, node_capsule)
self._node.remove_waitable(self)
self._client_handle = None

Expand Down
17 changes: 9 additions & 8 deletions rclpy/rclpy/action/graph.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
from typing import List
from typing import Tuple

from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
from rclpy.node import Node


Expand All @@ -33,9 +34,9 @@ def get_action_client_names_and_types_by_node(
The first element of each tuple is the action name and the second element is a list of
action types.
"""
with node.handle:
return node.handle.get_action_client_names_and_types_by_node(
remote_node_name, remote_node_namespace)
with node.handle as node_capsule:
return _rclpy.rclpy_action_get_client_names_and_types_by_node(
node_capsule, remote_node_name, remote_node_namespace)


def get_action_server_names_and_types_by_node(
Expand All @@ -53,9 +54,9 @@ def get_action_server_names_and_types_by_node(
The first element of each tuple is the action name and the second element is a list of
action types.
"""
with node.handle:
return node.handle.get_action_server_names_and_types_by_node(
remote_node_name, remote_node_namespace)
with node.handle as node_capsule:
return _rclpy.rclpy_action_get_server_names_and_types_by_node(
node_capsule, remote_node_name, remote_node_namespace)


def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]:
Expand All @@ -67,5 +68,5 @@ def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]:
The first element of each tuple is the action name and the second element is a list of
action types.
"""
with node.handle:
return node.handle.get_action_names_and_types()
with node.handle as node_capsule:
return _rclpy.rclpy_action_get_names_and_types(node_capsule)
Loading