diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 9d0379dec..1481e6a05 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -148,11 +148,13 @@ 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 @@ -161,7 +163,6 @@ 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 diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py index 0532ea24b..d476dc131 100644 --- a/rclpy/rclpy/action/client.py +++ b/rclpy/rclpy/action/client.py @@ -149,9 +149,9 @@ def __init__( self._node = node self._action_type = action_type self._action_name = action_name - with node.handle as node_capsule: - self._client_handle = _rclpy.rclpy_action_create_client( - node_capsule, + with node.handle: + self._client_handle = _rclpy.ActionClient( + node.handle, action_type, action_name, goal_service_qos_profile.get_c_qos_profile(), @@ -220,9 +220,7 @@ 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 = _rclpy.rclpy_action_wait_set_is_ready( - self._client_handle, - wait_set) + ready_entities = self._client_handle.is_ready(wait_set) self._is_feedback_ready = ready_entities[0] self._is_status_ready = ready_entities[1] self._is_goal_response_ready = ready_entities[2] @@ -234,36 +232,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 = _rclpy.rclpy_action_take_goal_response( - self._client_handle, self._action_type.Impl.SendGoalService.Response) + taken_data = self._client_handle.take_goal_response( + 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 = _rclpy.rclpy_action_take_cancel_response( - self._client_handle, self._action_type.Impl.CancelGoalService.Response) + taken_data = self._client_handle.take_cancel_response( + 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 = _rclpy.rclpy_action_take_result_response( - self._client_handle, self._action_type.Impl.GetResultService.Response) + taken_data = self._client_handle.take_result_response( + 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 = _rclpy.rclpy_action_take_feedback( - self._client_handle, self._action_type.Impl.FeedbackMessage) + taken_data = self._client_handle.take_feedback( + 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 = _rclpy.rclpy_action_take_status( - self._client_handle, self._action_type.Impl.GoalStatusMessage) + taken_data = self._client_handle.take_status( + self._action_type.Impl.GoalStatusMessage) # If take fails, then we get None if taken_data is not None: data['status'] = taken_data @@ -347,12 +345,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 = _rclpy.rclpy_action_wait_set_get_num_entities(self._client_handle) + num_entities = self._client_handle.get_num_entities() return NumberOfEntities(*num_entities) def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - _rclpy.rclpy_action_wait_set_add(self._client_handle, wait_set) + self._client_handle.add_to_waitset(wait_set) # End Waitable API def send_goal(self, goal, **kwargs): @@ -423,8 +421,7 @@ 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 = _rclpy.rclpy_action_send_goal_request( - self._client_handle, request) + sequence_number = self._client_handle.send_goal_request(request) if sequence_number in self._pending_goal_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending goal request'.format(sequence_number)) @@ -482,9 +479,7 @@ def _cancel_goal_async(self, goal_handle): cancel_request = CancelGoal.Request() cancel_request.goal_info.goal_id = goal_handle.goal_id - sequence_number = _rclpy.rclpy_action_send_cancel_request( - self._client_handle, - cancel_request) + sequence_number = self._client_handle.send_cancel_request(cancel_request) if sequence_number in self._pending_cancel_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending cancel request'.format(sequence_number)) @@ -536,9 +531,7 @@ 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 = _rclpy.rclpy_action_send_result_request( - self._client_handle, - result_request) + sequence_number = self._client_handle.send_result_request(result_request) if sequence_number in self._pending_result_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending result request'.format(sequence_number)) @@ -557,10 +550,8 @@ def server_is_ready(self): :return: True if an action server is ready, False otherwise. """ - with self._node.handle as node_capsule: - return _rclpy.rclpy_action_server_is_available( - node_capsule, - self._client_handle) + with self._node.handle: + return self._client_handle.is_action_server_available() def wait_for_server(self, timeout_sec=None): """ @@ -587,8 +578,8 @@ def destroy(self): """Destroy the underlying action client handle.""" if self._client_handle is None: return - with self._node.handle as node_capsule: - _rclpy.rclpy_action_destroy_entity(self._client_handle, node_capsule) + with self._node.handle: + self._client_handle.destroy_when_not_in_use() self._node.remove_waitable(self) self._client_handle = None diff --git a/rclpy/rclpy/action/graph.py b/rclpy/rclpy/action/graph.py index 828fd9a8c..362e48b97 100644 --- a/rclpy/rclpy/action/graph.py +++ b/rclpy/rclpy/action/graph.py @@ -15,7 +15,6 @@ from typing import List from typing import Tuple -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import Node @@ -34,9 +33,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 as node_capsule: - return _rclpy.rclpy_action_get_client_names_and_types_by_node( - node_capsule, remote_node_name, remote_node_namespace) + with node.handle: + return node.handle.get_action_client_names_and_types_by_node( + remote_node_name, remote_node_namespace) def get_action_server_names_and_types_by_node( @@ -54,9 +53,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 as node_capsule: - return _rclpy.rclpy_action_get_server_names_and_types_by_node( - node_capsule, remote_node_name, remote_node_namespace) + with node.handle: + return node.handle.get_action_server_names_and_types_by_node( + remote_node_name, remote_node_namespace) def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]: @@ -68,5 +67,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 as node_capsule: - return _rclpy.rclpy_action_get_names_and_types(node_capsule) + with node.handle: + return node.handle.get_action_names_and_types() diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index 7df498bfa..64e7aab72 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -43,14 +43,7 @@ class CancelResponse(Enum): ACCEPT = 2 -class GoalEvent(Enum): - """Goal events that cause state transitions.""" - - EXECUTE = 1 - CANCEL_GOAL = 2 - SUCCEED = 3 - ABORT = 4 - CANCELED = 5 +GoalEvent = _rclpy.GoalEvent class ServerGoalHandle: @@ -69,8 +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._handle = _rclpy.rclpy_action_accept_new_goal( - 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 @@ -96,9 +88,9 @@ def goal_id(self): @property def is_active(self): with self._lock: - if self._handle is None: + if self._goal_handle is None: return False - return _rclpy.rclpy_action_goal_handle_is_active(self._handle) + return self._goal_handle.is_active() @property def is_cancel_requested(self): @@ -107,24 +99,24 @@ def is_cancel_requested(self): @property def status(self): with self._lock: - if self._handle is None: + if self._goal_handle is None: return GoalStatus.STATUS_UNKNOWN - return _rclpy.rclpy_action_goal_handle_get_status(self._handle) + return self._goal_handle.get_status() def _update_state(self, event): with self._lock: # Ignore updates for already destructed goal handles - if self._handle is None: + if self._goal_handle is None: return # Update state - _rclpy.rclpy_action_update_goal_state(self._handle, event.value) + self._goal_handle.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 _rclpy.rclpy_action_goal_handle_is_active(self._handle): + if not self._goal_handle.is_active(): self._action_server.notify_goal_done() def execute(self, execute_callback=None): @@ -132,7 +124,7 @@ def execute(self, execute_callback=None): # In this case we want to avoid the illegal state transition to EXECUTING # but still call the users execute callback to let them handle canceling the goal. if not self.is_cancel_requested: - self._update_state(GoalEvent.EXECUTE) + self._update_state(_rclpy.GoalEvent.EXECUTE) self._action_server.notify_execute(self, execute_callback) def publish_feedback(self, feedback): @@ -141,7 +133,7 @@ def publish_feedback(self, feedback): with self._lock: # Ignore for already destructed goal handles - if self._handle is None: + if self._goal_handle is None: return # Populate the feedback message with metadata about this goal @@ -151,24 +143,23 @@ 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(GoalEvent.SUCCEED) + self._update_state(_rclpy.GoalEvent.SUCCEED) def abort(self): - self._update_state(GoalEvent.ABORT) + self._update_state(_rclpy.GoalEvent.ABORT) def canceled(self): - self._update_state(GoalEvent.CANCELED) + self._update_state(_rclpy.GoalEvent.CANCELED) def destroy(self): with self._lock: - if self._handle is None: + if self._goal_handle is None: return - _rclpy.rclpy_action_destroy_server_goal_handle(self._handle) - self._handle = None + self._goal_handle.destroy_when_not_in_use() + self._goal_handle = None self._action_server.remove_future(self._result_future) @@ -249,9 +240,9 @@ def __init__( check_for_type_support(action_type) 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( - node_capsule, + with node.handle, node.get_clock().handle: + self._handle = _rclpy.ActionServer( + node.handle, node.get_clock().handle, action_type, action_name, @@ -279,7 +270,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: @@ -310,11 +301,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)) @@ -360,8 +347,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) @@ -375,16 +362,12 @@ async def _execute_cancel_request(self, request_header_and_message): if CancelResponse.ACCEPT == response: # Notify goal handle - goal_handle._update_state(GoalEvent.CANCEL_GOAL) + goal_handle._update_state(_rclpy.GoalEvent.CANCEL_GOAL) else: # 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 @@ -399,11 +382,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 @@ -417,11 +396,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): @@ -431,7 +406,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] @@ -443,8 +418,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) @@ -453,8 +427,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) @@ -463,8 +436,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) @@ -473,10 +445,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 @@ -501,7 +470,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], @@ -512,7 +481,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): @@ -527,7 +496,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): """ @@ -615,8 +584,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 diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index d133ddb1d..f7a45ff8b 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -34,9 +34,6 @@ class Context: """ def __init__(self): - from rclpy.impl.implementation_singleton import rclpy_implementation - from .handle import Handle - self._handle = Handle(rclpy_implementation.rclpy_create_context()) self._lock = threading.Lock() self._callbacks = [] self._callbacks_lock = threading.Lock() @@ -44,7 +41,10 @@ def __init__(self): @property def handle(self): - return self._handle + return self.__context + + def destroy(self): + self.__context.destroy_when_not_in_use() def init(self, args: Optional[List[str]] = None, @@ -57,31 +57,36 @@ def init(self, :param args: List of command line arguments. """ # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation + from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy global g_logging_ref_count - with self._handle as capsule, self._lock: + with self._lock: if domain_id is not None and domain_id < 0: raise RuntimeError( 'Domain id ({}) should not be lower than zero.' .format(domain_id)) - rclpy_implementation.rclpy_init( - args if args is not None else sys.argv, - capsule, - domain_id if domain_id is not None else rclpy_implementation.RCL_DEFAULT_DOMAIN_ID) - if initialize_logging and not self._logging_initialized: - with g_logging_configure_lock: - g_logging_ref_count += 1 - if g_logging_ref_count == 1: - rclpy_implementation.rclpy_logging_configure(capsule) - self._logging_initialized = True + try: + if self.__context is not None: + raise RuntimeError + except AttributeError: + self.__context = _rclpy.Context( + args if args is not None else sys.argv, + domain_id if domain_id is not None else _rclpy.RCL_DEFAULT_DOMAIN_ID) + if initialize_logging and not self._logging_initialized: + with g_logging_configure_lock: + g_logging_ref_count += 1 + if g_logging_ref_count == 1: + _rclpy.rclpy_logging_configure(self.__context) + self._logging_initialized = True def ok(self): """Check if context hasn't been shut down.""" # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - return rclpy_implementation.rclpy_ok(capsule) + try: + with self.__context, self._lock: + return self.__context.ok() + except AttributeError: + return False def _call_on_shutdown_callbacks(self): with self._callbacks_lock: @@ -94,19 +99,17 @@ def _call_on_shutdown_callbacks(self): def shutdown(self): """Shutdown this context.""" # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - rclpy_implementation.rclpy_shutdown(capsule) + with self.__context, self._lock: + self.__context.shutdown() self._call_on_shutdown_callbacks() self._logging_fini() def try_shutdown(self): """Shutdown this context, if not already shutdown.""" # imported locally to avoid loading extensions on module import - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - if rclpy_implementation.rclpy_ok(capsule): - rclpy_implementation.rclpy_shutdown(capsule) + with self.__context, self._lock: + if self.__context.ok(): + self.__context.shutdown() self._call_on_shutdown_callbacks() def _remove_callback(self, weak_method): @@ -138,6 +141,5 @@ def _logging_fini(self): def get_domain_id(self): """Get domain id of context.""" - from rclpy.impl.implementation_singleton import rclpy_implementation - with self._handle as capsule, self._lock: - return rclpy_implementation.rclpy_context_get_domain_id(capsule) + with self.__context, self._lock: + return self.__context.get_domain_id() diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index a4cc77eaa..09a34d661 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -329,8 +329,8 @@ async def _execute_timer(self, tmr, _): await await_or_execute(tmr.callback) def _take_subscription(self, sub): - with sub.handle as capsule: - msg_info = _rclpy.rclpy_take(capsule, sub.msg_type, sub.raw) + with sub.handle: + msg_info = sub.handle.take_message(sub.msg_type, sub.raw) if msg_info is not None: return msg_info[0] return None @@ -507,12 +507,13 @@ def _wait_for_ready_callbacks( len(subscriptions), len(guards), len(timers), len(clients), len(services)) # Construct a wait set - wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() + wait_set = None with ExitStack() as context_stack: - sub_capsules = [] + sub_handles = [] for sub in subscriptions: try: - sub_capsules.append(context_stack.enter_context(sub.handle)) + context_stack.enter_context(sub.handle) + sub_handles.append(sub.handle) except InvalidHandle: entity_count.num_subscriptions -= 1 @@ -540,10 +541,11 @@ def _wait_for_ready_callbacks( except InvalidHandle: entity_count.num_timers -= 1 - guard_capsules = [] + guard_handles = [] for gc in guards: try: - guard_capsules.append(context_stack.enter_context(gc.handle)) + context_stack.enter_context(gc.handle) + guard_handles.append(gc.handle) except InvalidHandle: entity_count.num_guard_conditions -= 1 @@ -554,44 +556,44 @@ def _wait_for_ready_callbacks( except InvalidHandle: pass - context_capsule = context_stack.enter_context(self._context.handle) - _rclpy.rclpy_wait_set_init( - wait_set, + context_stack.enter_context(self._context.handle) + + wait_set = _rclpy.WaitSet( entity_count.num_subscriptions, entity_count.num_guard_conditions, entity_count.num_timers, entity_count.num_clients, entity_count.num_services, entity_count.num_events, - context_capsule) + self._context.handle) - _rclpy.rclpy_wait_set_clear_entities(wait_set) - for sub_capsule in sub_capsules: - _rclpy.rclpy_wait_set_add_entity('subscription', wait_set, sub_capsule) + wait_set.clear_entities() + for sub_handle in sub_handles: + wait_set.add_subscription(sub_handle) for cli_handle in client_handles: - _rclpy.rclpy_wait_set_add_client(wait_set, cli_handle) + wait_set.add_client(cli_handle) for srv_capsule in service_handles: - _rclpy.rclpy_wait_set_add_service(wait_set, srv_capsule) + wait_set.add_service(srv_capsule) for tmr_handle in timer_handles: - _rclpy.rclpy_wait_set_add_timer(wait_set, tmr_handle) - for gc_capsule in guard_capsules: - _rclpy.rclpy_wait_set_add_entity('guard_condition', wait_set, gc_capsule) + wait_set.add_timer(tmr_handle) + for gc_handle in guard_handles: + wait_set.add_guard_condition(gc_handle) for waitable in waitables: waitable.add_to_wait_set(wait_set) # Wait for something to become ready - _rclpy.rclpy_wait(wait_set, timeout_nsec) + wait_set.wait(timeout_nsec) if self._is_shutdown: raise ShutdownException() if not self._context.ok(): raise ExternalShutdownException() # get ready entities - subs_ready = _rclpy.rclpy_get_ready_entities('subscription', wait_set) - guards_ready = _rclpy.rclpy_get_ready_entities('guard_condition', wait_set) - timers_ready = _rclpy.rclpy_get_ready_entities('timer', wait_set) - clients_ready = _rclpy.rclpy_get_ready_entities('client', wait_set) - services_ready = _rclpy.rclpy_get_ready_entities('service', wait_set) + subs_ready = wait_set.get_ready_entities('subscription') + guards_ready = wait_set.get_ready_entities('guard_condition') + timers_ready = wait_set.get_ready_entities('timer') + clients_ready = wait_set.get_ready_entities('client') + services_ready = wait_set.get_ready_entities('service') # Mark all guards as triggered before yielding since they're auto-taken for gc in guards: diff --git a/rclpy/rclpy/guard_condition.py b/rclpy/rclpy/guard_condition.py index 319b55fb8..8b83df621 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.utilities import get_default_context @@ -21,8 +20,8 @@ class GuardCondition: def __init__(self, callback, callback_group, context=None): self._context = get_default_context() if context is None else context - with self._context.handle as capsule: - self.__handle = Handle(_rclpy.rclpy_create_guard_condition(capsule)) + with self._context.handle: + self.__gc = _rclpy.GuardCondition(self._context.handle) self.callback = callback self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor @@ -31,12 +30,12 @@ def __init__(self, callback, callback_group, context=None): self._executor_triggered = False def trigger(self): - with self.handle as capsule: - _rclpy.rclpy_trigger_guard_condition(capsule) + with self.__gc: + self.__gc.trigger_guard_condition() @property def handle(self): - return self.__handle + return self.__gc def destroy(self): - self.handle.destroy() + self.handle.destroy_when_not_in_use() diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index a4ad1560b..4a090704b 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -56,7 +56,6 @@ from rclpy.executors import Executor from rclpy.expand_topic_name import expand_topic_name from rclpy.guard_condition import GuardCondition -from rclpy.handle import Handle from rclpy.handle import InvalidHandle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.logging import get_logger @@ -170,16 +169,16 @@ def __init__( namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') - with self._context.handle as capsule: + with self._context.handle: try: - self.__handle = Handle(_rclpy.rclpy_create_node( + self.__node = _rclpy.Node( node_name, namespace, - capsule, + self._context.handle, cli_args, use_global_arguments, enable_rosout - )) + ) except ValueError: # these will raise more specific errors if the name or namespace is bad validate_node_name(node_name) @@ -191,16 +190,16 @@ def __init__( validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') - with self.handle as capsule: - self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(capsule)) + with self.handle: + self._logger = get_logger(self.__node.logger_name()) self.__executor_weakref = None self._parameter_event_publisher = self.create_publisher( ParameterEvent, '/parameter_events', qos_profile_parameter_events) - with self.handle as capsule: - self._parameter_overrides = _rclpy.rclpy_get_node_parameters(Parameter, capsule) + with self.handle: + self._parameter_overrides = self.__node.get_parameters(Parameter) # Combine parameters from params files with those from the node constructor and # use the set_parameters_atomically API so a parameter event is published. if parameter_overrides is not None: @@ -305,7 +304,7 @@ def handle(self): :raises: AttributeError if modified after creation. """ - return self.__handle + return self.__node @handle.setter def handle(self, value): @@ -313,13 +312,13 @@ def handle(self, value): def get_name(self) -> str: """Get the name of the node.""" - with self.handle as capsule: - return _rclpy.rclpy_get_node_name(capsule) + with self.handle: + return self.handle.get_node_name() def get_namespace(self) -> str: """Get the namespace of the node.""" - with self.handle as capsule: - return _rclpy.rclpy_get_node_namespace(capsule) + with self.handle: + return self.handle.get_namespace() def get_clock(self) -> Clock: """Get the clock used by the node.""" @@ -1190,8 +1189,8 @@ def resolve_topic_name(self, topic: str, *, only_expand: bool = False) -> str: :return: a fully qualified topic name, result of applying expansion and remapping to the given `topic`. """ - with self.handle as capsule: - return _rclpy.rclpy_resolve_name(capsule, topic, only_expand, False) + with self.handle: + return _rclpy.rclpy_resolve_name(self.handle, topic, only_expand, False) def resolve_service_name( self, service: str, *, only_expand: bool = False @@ -1204,8 +1203,8 @@ def resolve_service_name( :return: a fully qualified service name, result of applying expansion and remapping to the given `service`. """ - with self.handle as capsule: - return _rclpy.rclpy_resolve_name(capsule, service, only_expand, True) + with self.handle: + return _rclpy.rclpy_resolve_name(self.handle, service, only_expand, True) def create_publisher( self, @@ -1256,22 +1255,21 @@ def create_publisher( failed = False check_is_valid_msg_type(msg_type) try: - with self.handle as node_capsule: - publisher_capsule = _rclpy.rclpy_create_publisher( - node_capsule, msg_type, topic, qos_profile.get_c_qos_profile()) + with self.handle: + publisher_object = _rclpy.Publisher( + self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(topic) - publisher_handle = Handle(publisher_capsule) try: publisher = Publisher( - publisher_handle, msg_type, topic, qos_profile, + publisher_object, msg_type, topic, qos_profile, event_callbacks=event_callbacks or PublisherEventCallbacks(), callback_group=callback_group) except Exception: - publisher_handle.destroy() + publisher_object.destroy_when_not_in_use() raise self.__publishers.append(publisher) self._wake_executor() @@ -1334,22 +1332,21 @@ def create_subscription( failed = False check_is_valid_msg_type(msg_type) try: - with self.handle as capsule: - subscription_capsule = _rclpy.rclpy_create_subscription( - capsule, msg_type, topic, qos_profile.get_c_qos_profile()) + with self.handle: + subscription_object = _rclpy.Subscription( + self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(topic) - subscription_handle = Handle(subscription_capsule) try: subscription = Subscription( - subscription_handle, msg_type, + subscription_object, msg_type, topic, callback, callback_group, qos_profile, raw, event_callbacks=event_callbacks or SubscriptionEventCallbacks()) except Exception: - subscription_handle.destroy() + subscription_object.destroy_when_not_in_use() raise self.__subscriptions.append(subscription) callback_group.add_entity(subscription) @@ -1382,9 +1379,9 @@ def create_client( check_is_valid_srv_type(srv_type) failed = False try: - with self.handle as node_capsule: + with self.handle: client_impl = _rclpy.Client( - node_capsule, + self.handle, srv_type, srv_name, qos_profile.get_c_qos_profile()) @@ -1427,9 +1424,9 @@ def create_service( check_is_valid_srv_type(srv_type) failed = False try: - with self.handle as node_capsule: + with self.handle: service_impl = _rclpy.Service( - node_capsule, + self.handle, srv_type, srv_name, qos_profile.get_c_qos_profile()) @@ -1655,7 +1652,7 @@ def destroy_node(self) -> bool: self.destroy_timer(self.__timers[0]) while self.__guards: self.destroy_guard_condition(self.__guards[0]) - self.handle.destroy() + self.__node.destroy_when_not_in_use() self._wake_executor() def get_publisher_names_and_types_by_node( @@ -1676,9 +1673,9 @@ def get_publisher_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_publisher_names_and_types_by_node( - capsule, no_demangle, node_name, node_namespace) + self.handle, no_demangle, node_name, node_namespace) def get_subscriber_names_and_types_by_node( self, @@ -1698,9 +1695,9 @@ def get_subscriber_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_subscriber_names_and_types_by_node( - capsule, no_demangle, node_name, node_namespace) + self.handle, no_demangle, node_name, node_namespace) def get_service_names_and_types_by_node( self, @@ -1718,9 +1715,9 @@ def get_service_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_service_names_and_types_by_node( - capsule, node_name, node_namespace) + self.handle, node_name, node_namespace) def get_client_names_and_types_by_node( self, @@ -1738,9 +1735,9 @@ def get_client_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle as capsule: + with self.handle: return _rclpy.rclpy_get_client_names_and_types_by_node( - capsule, node_name, node_namespace) + self.handle, node_name, node_namespace) def get_topic_names_and_types(self, no_demangle: bool = False) -> List[Tuple[str, List[str]]]: """ @@ -1751,8 +1748,8 @@ def get_topic_names_and_types(self, no_demangle: bool = False) -> List[Tuple[str The first element of each tuple is the topic name and the second element is a list of topic types. """ - with self.handle as capsule: - return _rclpy.rclpy_get_topic_names_and_types(capsule, no_demangle) + with self.handle: + return _rclpy.rclpy_get_topic_names_and_types(self.handle, no_demangle) def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: """ @@ -1762,8 +1759,8 @@ def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: The first element of each tuple is the service name and the second element is a list of service types. """ - with self.handle as capsule: - return _rclpy.rclpy_get_service_names_and_types(capsule) + with self.handle: + return _rclpy.rclpy_get_service_names_and_types(self.handle) def get_node_names(self) -> List[str]: """ @@ -1771,8 +1768,8 @@ def get_node_names(self) -> List[str]: :return: List of node names. """ - with self.handle as capsule: - names_ns = _rclpy.rclpy_get_node_names_and_namespaces(capsule) + with self.handle: + names_ns = self.handle.get_node_names_and_namespaces() return [n[0] for n in names_ns] def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: @@ -1781,8 +1778,8 @@ def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: :return: List of tuples containing two strings: the node name and node namespace. """ - with self.handle as capsule: - return _rclpy.rclpy_get_node_names_and_namespaces(capsule) + with self.handle: + return self.handle.get_node_names_and_namespaces() def get_node_names_and_namespaces_with_enclaves(self) -> List[Tuple[str, str, str]]: """ @@ -1791,8 +1788,8 @@ def get_node_names_and_namespaces_with_enclaves(self) -> List[Tuple[str, str, st :return: List of tuples containing three strings: the node name, node namespace and enclave. """ - with self.handle as capsule: - return _rclpy.rclpy_get_node_names_and_namespaces_with_enclaves(capsule) + with self.handle: + return self.handle.get_node_names_and_namespaces_with_enclaves() def get_fully_qualified_name(self) -> str: """ @@ -1800,14 +1797,14 @@ def get_fully_qualified_name(self) -> str: :return: Fully qualified node name. """ - with self.handle as capsule: - return _rclpy.rclpy_node_get_fully_qualified_name(capsule) + with self.handle: + return self.handle.get_fully_qualified_name() def _count_publishers_or_subscribers(self, topic_name, func): fq_topic_name = expand_topic_name(topic_name, self.get_name(), self.get_namespace()) validate_full_topic_name(fq_topic_name) - with self.handle as node_capsule: - return func(node_capsule, fq_topic_name) + with self.handle: + return func(fq_topic_name) def count_publishers(self, topic_name: str) -> int: """ @@ -1820,7 +1817,9 @@ def count_publishers(self, topic_name: str) -> int: :param topic_name: the topic_name on which to count the number of publishers. :return: the number of publishers on the topic. """ - return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_publishers) + with self.handle: + return self._count_publishers_or_subscribers( + topic_name, self.handle.get_count_publishers) def count_subscribers(self, topic_name: str) -> int: """ @@ -1833,7 +1832,9 @@ def count_subscribers(self, topic_name: str) -> int: :param topic_name: the topic_name on which to count the number of subscribers. :return: the number of subscribers on the topic. """ - return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_subscribers) + with self.handle: + return self._count_publishers_or_subscribers( + topic_name, self.handle.get_count_subscribers) def _get_info_by_topic( self, @@ -1841,16 +1842,16 @@ def _get_info_by_topic( no_mangle: bool, func: Callable[[object, str, bool], List[Dict]] ) -> List[TopicEndpointInfo]: - with self.handle as node_capsule: + with self.handle: if no_mangle: fq_topic_name = topic_name else: fq_topic_name = expand_topic_name( topic_name, self.get_name(), self.get_namespace()) validate_full_topic_name(fq_topic_name) - fq_topic_name = _rclpy.rclpy_remap_topic_name(node_capsule, fq_topic_name) + fq_topic_name = _rclpy.rclpy_remap_topic_name(self.handle, fq_topic_name) - info_dicts = func(node_capsule, fq_topic_name, no_mangle) + info_dicts = func(self.handle, fq_topic_name, no_mangle) infos = [TopicEndpointInfo(**x) for x in info_dicts] return infos diff --git a/rclpy/rclpy/publisher.py b/rclpy/rclpy/publisher.py index eb9c31d7c..ccd41e58f 100644 --- a/rclpy/rclpy/publisher.py +++ b/rclpy/rclpy/publisher.py @@ -15,7 +15,6 @@ from typing import TypeVar, Union from rclpy.callback_groups import CallbackGroup -from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile from rclpy.qos_event import PublisherEventCallbacks @@ -28,7 +27,7 @@ class Publisher: def __init__( self, - publisher_handle: Handle, + publisher_impl: _rclpy.Publisher, msg_type: MsgType, topic: str, qos_profile: QoSProfile, @@ -44,18 +43,18 @@ def __init__( A publisher is used as a primary means of communication in a ROS system by publishing messages on a ROS topic. - :param publisher_handle: Capsule pointing to the underlying ``rcl_publisher_t`` object. + :param publisher_impl: Publisher wrapping the underlying ``rcl_publisher_t`` object. :param msg_type: The type of ROS messages the publisher will publish. :param topic: The name of the topic the publisher will publish to. :param qos_profile: The quality of service profile to apply to the publisher. """ - self.__handle = publisher_handle + self.__publisher = publisher_impl self.msg_type = msg_type self.topic = topic self.qos_profile = qos_profile self.event_handlers: QoSEventHandler = event_callbacks.create_event_handlers( - callback_group, publisher_handle, topic) + callback_group, publisher_impl, topic) def publish(self, msg: Union[MsgType, bytes]) -> None: """ @@ -65,32 +64,32 @@ def publish(self, msg: Union[MsgType, bytes]) -> None: :raises: TypeError if the type of the passed message isn't an instance of the provided type when the publisher was constructed. """ - with self.handle as capsule: + with self.handle: if isinstance(msg, self.msg_type): - _rclpy.rclpy_publish(capsule, msg) + self.__publisher.publish(msg) elif isinstance(msg, bytes): - _rclpy.rclpy_publish_raw(capsule, msg) + self.__publisher.publish_raw(msg) else: raise TypeError('Expected {}, got {}'.format(self.msg_type, type(msg))) def get_subscription_count(self) -> int: """Get the amount of subscribers that this publisher has.""" - with self.handle as capsule: - return _rclpy.rclpy_publisher_get_subscription_count(capsule) + with self.handle: + return self.__publisher.get_subscription_count() @property def topic_name(self) -> str: - with self.handle as capsule: - return _rclpy.rclpy_publisher_get_topic_name(capsule) + with self.handle: + return self.__publisher.get_topic_name() @property def handle(self): - return self.__handle + return self.__publisher def destroy(self): for handler in self.event_handlers: handler.destroy() - self.handle.destroy() + self.__publisher.destroy_when_not_in_use() def assert_liveliness(self) -> None: """ @@ -99,5 +98,5 @@ def assert_liveliness(self) -> None: If the QoS Liveliness policy is set to MANUAL_BY_TOPIC, the application must call this at least as often as ``QoSProfile.liveliness_lease_duration``. """ - with self.handle as capsule: - _rclpy.rclpy_assert_liveliness(capsule) + with self.handle: + _rclpy.rclpy_assert_liveliness(self.handle) diff --git a/rclpy/rclpy/qos_event.py b/rclpy/rclpy/qos_event.py index 1e0edf33e..4969816da 100644 --- a/rclpy/rclpy/qos_event.py +++ b/rclpy/rclpy/qos_event.py @@ -19,7 +19,6 @@ import rclpy from rclpy.callback_groups import CallbackGroup -from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.logging import get_logger from rclpy.qos import qos_policy_name_from_kind @@ -70,15 +69,16 @@ def __init__( callback_group: CallbackGroup, callback: Callable, event_type: IntEnum, - parent_handle: Handle, + parent_impl, ): # Waitable init adds self to callback_group super().__init__(callback_group) self.event_type = event_type self.callback = callback - with parent_handle as parent_capsule: - self.__event = _rclpy.QoSEvent(parent_capsule, event_type) + with parent_impl: + self.__event = _rclpy.QoSEvent(parent_impl, event_type) + self._ready_to_take_data = False self._event_index = None @@ -87,7 +87,7 @@ def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" if self._event_index is None: return False - if _rclpy.rclpy_wait_set_is_ready('event', wait_set, self._event_index): + if wait_set.is_ready('event', self._event_index): self._ready_to_take_data = True return self._ready_to_take_data @@ -112,7 +112,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entites to wait set.""" with self.__event: - self._event_index = _rclpy.rclpy_wait_set_add_event(wait_set, self.__event) + self._event_index = wait_set.add_event(self.__event) def __enter__(self): """Mark event as in-use to prevent destruction while waiting on it.""" @@ -158,10 +158,10 @@ def __init__( self.use_default_callbacks = use_default_callbacks def create_event_handlers( - self, callback_group: CallbackGroup, subscription_handle: Handle, topic_name: str, + self, callback_group: CallbackGroup, subscription: _rclpy.Subscription, topic_name: str, ) -> List[QoSEventHandler]: - with subscription_handle as subscription_capsule: - logger = get_logger(_rclpy.rclpy_get_subscription_logger_name(subscription_capsule)) + with subscription: + logger = get_logger(subscription.get_logger_name()) event_handlers = [] if self.deadline: @@ -169,14 +169,14 @@ def create_event_handlers( callback_group=callback_group, callback=self.deadline, event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, - parent_handle=subscription_handle)) + parent_impl=subscription)) if self.incompatible_qos: event_handlers.append(QoSEventHandler( callback_group=callback_group, callback=self.incompatible_qos, event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, - parent_handle=subscription_handle)) + parent_impl=subscription)) elif self.use_default_callbacks: # Register default callback when not specified try: @@ -192,7 +192,7 @@ def _default_incompatible_qos_callback(event): callback_group=callback_group, callback=_default_incompatible_qos_callback, event_type=event_type, - parent_handle=subscription_handle)) + parent_impl=subscription)) except UnsupportedEventTypeError: pass @@ -202,14 +202,14 @@ def _default_incompatible_qos_callback(event): callback_group=callback_group, callback=self.liveliness, event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED, - parent_handle=subscription_handle)) + parent_impl=subscription)) if self.message_lost: event_handlers.append(QoSEventHandler( callback_group=callback_group, callback=self.message_lost, event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_MESSAGE_LOST, - parent_handle=subscription_handle)) + parent_impl=subscription)) return event_handlers @@ -243,10 +243,10 @@ def __init__( self.use_default_callbacks = use_default_callbacks def create_event_handlers( - self, callback_group: CallbackGroup, publisher_handle: Handle, topic_name: str, + self, callback_group: CallbackGroup, publisher: _rclpy.Publisher, topic_name: str, ) -> List[QoSEventHandler]: - with publisher_handle as publisher_capsule: - logger = get_logger(_rclpy.rclpy_get_publisher_logger_name(publisher_capsule)) + with publisher: + logger = get_logger(publisher.get_logger_name()) event_handlers = [] if self.deadline: @@ -254,21 +254,21 @@ def create_event_handlers( callback_group=callback_group, callback=self.deadline, event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, - parent_handle=publisher_handle)) + parent_impl=publisher)) if self.liveliness: event_handlers.append(QoSEventHandler( callback_group=callback_group, callback=self.liveliness, event_type=QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST, - parent_handle=publisher_handle)) + parent_impl=publisher)) if self.incompatible_qos: event_handlers.append(QoSEventHandler( callback_group=callback_group, callback=self.incompatible_qos, event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, - parent_handle=publisher_handle)) + parent_impl=publisher)) elif self.use_default_callbacks: # Register default callback when not specified try: @@ -283,7 +283,7 @@ def _default_incompatible_qos_callback(event): callback_group=callback_group, callback=_default_incompatible_qos_callback, event_type=QoSPublisherEventType.RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, - parent_handle=publisher_handle)) + parent_impl=publisher)) except UnsupportedEventTypeError: pass diff --git a/rclpy/rclpy/signals.py b/rclpy/rclpy/signals.py index 0dc425af7..df47ffc18 100644 --- a/rclpy/rclpy/signals.py +++ b/rclpy/rclpy/signals.py @@ -21,8 +21,9 @@ class SignalHandlerGuardCondition(GuardCondition): def __init__(self, context=None): super().__init__(callback=None, callback_group=None, context=context) - with self.handle as capsule: - _signals.rclpy_register_sigint_guard_condition(capsule) + with self.handle: + # TODO(ahcorde): Remove the pycapsule method when #728 is in + _signals.rclpy_register_sigint_guard_condition(self.handle.pycapsule()) def __del__(self): try: @@ -35,6 +36,7 @@ def __del__(self): pass def destroy(self): - with self.handle as capsule: - _signals.rclpy_unregister_sigint_guard_condition(capsule) + with self.handle: + # TODO(ahcorde): Remove the pycapsule method when #728 is in + _signals.rclpy_unregister_sigint_guard_condition(self.handle.pycapsule()) super().destroy() diff --git a/rclpy/rclpy/subscription.py b/rclpy/rclpy/subscription.py index 3ad22766c..ed16d5ecb 100644 --- a/rclpy/rclpy/subscription.py +++ b/rclpy/rclpy/subscription.py @@ -16,7 +16,6 @@ from typing import TypeVar from rclpy.callback_groups import CallbackGroup -from rclpy.handle import Handle from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile from rclpy.qos_event import QoSEventHandler @@ -31,7 +30,7 @@ class Subscription: def __init__( self, - subscription_handle: Handle, + subscription_impl: _rclpy.Subscription, msg_type: MsgType, topic: str, callback: Callable, @@ -46,8 +45,8 @@ def __init__( .. warning:: Users should not create a subscription with this constructor, instead they should call :meth:`.Node.create_subscription`. - :param subscription_handle: :class:`Handle` wrapping the underlying ``rcl_subscription_t`` - object. + :param subscription_impl: :class:`Subscription` wrapping the underlying + ``rcl_subscription_t`` object. :param msg_type: The type of ROS messages the subscription will subscribe to. :param topic: The name of the topic the subscription will subscribe to. :param callback: A user-defined callback function that is called when a message is @@ -58,7 +57,7 @@ def __init__( :param raw: If ``True``, then received messages will be stored in raw binary representation. """ - self.__handle = subscription_handle + self.__subscription = subscription_impl self.msg_type = msg_type self.topic = topic self.callback = callback @@ -69,18 +68,18 @@ def __init__( self.raw = raw self.event_handlers: QoSEventHandler = event_callbacks.create_event_handlers( - callback_group, subscription_handle, topic) + callback_group, subscription_impl, topic) @property def handle(self): - return self.__handle + return self.__subscription def destroy(self): for handler in self.event_handlers: handler.destroy() - self.handle.destroy() + self.handle.destroy_when_not_in_use() @property def topic_name(self): - with self.handle as h: - return _rclpy.rclpy_get_subscription_topic_name(h) + with self.handle: + return self.__subscription.get_topic_name() diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index 83d4fc4f8..c570524d9 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -25,9 +25,9 @@ class Timer: def __init__(self, callback, callback_group, timer_period_ns, clock, *, context=None): self._context = get_default_context() if context is None else context self._clock = clock - with self._clock.handle, self._context.handle as context_capsule: + with self._clock.handle, self._context.handle: self.__timer = _rclpy.Timer( - self._clock.handle, context_capsule, timer_period_ns) + self._clock.handle, self._context.handle, timer_period_ns) self.timer_period_ns = timer_period_ns self.callback = callback self.callback_group = callback_group diff --git a/rclpy/src/rclpy/_rclpy_action.cpp b/rclpy/src/rclpy/_rclpy_action.cpp deleted file mode 100644 index d105ad6f0..000000000 --- a/rclpy/src/rclpy/_rclpy_action.cpp +++ /dev/null @@ -1,1320 +0,0 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include -#include - -#include "rclpy_common/exceptions.hpp" - -#include "rclpy_common/common.h" -#include "rclpy_common/handle.h" - -#include "clock.hpp" - -namespace py = pybind11; - - -template -T -get_pointer(py::capsule & capsule, const char * name) -{ - if (strcmp(name, capsule.name())) { - std::string error_text{"Expected capusle with name '"}; - error_text += name; - error_text += "' but got '"; - error_text += capsule.name(); - error_text += "'"; - throw py::value_error(error_text); - } - // TODO(sloretz) use get_pointer() in pybind11 2.6+ - return static_cast(capsule); -} - - -/// Destroy an rcl_action entity. -/** - * Raises RuntimeError on failure. - * - * \param[in] pyentity Capsule pointing to the entity to destroy. - * \param[in] pynode Capsule pointing to the node the action client was added to. - */ -void -rclpy_action_destroy_entity(py::capsule pyentity, py::capsule pynode) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_ret_t ret; - if (0 == std::strcmp("rcl_action_client_t", pyentity.name())) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - ret = rcl_action_client_fini(action_client, node); - PyMem_Free(action_client); - } else if (0 == std::strcmp("rcl_action_server_t", pyentity.name())) { - auto action_server = get_pointer(pyentity, "rcl_action_server_t"); - ret = rcl_action_server_fini(action_server, node); - PyMem_Free(action_server); - } else { - std::string entity_name = pyentity.name(); - throw std::runtime_error(entity_name + " is not a known entity"); - } - - if (ret != RCL_RET_OK) { - std::string error_text = "Failed to fini '"; - error_text += pyentity.name(); - error_text += "'"; - throw rclpy::RCLError(error_text); - } - - if (PyCapsule_SetName(pyentity.ptr(), "_destroyed_by_rclpy_action_destroy_entity_")) { - throw py::error_already_set(); - } -} - -void -rclpy_action_destroy_server_goal_handle(py::capsule pygoal_handle) -{ - if (strcmp("rcl_action_goal_handle_t", pygoal_handle.name())) { - throw py::value_error("Capsule must be an rcl_action_goal_handle_t"); - } - - auto goal_handle = - get_pointer(pygoal_handle, "rcl_action_goal_handle_t"); - - rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Error destroying action goal handle"); - } -} - -/// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. -/** - * Raises RuntimeError if the QoS profile is unknown. - * - * This function takes a string defining a rmw_qos_profile_t and returns the - * corresponding Python QoSProfile object. - * \param[in] rmw_profile String with the name of the profile to load. - * \return QoSProfile object. - */ -py::dict -rclpy_action_get_rmw_qos_profile(const char * rmw_profile) -{ - PyObject * pyqos_profile = NULL; - if (0 == strcmp(rmw_profile, "rcl_action_qos_profile_status_default")) { - pyqos_profile = rclpy_common_convert_to_qos_dict(&rcl_action_qos_profile_status_default); - } else { - std::string error_text = "Requested unknown rmw_qos_profile: "; - error_text += rmw_profile; - throw std::runtime_error(error_text); - } - return py::reinterpret_steal(pyqos_profile); -} - -/// Add an action entitiy to a wait set. -/** - * Raises RuntimeError on failure. - * \param[in] pyentity Capsule pointer to an action entity - * (rcl_action_client_t or rcl_action_server_t). - * \param[in] pywait_set Capsule pointer to an rcl_wait_set_t. - */ -void -rclpy_action_wait_set_add(py::capsule pyentity, py::capsule pywait_set) -{ - auto wait_set = get_pointer(pywait_set, "rcl_wait_set_t"); - - rcl_ret_t ret; - if (0 == strcmp(pyentity.name(), "rcl_action_client_t")) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - ret = rcl_action_wait_set_add_action_client(wait_set, action_client, NULL, NULL); - } else if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { - auto action_server = get_pointer(pyentity, "rcl_action_server_t"); - ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); - } else { - std::string error_text{"Unknown entity: "}; - error_text += pyentity.name(); - throw std::runtime_error(error_text); - } - - if (RCL_RET_OK != ret) { - std::string error_text{"Failed to add '"}; - error_text += pyentity.name(); - error_text += "' to wait set"; - throw rclpy::RCLError(error_text); - } -} - -/// Get the number of wait set entities that make up an action entity. -/** - * \param[in] pyentity Capsule pointer to an action entity - * (rcl_action_client_t or rcl_action_server_t). - * \return Tuple containing the number of wait set entities: - * (num_subscriptions, - * num_guard_conditions, - * num_timers, - * num_clients, - * num_services) - */ -py::tuple -rclpy_action_wait_set_get_num_entities(py::capsule pyentity) -{ - size_t num_subscriptions = 0u; - size_t num_guard_conditions = 0u; - size_t num_timers = 0u; - size_t num_clients = 0u; - size_t num_services = 0u; - - rcl_ret_t ret; - if (0 == strcmp(pyentity.name(), "rcl_action_client_t")) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - - ret = rcl_action_client_wait_set_get_num_entities( - action_client, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - } else if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { - auto action_server = get_pointer(pyentity, "rcl_action_server_t"); - - ret = rcl_action_server_wait_set_get_num_entities( - action_server, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - } else { - std::string error_text{"Unknown entity: "}; - error_text += pyentity.name(); - throw std::runtime_error(error_text); - } - - if (RCL_RET_OK != ret) { - std::string error_text{"Failed to get number of entities for '"}; - error_text += pyentity.name(); - error_text += "'"; - throw rclpy::RCLError(error_text); - } - - py::tuple result_tuple(5); - result_tuple[0] = py::int_(num_subscriptions); - result_tuple[1] = py::int_(num_guard_conditions); - result_tuple[2] = py::int_(num_timers); - result_tuple[3] = py::int_(num_clients); - result_tuple[4] = py::int_(num_services); - return result_tuple; -} - -/// Check if an action entity has any ready wait set entities. -/** - * This must be called after waiting on the wait set. - * Raises RuntimeError on failure. - * - * \param[in] entity Capsule pointing to the action entity - * (rcl_action_client_t or rcl_action_server_t). - * \param[in] pywait_set Capsule pointing to the wait set structure. - * \return A tuple of Bool representing the ready sub-entities. - * For a rcl_action_client_t: - * (is_feedback_ready, - * is_status_ready, - * is_goal_response_ready, - * is_cancel_response_ready, - * is_result_response_ready) - * - * For a rcl_action_server_t: - * (is_goal_request_ready, - * is_cancel_request_ready, - * is_result_request_ready, - * is_goal_expired) - */ -py::tuple -rclpy_action_wait_set_is_ready(py::capsule pyentity, py::capsule pywait_set) -{ - auto wait_set = get_pointer(pywait_set, "rcl_wait_set_t"); - - if (0 == strcmp(pyentity.name(), "rcl_action_client_t")) { - auto action_client = get_pointer(pyentity, "rcl_action_client_t"); - bool is_feedback_ready = false; - bool is_status_ready = false; - bool is_goal_response_ready = false; - bool is_cancel_response_ready = false; - bool is_result_response_ready = false; - rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( - wait_set, - action_client, - &is_feedback_ready, - &is_status_ready, - &is_goal_response_ready, - &is_cancel_response_ready, - &is_result_response_ready); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get number of ready entities for action client"); - } - - py::tuple result_tuple(5); - result_tuple[0] = py::bool_(is_feedback_ready); - result_tuple[1] = py::bool_(is_status_ready); - result_tuple[2] = py::bool_(is_goal_response_ready); - result_tuple[3] = py::bool_(is_cancel_response_ready); - result_tuple[4] = py::bool_(is_result_response_ready); - return result_tuple; - } else if (0 == strcmp(pyentity.name(), "rcl_action_server_t")) { - auto action_server = get_pointer(pyentity, "rcl_action_server_t"); - bool is_goal_request_ready = false; - bool is_cancel_request_ready = false; - bool is_result_request_ready = false; - bool is_goal_expired = false; - rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( - wait_set, - action_server, - &is_goal_request_ready, - &is_cancel_request_ready, - &is_result_request_ready, - &is_goal_expired); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get number of ready entities for action server"); - } - - py::tuple result_tuple(4); - result_tuple[0] = py::bool_(is_goal_request_ready); - result_tuple[1] = py::bool_(is_cancel_request_ready); - result_tuple[2] = py::bool_(is_result_request_ready); - result_tuple[3] = py::bool_(is_goal_expired); - return result_tuple; - } - - std::string error_text{"Unknown entity: "}; - error_text += pyentity.name(); - throw std::runtime_error(error_text); -} - -/// Create an action client. -/** - * This function will create an action client for the given action name. - * This client will use the typesupport defined in the action module - * provided as pyaction_type to send messages over the wire. - * - * On a successful call a capsule referencing the created rcl_action_client_t structure - * is returned. - * - * Raises ValueError if action name is invalid - * Raises RuntimeError if the action client could not be created. - * - * \remark Call rclpy_action_destroy_entity() to destroy an action client. - * \param[in] pynode Capsule pointing to the node to add the action client to. - * \param[in] pyaction_type Action module associated with the action client. - * \param[in] pyaction_name Python object containing the action name. - * \param[in] goal_service_qos rmw_qos_profile_t object for the goal service. - * \param[in] result_service_qos rmw_qos_profile_t object for the result service. - * \param[in] cancel_service_qos rmw_qos_profile_t object for the cancel service. - * \param[in] feedback_qos rmw_qos_profile_t object for the feedback subscriber. - * \param[in] status_qos rmw_qos_profile_t object for the status subscriber. - * \return Capsule named 'rcl_action_client_t', or - * \return NULL on failure. - */ -py::capsule -rclpy_action_create_client( - py::capsule pynode, - py::object pyaction_type, - const char * action_name, - const rmw_qos_profile_t & goal_service_qos, - const rmw_qos_profile_t & result_service_qos, - const rmw_qos_profile_t & cancel_service_qos, - const rmw_qos_profile_t & feedback_topic_qos, - const rmw_qos_profile_t & status_topic_qos) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rosidl_action_type_support_t * ts = - static_cast(rclpy_common_get_type_support( - pyaction_type.ptr())); - if (!ts) { - throw py::error_already_set(); - } - - rcl_action_client_options_t action_client_ops = rcl_action_client_get_default_options(); - - action_client_ops.goal_service_qos = goal_service_qos; - action_client_ops.result_service_qos = result_service_qos; - action_client_ops.cancel_service_qos = cancel_service_qos; - action_client_ops.feedback_topic_qos = feedback_topic_qos; - action_client_ops.status_topic_qos = status_topic_qos; - - auto deleter = [](rcl_action_client_t * ptr) {PyMem_Free(ptr);}; - auto action_client = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_action_client_t))), - deleter); - if (!action_client) { - throw std::bad_alloc(); - } - - *action_client = rcl_action_get_zero_initialized_client(); - rcl_ret_t ret = rcl_action_client_init( - action_client.get(), - node, - ts, - action_name, - &action_client_ops); - if (ret == RCL_RET_ACTION_NAME_INVALID) { - std::string error_text{"Failed to create action client due to invalid topic name '"}; - error_text += action_name; - error_text += "' : "; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - throw py::value_error(error_text); - } else if (ret != RCL_RET_OK) { - std::string error_text{"Failed to create action client: "}; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - throw py::value_error(error_text); - } - - return py::capsule(action_client.release(), "rcl_action_client_t"); -} - -/// Create an action server. -/** - * This function will create an action server for the given action name. - * This server will use the typesupport defined in the action module - * provided as pyaction_type to send messages over the wire. - * - * On a successful call a capsule referencing the created rcl_action_server_t structure - * is returned. - * - * Raises AttributeError if action type is invalid - * Raises ValueError if action name is invalid - * Raises RuntimeError if the action server could not be created. - * - * \remark Call rclpy_action_destroy_entity() to destroy an action server. - * \param[in] pynode Capsule pointing to the node to add the action server to. - * \param[in] pyaction_type Action module associated with the action server. - * \param[in] pyaction_name Python object containing the action name. - * \param[in] goal_service_qos rmw_qos_profile_t object for the goal service. - * \param[in] result_service_qos rmw_qos_profile_t object for the result service. - * \param[in] cancel_service_qos rmw_qos_profile_t object for the cancel service. - * \param[in] feedback_qos rmw_qos_profile_t object for the feedback subscriber. - * \param[in] status_qos rmw_qos_profile_t object for the status subscriber. - * \return Capsule named 'rcl_action_server_t', or - * \return NULL on failure. - */ -py::capsule -rclpy_action_create_server( - py::capsule pynode, - const rclpy::Clock & rclpy_clock, - py::object pyaction_type, - const char * action_name, - const rmw_qos_profile_t & goal_service_qos, - const rmw_qos_profile_t & result_service_qos, - const rmw_qos_profile_t & cancel_service_qos, - const rmw_qos_profile_t & feedback_topic_qos, - const rmw_qos_profile_t & status_topic_qos, - double result_timeout) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_clock_t * clock = rclpy_clock.rcl_ptr(); - - rosidl_action_type_support_t * ts = static_cast( - rclpy_common_get_type_support(pyaction_type.ptr())); - if (!ts) { - throw py::error_already_set(); - } - - rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); - - action_server_ops.goal_service_qos = goal_service_qos; - action_server_ops.result_service_qos = result_service_qos; - action_server_ops.cancel_service_qos = cancel_service_qos; - action_server_ops.feedback_topic_qos = feedback_topic_qos; - action_server_ops.status_topic_qos = status_topic_qos; - action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(result_timeout); - - auto deleter = [](rcl_action_server_t * ptr) {PyMem_Free(ptr);}; - auto action_server = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_action_server_t))), - deleter); - if (!action_server) { - throw std::bad_alloc(); - } - - *action_server = rcl_action_get_zero_initialized_server(); - rcl_ret_t ret = rcl_action_server_init( - action_server.get(), - node, - clock, - ts, - action_name, - &action_server_ops); - if (ret == RCL_RET_ACTION_NAME_INVALID) { - std::string error_text{"Failed to create action server due to invalid topic name '"}; - error_text += action_name; - error_text += "' : "; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - throw py::value_error(error_text); - } else if (ret != RCL_RET_OK) { - std::string error_text{"Failed to create action server: "}; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - throw py::value_error(error_text); - } - - return py::capsule(action_server.release(), "rcl_action_server_t"); -} - - -/// Check if an action server is available for the given action client. -/** - * Raises RuntimeError on failure. - * - * \param[in] pynode Capsule pointing to the node to associated with the action client. - * \param[in] pyaction_client The action client to use when checking for an available server. - * \return True if an action server is available, False otherwise. - */ -bool -rclpy_action_server_is_available(py::capsule pynode, py::capsule pyaction_client) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); - - bool is_available = false; - rcl_ret_t ret = rcl_action_server_is_available(node, action_client, &is_available); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to check if action server is available"); - } - return is_available; -} - -#define SEND_SERVICE_REQUEST(Type) \ - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - void * raw_ros_request = rclpy_convert_from_py(pyrequest.ptr(), &destroy_ros_message); \ - if (!raw_ros_request) { \ - throw py::error_already_set(); \ - } \ - int64_t sequence_number; \ - rcl_ret_t ret = rcl_action_send_ ## Type ## _request( \ - action_client, raw_ros_request, &sequence_number); \ - destroy_ros_message(raw_ros_request); \ - if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to send " #Type " request"); \ - } \ - return sequence_number; - -#define SEND_SERVICE_RESPONSE(Type) \ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - void * raw_ros_response = rclpy_convert_from_py(pyresponse.ptr(), &destroy_ros_message); \ - if (!raw_ros_response) { \ - throw py::error_already_set(); \ - } \ - rcl_ret_t ret = rcl_action_send_ ## Type ## _response(action_server, header, raw_ros_response); \ - destroy_ros_message(raw_ros_response); \ - if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to send " #Type " response"); \ - } - -#define TAKE_SERVICE_REQUEST(Type) \ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - /* taken_msg is always destroyed in this function */ \ - void * taken_msg = rclpy_create_from_py(pymsg_type.ptr(), &destroy_ros_message); \ - if (!taken_msg) { \ - throw py::error_already_set(); \ - } \ - auto taken_msg_ptr = \ - std::unique_ptr(taken_msg, destroy_ros_message); \ - rmw_request_id_t header; \ - rcl_ret_t ret = \ - rcl_action_take_ ## Type ## _request(action_server, &header, taken_msg_ptr.get()); \ - /* Create the tuple to return */ \ - py::tuple pytuple(2); \ - if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED || ret == RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ - pytuple[0] = py::none(); \ - pytuple[1] = py::none(); \ - return pytuple; \ - } else if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to take " #Type); \ - } \ - pytuple[0] = header; \ - pytuple[1] = py::reinterpret_steal( \ - rclpy_convert_to_py(taken_msg_ptr.get(), pymsg_type.ptr())); \ - return pytuple; - -#define TAKE_SERVICE_RESPONSE(Type) \ - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - /* taken_msg is always destroyed in this function */ \ - void * taken_msg = rclpy_create_from_py(pymsg_type.ptr(), &destroy_ros_message); \ - if (!taken_msg) { \ - throw py::error_already_set(); \ - } \ - auto taken_msg_ptr = \ - std::unique_ptr(taken_msg, destroy_ros_message); \ - rmw_request_id_t header; \ - rcl_ret_t ret = rcl_action_take_ ## Type ## _response(action_client, &header, taken_msg); \ - int64_t sequence = header.sequence_number; \ - /* Create the tuple to return */ \ - py::tuple pytuple(2); \ - if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED || ret == RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ - pytuple[0] = py::none(); \ - pytuple[1] = py::none(); \ - return pytuple; \ - } else if (ret != RCL_RET_OK) { \ - throw rclpy::RCLError("Failed to take " #Type); \ - } \ - pytuple[0] = py::int_(sequence); \ - pytuple[1] = py::reinterpret_steal( \ - rclpy_convert_to_py(taken_msg_ptr.release(), pymsg_type.ptr())); \ - return pytuple; \ - - -/// Send an action goal request. -/** - * Raises AttributeError if there is an issue parsing the pygoal_request. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pygoal_request The request message to send. - * \return sequence_number PyLong object representing the index of the sent request, or - * \return NULL if there is a failure. - */ -int64_t -rclpy_action_send_goal_request(py::capsule pyaction_client, py::object pyrequest) -{ - SEND_SERVICE_REQUEST(goal) -} - -/// Take an action goal request. -/** - * Raises AttributeError if there is an issue parsing the pygoal_request_type. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when taking the request. - * \param[in] pygoal_request_type An instance of the type of request message to take. - * \return 2-tuple (header, received request message) where the header is an - * "rclpy.rmw_request_id_t" type, or - * \return 2-tuple (None, None) if there as no message to take, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_goal_request(py::capsule pyaction_server, py::object pymsg_type) -{ - TAKE_SERVICE_REQUEST(goal) -} - -/// Send an action goal response. -/** - * Raises AttributeError if there is an issue parsing the pygoal_response. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when sending the response. - * \param[in] header Pointer to the message header. - * \param[in] pygoal_response The response message to send. - * \return None - * \return NULL if there is a failure. - */ -void -rclpy_action_send_goal_response( - py::capsule pyaction_server, rmw_request_id_t * header, py::object pyresponse) -{ - SEND_SERVICE_RESPONSE(goal) -} - -/// Take an action goal response. -/** - * Raises AttributeError if there is an issue parsing the pygoal_response_type. - * Raises RuntimeError if the underlying rcl library returns an error when taking the response. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pygoal_response_type An instance of the response message type to take. - * \return 2-tuple (sequence number, received response), or - * \return 2-tuple (None, None) if there is no response, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_goal_response(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_SERVICE_RESPONSE(goal) -} - -/// Send an action result request. -/** - * Raises AttributeError if there is an issue parsing the pyresult_request. - * Raises RuntimeError if the underlying rcl library returns an error when sending the request. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pyresult_request The request message to send. - * \return sequence_number PyLong object representing the index of the sent request, or - * \return NULL if there is a failure. - */ -int64_t -rclpy_action_send_result_request(py::capsule pyaction_client, py::object pyrequest) -{ - SEND_SERVICE_REQUEST(result); -} - -/// Take an action result request. -/** - * Raises AttributeError if there is an issue parsing the pyresult_request_type. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when taking the request. - * \param[in] pyresult_request_type An instance of the type of request message to take. - * \return 2-tuple (header, received request message) where the header is an - * "rclpy.rmw_request_id_t" type, or - * \return 2-tuple (None, None) if there as no message to take, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_result_request(py::capsule pyaction_server, py::object pymsg_type) -{ - TAKE_SERVICE_REQUEST(result) -} - -/// Send an action result response. -/** - * Raises AttributeError if there is an issue parsing the pyresult_response. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when sending the response. - * \param[in] pyheader Pointer to the message header. - * \param[in] pyresult_response The response message to send. - * \return None - * \return NULL if there is a failure. - */ -void -rclpy_action_send_result_response( - py::capsule pyaction_server, rmw_request_id_t * header, py::object pyresponse) -{ - SEND_SERVICE_RESPONSE(result) -} - -/// Take an action result response. -/** - * Raises AttributeError if there is an issue parsing the pyresult_response_type. - * Raises RuntimeError if the underlying rcl library returns an error when taking the response. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pyresult_response_type An instance of the response message type to take. - * \return 2-tuple (sequence number, received response), or - * \return 2-tuple (None, None) if there is no response, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_result_response(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_SERVICE_RESPONSE(result); -} - -/// Send an action cancel request. -/** - * Raises AttributeError if there is an issue parsing the pycancel_request. - * Raises RuntimeError if the underlying rcl library returns an error when sending the request. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pycancel_request The request message to send. - * \return sequence_number PyLong object representing the index of the sent request, or - * \return NULL if there is a failure. - */ -int64_t -rclpy_action_send_cancel_request(py::capsule pyaction_client, py::object pyrequest) -{ - SEND_SERVICE_REQUEST(cancel) -} - -/// Take an action cancel request. -/** - * Raises AttributeError if there is an issue parsing the pycancel_request_type. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when taking the request. - * \param[in] pycancel_request_type An instance of the type of request message to take. - * \return 2-tuple (header, received request message) where the header is an - * "rmw_request_id_t" type, or - * \return 2-tuple (None, None) if there as no message to take, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_cancel_request(py::capsule pyaction_server, py::object pymsg_type) -{ - TAKE_SERVICE_REQUEST(cancel) -} - -/// Send an action cancel response. -/** - * Raises AttributeError if there is an issue parsing the pycancel_response. - * Raises RuntimeError on failure. - * - * \param[in] pyaction_server The action server to use when sending the response. - * \param[in] pyheader Pointer to the message header. - * \param[in] pycancel_response The response message to send. - * \return sequence_number PyLong object representing the index of the sent response, or - * \return NULL if there is a failure. - */ -void -rclpy_action_send_cancel_response( - py::capsule pyaction_server, rmw_request_id_t * header, py::object pyresponse) -{ - SEND_SERVICE_RESPONSE(cancel) -} - -/// Take an action cancel response. -/** - * Raises AttributeError if there is an issue parsing the pycancel_response_type. - * Raises RuntimeError if the underlying rcl library returns an error when taking the response. - * - * \param[in] pyaction_client The action client to use when sending the request. - * \param[in] pycancel_response_type An instance of the response message type to take. - * \return 2-tuple (sequence number, received response), or - * \return 2-tuple (None, None) if there is no response, or - * \return NULL if there is a failure. - */ -py::tuple -rclpy_action_take_cancel_response(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_SERVICE_RESPONSE(cancel) -} - -#define TAKE_MESSAGE(Type) \ - auto action_client = get_pointer(pyaction_client, "rcl_action_client_t"); \ - destroy_ros_message_signature * destroy_ros_message = NULL; \ - void * taken_msg = rclpy_create_from_py(pymsg_type.ptr(), &destroy_ros_message); \ - if (!taken_msg) { \ - throw py::error_already_set(); \ - } \ - auto taken_msg_ptr = std::unique_ptr( \ - taken_msg, destroy_ros_message); \ - rcl_ret_t ret = rcl_action_take_ ## Type(action_client, taken_msg); \ - if (ret != RCL_RET_OK) { \ - if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED) { \ - /* if take failed, just do nothing */ \ - return py::none(); \ - } \ - throw rclpy::RCLError("Failed to take " #Type " with an action client"); \ - } \ - return py::reinterpret_steal(rclpy_convert_to_py(taken_msg, pymsg_type.ptr())); - -/// Publish a feedback message from a given action server. -/** - * Raises AttributeError if there is an issue parsing the pyfeedback_msg. - * Raises RuntimeError on failure while publishing a feedback message. - * - * \param[in] pyaction_server Capsule pointing to the action server to publish the message. - * \param[in] pyfeedback_msg The feedback message to publish. - * \return None - */ -void -rclpy_action_publish_feedback(py::capsule pyaction_server, py::object pymsg) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - destroy_ros_message_signature * destroy_ros_message = NULL; - void * raw_ros_message = rclpy_convert_from_py(pymsg.ptr(), &destroy_ros_message); - if (!raw_ros_message) { - throw py::error_already_set(); - } - auto raw_ros_message_ptr = std::unique_ptr( - raw_ros_message, destroy_ros_message); - rcl_ret_t ret = rcl_action_publish_feedback(action_server, raw_ros_message); - if (ret != RCL_RET_OK) { - throw rclpy::RCLError("Failed to publish feedback with an action server"); - } -} - -/// Take a feedback message from a given action client. -/** - * Raises AttributeError if there is an issue parsing the pyfeedback_type. - * Raises RuntimeError on failure while taking a feedback message. Note, this does not include - * the case where there are no messages available. - * - * \param[in] pyaction_client Capsule pointing to the action client to process the message. - * \param[in] pyfeedback_type Instance of the feedback message type to take. - * \return Python message with all fields populated with received message, or - * \return None if there is nothing to take, or - * \return NULL if there is a failure. - */ -py::object -rclpy_action_take_feedback(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_MESSAGE(feedback) -} - -/// Publish a status message from a given action server. -/** - * Raises RuntimeError on failure while publishing a status message. - * - * \param[in] pyaction_server Capsule pointing to the action server to publish the message. - * \return None - */ -void -rclpy_action_publish_status(py::capsule pyaction_server) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - rcl_action_goal_status_array_t status_message = - rcl_action_get_zero_initialized_goal_status_array(); - rcl_ret_t ret = rcl_action_get_goal_status_array(action_server, &status_message); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed get goal status array"); - } - - ret = rcl_action_publish_status(action_server, &status_message); - - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed publish goal status array"); - } -} - -/// Take a status message from a given action client. -/** - * Raises AttributeError if there is an issue parsing the pystatus_type. - * Raises RuntimeError on failure while taking a status message. Note, this does not include - * the case where there are no messages available. - * - * \param[in] pyaction_client Capsule pointing to the action client to process the message. - * \param[in] pystatus_type Instance of the status message type to take. - * \return Python message with all fields populated with received message, or - * \return None if there is nothing to take, or - * \return NULL if there is a failure. - */ -py::object -rclpy_action_take_status(py::capsule pyaction_client, py::object pymsg_type) -{ - TAKE_MESSAGE(status) -} - -py::capsule -rclpy_action_accept_new_goal(py::capsule pyaction_server, py::object pygoal_info_msg) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - destroy_ros_message_signature * destroy_ros_message = NULL; - auto goal_info_msg = static_cast( - rclpy_convert_from_py(pygoal_info_msg.ptr(), &destroy_ros_message)); - if (!goal_info_msg) { - throw py::error_already_set(); - } - - auto goal_info_msg_ptr = std::unique_ptr( - goal_info_msg, destroy_ros_message); - - rcl_action_goal_handle_t * goal_handle = rcl_action_accept_new_goal( - action_server, goal_info_msg); - if (!goal_handle) { - throw rclpy::RCLError("Failed to accept new goal"); - } - // TODO(sloretz) capsule destructor instead of rclpy_action_destroy_server_goal_handle() - return py::capsule(goal_handle, "rcl_action_goal_handle_t"); -} - -void -rclpy_action_notify_goal_done(py::capsule pyaction_server) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - rcl_ret_t ret = rcl_action_notify_goal_done(action_server); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to notfiy action server of goal done"); - } -} - -/// Convert from a Python GoalEvent code to an rcl goal event code. -/** - * Raises std::runtime_error if conversion fails - * - * \param[in] pyevent The Python GoalEvent code. - * \return The rcl equivalent of the Python GoalEvent code - */ -static rcl_action_goal_event_t -convert_from_py_goal_event(const int64_t event) -{ - py::module server_module = py::module::import("rclpy.action.server"); - py::object goal_event_class = server_module.attr("GoalEvent"); - py::int_ pyevent(event); - - if (goal_event_class.attr("EXECUTE").attr("value").cast().is(pyevent)) { - return GOAL_EVENT_EXECUTE; - } - if (goal_event_class.attr("CANCEL_GOAL").attr("value").cast().is(pyevent)) { - return GOAL_EVENT_CANCEL_GOAL; - } - if (goal_event_class.attr("SUCCEED").attr("value").cast().is(pyevent)) { - return GOAL_EVENT_SUCCEED; - } - if (goal_event_class.attr("ABORT").attr("value").cast().is(pyevent)) { - return GOAL_EVENT_ABORT; - } - if (goal_event_class.attr("CANCELED").attr("value").cast().is(pyevent)) { - return GOAL_EVENT_CANCELED; - } - throw std::runtime_error("Error converting goal event type: unknown goal event"); -} - -void -rclpy_action_update_goal_state(py::capsule pygoal_handle, int64_t pyevent) -{ - auto goal_handle = get_pointer( - pygoal_handle, "rcl_action_goal_handle_t"); - - rcl_action_goal_event_t event = convert_from_py_goal_event(pyevent); - - rcl_ret_t ret = rcl_action_update_goal_state(goal_handle, event); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to update goal state"); - } -} - -bool -rclpy_action_goal_handle_is_active(py::capsule pygoal_handle) -{ - auto goal_handle = get_pointer( - pygoal_handle, "rcl_action_goal_handle_t"); - return rcl_action_goal_handle_is_active(goal_handle); -} - -bool -rclpy_action_server_goal_exists(py::capsule pyaction_server, py::object pygoal_info) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - destroy_ros_message_signature * destroy_ros_message = NULL; - rcl_action_goal_info_t * goal_info = static_cast( - rclpy_convert_from_py(pygoal_info.ptr(), &destroy_ros_message)); - if (!goal_info) { - throw py::error_already_set(); - } - - auto goal_info_ptr = std::unique_ptr( - goal_info, destroy_ros_message); - - return rcl_action_server_goal_exists(action_server, goal_info); -} - -rcl_action_goal_state_t -rclpy_action_goal_handle_get_status(py::capsule pygoal_handle) -{ - auto goal_handle = get_pointer( - pygoal_handle, "rcl_action_goal_handle_t"); - - rcl_action_goal_state_t status; - rcl_ret_t ret = rcl_action_goal_handle_get_status(goal_handle, &status); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get goal status"); - } - - return status; -} - -py::object -rclpy_action_process_cancel_request( - py::capsule pyaction_server, py::object pycancel_request, py::object pycancel_response_type) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - - destroy_ros_message_signature * destroy_cancel_request = NULL; - rcl_action_cancel_request_t * cancel_request = static_cast( - rclpy_convert_from_py(pycancel_request.ptr(), &destroy_cancel_request)); - if (!cancel_request) { - throw py::error_already_set(); - } - auto cancel_request_ptr = - std::unique_ptr( - cancel_request, destroy_cancel_request); - - rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); - rcl_ret_t ret = rcl_action_process_cancel_request( - action_server, cancel_request, &cancel_response); - - if (RCL_RET_OK != ret) { - std::string error_text{"Failed to process cancel request: "}; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_cancel_response_fini(&cancel_response); - if (RCL_RET_OK != ret) { - error_text += ". Also failed to cleanup response: "; - error_text += rcl_get_error_string().str; - rcl_reset_error(); - } - throw std::runtime_error(error_text); - } - - PyObject * pycancel_response = - rclpy_convert_to_py(&cancel_response.msg, pycancel_response_type.ptr()); - if (!pycancel_response) { - rcl_ret_t ignore = rcl_action_cancel_response_fini(&cancel_response); - (void) ignore; - throw py::error_already_set(); - } - py::object return_value = py::reinterpret_steal(pycancel_response); - - ret = rcl_action_cancel_response_fini(&cancel_response); - - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to finalize cancel response"); - } - return return_value; -} - -py::tuple -rclpy_action_expire_goals(py::capsule pyaction_server, int64_t max_num_goals) -{ - auto action_server = get_pointer(pyaction_server, "rcl_action_server_t"); - - auto expired_goals = - std::unique_ptr(new rcl_action_goal_info_t[max_num_goals]); - size_t num_expired; - rcl_ret_t ret = rcl_action_expire_goals( - action_server, expired_goals.get(), max_num_goals, &num_expired); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to expire goals"); - } - - // Get Python GoalInfo type - py::module pyaction_msgs_module = py::module::import("action_msgs.msg"); - py::object pygoal_info_class = pyaction_msgs_module.attr("GoalInfo"); - py::object pygoal_info_type = pygoal_info_class(); - - // Create a tuple of GoalInfo instances to return - py::tuple result_tuple(num_expired); - - for (size_t i = 0; i < num_expired; ++i) { - result_tuple[i] = py::reinterpret_steal( - rclpy_convert_to_py(&(expired_goals.get()[i]), pygoal_info_type.ptr())); - } - - return result_tuple; -} - - -py::object -rclpy_action_get_client_names_and_types_by_node( - py::capsule pynode, const char * remote_node_name, const char * remote_node_namespace) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_action_get_client_names_and_types_by_node( - node, - &allocator, - remote_node_name, - remote_node_namespace, - &names_and_types); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get action client names and type"); - } - - py::object pynames_and_types = py::reinterpret_steal( - rclpy_convert_to_py_names_and_types(&names_and_types)); - if (!rclpy_names_and_types_fini(&names_and_types)) { - throw py::error_already_set(); - } - return pynames_and_types; -} - -py::object -rclpy_action_get_server_names_and_types_by_node( - py::capsule pynode, const char * remote_node_name, const char * remote_node_namespace) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_action_get_server_names_and_types_by_node( - node, - &allocator, - remote_node_name, - remote_node_namespace, - &names_and_types); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get action server names and type"); - } - - py::object pynames_and_types = py::reinterpret_steal( - rclpy_convert_to_py_names_and_types(&names_and_types)); - if (!rclpy_names_and_types_fini(&names_and_types)) { - throw py::error_already_set(); - } - return pynames_and_types; -} - -py::object -rclpy_action_get_names_and_types(py::capsule pynode) -{ - rcl_node_t * node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_action_get_names_and_types(node, &allocator, &names_and_types); - if (RCL_RET_OK != ret) { - throw rclpy::RCLError("Failed to get action names and type"); - } - - py::object pynames_and_types = py::reinterpret_steal( - rclpy_convert_to_py_names_and_types(&names_and_types)); - if (!rclpy_names_and_types_fini(&names_and_types)) { - throw py::error_already_set(); - } - return pynames_and_types; -} - - -namespace rclpy -{ -void -define_action_api(py::module m) -{ - m.def( - "rclpy_action_destroy_entity", &rclpy_action_destroy_entity, - "Destroy a rclpy_action entity."); - m.def( - "rclpy_action_destroy_server_goal_handle", &rclpy_action_destroy_server_goal_handle, - "Destroy a ServerGoalHandle."); - m.def( - "rclpy_action_get_rmw_qos_profile", &rclpy_action_get_rmw_qos_profile, - "Get an action RMW QoS profile."); - m.def( - "rclpy_action_wait_set_add", &rclpy_action_wait_set_add, - "Add an action entitiy to a wait set."); - m.def( - "rclpy_action_wait_set_get_num_entities", &rclpy_action_wait_set_get_num_entities, - "Get the number of wait set entities for an action entitity."); - m.def( - "rclpy_action_wait_set_is_ready", &rclpy_action_wait_set_is_ready, - "Check if an action entity has any sub-entities ready in a wait set."); - m.def( - "rclpy_action_create_client", &rclpy_action_create_client, - "Create an action client."); - m.def( - "rclpy_action_create_server", &rclpy_action_create_server, - "Create an action server."); - m.def( - "rclpy_action_server_is_available", &rclpy_action_server_is_available, - "Check if an action server is available for a given client."); - m.def( - "rclpy_action_send_goal_request", &rclpy_action_send_goal_request, - "Send a goal request."); - m.def( - "rclpy_action_take_goal_request", &rclpy_action_take_goal_request, - "Take a goal request."); - m.def( - "rclpy_action_send_goal_response", &rclpy_action_send_goal_response, - "Send a goal response."); - m.def( - "rclpy_action_take_goal_response", &rclpy_action_take_goal_response, - "Take a goal response."); - m.def( - "rclpy_action_send_result_request", &rclpy_action_send_result_request, - "Send a result request."); - m.def( - "rclpy_action_take_result_request", &rclpy_action_take_result_request, - "Take a result request."); - m.def( - "rclpy_action_send_result_response", &rclpy_action_send_result_response, - "Send a result response."); - m.def( - "rclpy_action_take_result_response", &rclpy_action_take_result_response, - "Take a result response."); - m.def( - "rclpy_action_send_cancel_request", &rclpy_action_send_cancel_request, - "Send a cancel request."); - m.def( - "rclpy_action_take_cancel_request", &rclpy_action_take_cancel_request, - "Take a cancel request."); - m.def( - "rclpy_action_send_cancel_response", &rclpy_action_send_cancel_response, - "Send a cancel response."); - m.def( - "rclpy_action_take_cancel_response", &rclpy_action_take_cancel_response, - "Take a cancel response."); - m.def( - "rclpy_action_publish_feedback", &rclpy_action_publish_feedback, - "Publish a feedback message."); - m.def( - "rclpy_action_take_feedback", &rclpy_action_take_feedback, - "Take a feedback message."); - m.def( - "rclpy_action_publish_status", &rclpy_action_publish_status, - "Publish a status message."); - m.def( - "rclpy_action_take_status", &rclpy_action_take_status, - "Take a status message."); - m.def( - "rclpy_action_accept_new_goal", &rclpy_action_accept_new_goal, - "Accept a new goal using an action server."); - m.def( - "rclpy_action_notify_goal_done", &rclpy_action_notify_goal_done, - "Notify and action server that a goal has reached a terminal state."); - m.def( - "rclpy_action_update_goal_state", &rclpy_action_update_goal_state, - "Update a goal state."); - m.def( - "rclpy_action_goal_handle_is_active", &rclpy_action_goal_handle_is_active, - "Check if a goal is active."); - m.def( - "rclpy_action_server_goal_exists", &rclpy_action_server_goal_exists, - "Check if a goal being tracked by an action server."); - m.def( - "rclpy_action_goal_handle_get_status", &rclpy_action_goal_handle_get_status, - "Get the status of a goal."); - m.def( - "rclpy_action_process_cancel_request", &rclpy_action_process_cancel_request, - "Process a cancel request to determine what goals should be canceled."); - m.def( - "rclpy_action_expire_goals", &rclpy_action_expire_goals, - "Expire goals associated with an action server."); - m.def( - "rclpy_action_get_client_names_and_types_by_node", - &rclpy_action_get_client_names_and_types_by_node, - "Get action client names and types by node."); - m.def( - "rclpy_action_get_server_names_and_types_by_node", - &rclpy_action_get_server_names_and_types_by_node, - "Get action server names and types by node."); - m.def( - "rclpy_action_get_names_and_types", &rclpy_action_get_names_and_types, - "Get action names and types."); -} -} // namespace rclpy diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 2fbacb1ae..e191984b3 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -15,8 +15,11 @@ #include #include +#include -#include "action_api.hpp" +#include "action_client.hpp" +#include "action_goal_handle.hpp" +#include "action_server.hpp" #include "client.hpp" #include "clock.hpp" #include "context.hpp" @@ -25,7 +28,6 @@ #include "graph.hpp" #include "guard_condition.hpp" #include "handle_api.hpp" -#include "init.hpp" #include "logging.hpp" #include "logging_api.hpp" #include "names.hpp" @@ -57,6 +59,13 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { .value("SYSTEM_TIME", RCL_SYSTEM_TIME) .value("STEADY_TIME", RCL_STEADY_TIME); + py::enum_(m, "GoalEvent") + .value("EXECUTE", GOAL_EVENT_EXECUTE) + .value("CANCEL_GOAL", GOAL_EVENT_CANCEL_GOAL) + .value("SUCCEED", GOAL_EVENT_SUCCEED) + .value("ABORT", GOAL_EVENT_ABORT) + .value("CANCELED", GOAL_EVENT_CANCELED); + m.attr("RCL_DEFAULT_DOMAIN_ID") = py::int_(RCL_DEFAULT_DOMAIN_ID); py::enum_(m, "ClockChange") @@ -100,45 +109,13 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { py::register_exception( m, "InvalidHandle", PyExc_RuntimeError); - m.def( - "rclpy_init", &rclpy::init, - "Initialize RCL."); - m.def( - "rclpy_shutdown", &rclpy::shutdown, - "rclpy_shutdown."); - rclpy::define_client(m); - m.def( - "rclpy_context_get_domain_id", &rclpy::context_get_domain_id, - "Retrieves domain ID from init_options of context"); - m.def( - "rclpy_create_context", &rclpy::create_context, - "Create a capsule with an rcl_context_t instance"); - m.def( - "rclpy_ok", &rclpy::context_is_valid, - "Return true if the context is valid"); + rclpy::define_context(m); rclpy::define_duration(m); - m.def( - "rclpy_create_publisher", &rclpy::publisher_create, - "Create a Publisher"); - m.def( - "rclpy_get_publisher_logger_name", &rclpy::publisher_get_logger_name, - "Get the logger name associated with the node of a publisher."); - m.def( - "rclpy_publisher_get_subscription_count", &rclpy::publisher_get_subscription_count, - "Count subscribers from a publisher"); - m.def( - "rclpy_publisher_get_topic_name", &rclpy::publisher_get_topic_name, - "Get the resolved name(topic) of publisher"); - m.def( - "rclpy_publish", &rclpy::publisher_publish_message, - "Publish a message"); - m.def( - "rclpy_publish_raw", &rclpy::publisher_publish_raw, - "Publish a serialized message"); + rclpy::define_publisher(m); rclpy::define_service(m); @@ -148,64 +125,18 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { "rclpy_qos_check_compatible", &rclpy::qos_check_compatible, "Check if two QoS profiles are compatible."); + rclpy::define_action_client(m); + rclpy::define_action_goal_handle(m); + rclpy::define_action_server(m); m.def( - "rclpy_create_guard_condition", &rclpy::guard_condition_create, - "Create a general purpose guard condition"); - m.def( - "rclpy_trigger_guard_condition", &rclpy::guard_condition_trigger, - "Trigger a general purpose guard condition"); - + "rclpy_action_get_rmw_qos_profile", &rclpy::rclpy_action_get_rmw_qos_profile, + "Get an action RMW QoS profile."); + rclpy::define_guard_condition(m); rclpy::define_timer(m); - - m.def( - "rclpy_create_subscription", &rclpy::subscription_create, - "Create a Subscription"); - m.def( - "rclpy_get_subscription_logger_name", &rclpy::subscription_get_logger_name, - "Get the logger name associated with the node of a subscription"); - m.def( - "rclpy_get_subscription_topic_name", &rclpy::subscription_get_topic_name, - "Get the topic name of a subscription"); - m.def( - "rclpy_take", &rclpy::subscription_take_message, - "Take a message and its metadata from a subscription"); - + rclpy::define_subscription(m); rclpy::define_time_point(m); rclpy::define_clock(m); - - m.def( - "rclpy_get_zero_initialized_wait_set", &rclpy::get_zero_initialized_wait_set, - "rclpy_get_zero_initialized_wait_set."); - m.def( - "rclpy_wait_set_init", &rclpy::wait_set_init, - "rclpy_wait_set_init."); - m.def( - "rclpy_wait_set_clear_entities", &rclpy::wait_set_clear_entities, - "rclpy_wait_set_clear_entities."); - m.def( - "rclpy_wait_set_add_entity", &rclpy::wait_set_add_entity, - "rclpy_wait_set_add_entity."); - m.def( - "rclpy_wait_set_add_client", &rclpy::wait_set_add_client, - "Add a client to the wait set."); - m.def( - "rclpy_wait_set_add_service", &rclpy::wait_set_add_service, - "Add a service to the wait set."); - m.def( - "rclpy_wait_set_add_timer", &rclpy::wait_set_add_timer, - "Add a timer to the wait set."); - m.def( - "rclpy_wait_set_add_event", &rclpy::wait_set_add_event, - "Add an event to the wait set."); - m.def( - "rclpy_wait_set_is_ready", &rclpy::wait_set_is_ready, - "rclpy_wait_set_is_ready."); - m.def( - "rclpy_get_ready_entities", &rclpy::get_ready_entities, - "List non null entities in wait set."); - m.def( - "rclpy_wait", &rclpy::wait, - "rclpy_wait."); + rclpy::define_waitset(m); m.def( "rclpy_expand_topic_name", &rclpy::expand_topic_name, @@ -270,43 +201,9 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { "rclpy_deserialize", &rclpy::deserialize, "Deserialize a ROS message."); - m.def( - "rclpy_create_node", &rclpy::create_node, - "Create a Node."); - m.def( - "rclpy_node_get_fully_qualified_name", &rclpy::get_node_fully_qualified_name, - "Get the fully qualified name of node."); - m.def( - "rclpy_get_node_logger_name", &rclpy::get_node_logger_name, - "Get the logger name associated with a node."); - m.def( - "rclpy_get_node_name", &rclpy::get_node_name, - "Get the name of a node."); - m.def( - "rclpy_get_node_namespace", &rclpy::get_node_namespace, - "Get the namespace of a node."); - m.def( - "rclpy_get_node_names_and_namespaces", &rclpy::get_node_names_and_namespaces, - "Get node names and namespaces list from graph API."); - m.def( - "rclpy_get_node_names_and_namespaces_with_enclaves", - &rclpy::get_node_names_and_namespaces_with_enclaves, - "Get node names, namespaces, and enclaves list from graph API."); - - m.def( - "rclpy_count_publishers", &rclpy::get_count_publishers, - "Count publishers for a topic."); - - m.def( - "rclpy_count_subscribers", &rclpy::get_count_subscribers, - "Count subscribers for a topic."); - + rclpy::define_node(m); rclpy::define_qos_event(m); - m.def( - "rclpy_get_node_parameters", &rclpy::get_node_parameters, - "Get the initial parameters for a node from the command line."); - m.def( "rclpy_get_rmw_implementation_identifier", &rclpy::get_rmw_implementation_identifier, @@ -332,5 +229,4 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { rclpy::define_pycapsule_api(m); rclpy::define_handle_api(m); rclpy::define_logging_api(m); - rclpy::define_action_api(m); } diff --git a/rclpy/src/rclpy/action_api.hpp b/rclpy/src/rclpy/action_api.hpp deleted file mode 100644 index 6da5c7d3c..000000000 --- a/rclpy/src/rclpy/action_api.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCLPY__ACTION_API_HPP_ -#define RCLPY__ACTION_API_HPP_ - -#include - -namespace py = pybind11; - -namespace rclpy -{ -/// Define methods on a module for the action API -/** - * \param[in] module a pybind11 module to add the definition to - */ -void -define_action_api(py::module module); -} // namespace rclpy -#endif // RCLPY__ACTION_API_HPP_ diff --git a/rclpy/src/rclpy/action_client.cpp b/rclpy/src/rclpy/action_client.cpp new file mode 100644 index 000000000..6fac5bf81 --- /dev/null +++ b/rclpy/src/rclpy/action_client.cpp @@ -0,0 +1,321 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Include pybind11 before rclpy_common/handle.h includes Python.h +#include + +#include + +#include +#include + +#include "rclpy_common/common.h" +#include "rclpy_common/exceptions.hpp" +#include "rclpy_common/handle.h" + +#include "action_client.hpp" +#include "utils.hpp" + +namespace rclpy +{ + +void +ActionClient::destroy() +{ + rcl_action_client_.reset(); + node_.destroy(); +} + +ActionClient::ActionClient( + Node & node, + py::object pyaction_type, + const char * action_name, + const rmw_qos_profile_t & goal_service_qos, + const rmw_qos_profile_t & result_service_qos, + const rmw_qos_profile_t & cancel_service_qos, + const rmw_qos_profile_t & feedback_topic_qos, + const rmw_qos_profile_t & status_topic_qos) +: node_(node) +{ + rosidl_action_type_support_t * ts = + static_cast(rclpy_common_get_type_support( + pyaction_type.ptr())); + if (!ts) { + throw py::error_already_set(); + } + + rcl_action_client_options_t action_client_ops = rcl_action_client_get_default_options(); + + action_client_ops.goal_service_qos = goal_service_qos; + action_client_ops.result_service_qos = result_service_qos; + action_client_ops.cancel_service_qos = cancel_service_qos; + action_client_ops.feedback_topic_qos = feedback_topic_qos; + action_client_ops.status_topic_qos = status_topic_qos; + + rcl_action_client_ = std::shared_ptr( + new rcl_action_client_t, + [node](rcl_action_client_t * action_client) + { + // Intentionally capture node by value so shared_ptr can be transferred to copies + rcl_ret_t ret = rcl_action_client_fini(action_client, node.rcl_ptr()); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini publisher: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete action_client; + }); + + *rcl_action_client_ = rcl_action_get_zero_initialized_client(); + + rcl_ret_t ret = rcl_action_client_init( + rcl_action_client_.get(), + node_.rcl_ptr(), + ts, + action_name, + &action_client_ops); + if (RCL_RET_ACTION_NAME_INVALID == ret) { + std::string error_text{"Failed to create action client due to invalid topic name '"}; + error_text += action_name; + error_text += "' : "; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + throw py::value_error(error_text); + } + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to create action client: "}; + error_text += rcl_get_error_string().str; + rcl_reset_error(); + throw py::value_error(error_text); + } +} + +#define TAKE_SERVICE_RESPONSE(Type) \ + /* taken_msg is always destroyed in this function */ \ + auto taken_msg = create_from_py(pymsg_type); \ + rmw_request_id_t header; \ + rcl_ret_t ret = rcl_action_take_ ## Type ## _response( \ + rcl_action_client_.get(), &header, taken_msg.get()); \ + int64_t sequence = header.sequence_number; \ + /* Create the tuple to return */ \ + if (RCL_RET_ACTION_CLIENT_TAKE_FAILED == ret || RCL_RET_ACTION_SERVER_TAKE_FAILED == ret) { \ + return py::make_tuple(py::none(), py::none()); \ + } else if (RCL_RET_OK != ret) { \ + throw rclpy::RCLError("Failed to take " #Type); \ + } \ + return py::make_tuple(sequence, convert_to_py(taken_msg.get(), pymsg_type)); \ + +py::tuple +ActionClient::take_goal_response(py::object pymsg_type) +{ + TAKE_SERVICE_RESPONSE(goal) +} + +#define SEND_SERVICE_REQUEST(Type) \ + auto ros_request = convert_from_py(pyrequest); \ + int64_t sequence_number; \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _request( \ + rcl_action_client_.get(), ros_request.get(), &sequence_number); \ + if (RCL_RET_OK != ret) { \ + throw rclpy::RCLError("Failed to send " #Type " request"); \ + } \ + return sequence_number; + +int64_t +ActionClient::send_result_request(py::object pyrequest) +{ + SEND_SERVICE_REQUEST(result); +} + +py::tuple +ActionClient::take_cancel_response(py::object pymsg_type) +{ + TAKE_SERVICE_RESPONSE(cancel) +} + +#define TAKE_MESSAGE(Type) \ + auto taken_msg = create_from_py(pymsg_type); \ + rcl_ret_t ret = rcl_action_take_ ## Type(rcl_action_client_.get(), taken_msg.get()); \ + if (RCL_RET_OK != ret) { \ + if (RCL_RET_ACTION_CLIENT_TAKE_FAILED == ret) { \ + /* if take failed, just do nothing */ \ + return py::none(); \ + } \ + throw rclpy::RCLError("Failed to take " #Type " with an action client"); \ + } \ + return convert_to_py(taken_msg.get(), pymsg_type); + +py::object +ActionClient::take_feedback(py::object pymsg_type) +{ + TAKE_MESSAGE(feedback) +} + +py::object +ActionClient::take_status(py::object pymsg_type) +{ + TAKE_MESSAGE(status) +} + +int64_t +ActionClient::send_cancel_request(py::object pyrequest) +{ + SEND_SERVICE_REQUEST(cancel) +} + +int64_t +ActionClient::send_goal_request(py::object pyrequest) +{ + SEND_SERVICE_REQUEST(goal) +} + +py::tuple +ActionClient::take_result_response(py::object pymsg_type) +{ + TAKE_SERVICE_RESPONSE(result); +} + +py::tuple +ActionClient::get_num_entities() +{ + size_t num_subscriptions = 0u; + size_t num_guard_conditions = 0u; + size_t num_timers = 0u; + size_t num_clients = 0u; + size_t num_services = 0u; + + rcl_ret_t ret; + ret = rcl_action_client_wait_set_get_num_entities( + rcl_action_client_.get(), + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to get number of entities for 'rcl_action_client_t'"}; + throw rclpy::RCLError(error_text); + } + + return py::make_tuple( + num_subscriptions, num_guard_conditions, num_timers, + num_clients, num_services); +} + +bool +ActionClient::is_action_server_available() +{ + bool is_available = false; + rcl_ret_t ret = rcl_action_server_is_available( + node_.rcl_ptr(), rcl_action_client_.get(), &is_available); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to check if action server is available"); + } + return is_available; +} + +void +ActionClient::add_to_waitset(WaitSet & wait_set) +{ + rcl_ret_t ret = rcl_action_wait_set_add_action_client( + wait_set.rcl_ptr(), rcl_action_client_.get(), NULL, NULL); + if (RCL_RET_OK != ret) { + std::string error_text{"Failed to add 'rcl_action_client_t' to wait set"}; + throw rclpy::RCLError(error_text); + } +} + +py::tuple +ActionClient::is_ready(WaitSet & wait_set) +{ + bool is_feedback_ready = false; + bool is_status_ready = false; + bool is_goal_response_ready = false; + bool is_cancel_response_ready = false; + bool is_result_response_ready = false; + rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( + wait_set.rcl_ptr(), + rcl_action_client_.get(), + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get number of ready entities for action client"); + } + + py::tuple result_tuple(5); + result_tuple[0] = py::bool_(is_feedback_ready); + result_tuple[1] = py::bool_(is_status_ready); + result_tuple[2] = py::bool_(is_goal_response_ready); + result_tuple[3] = py::bool_(is_cancel_response_ready); + result_tuple[4] = py::bool_(is_result_response_ready); + return result_tuple; +} + +void +define_action_client(py::object module) +{ + py::class_>(module, "ActionClient") + .def( + py::init()) + .def_property_readonly( + "pointer", [](const ActionClient & action_client) { + return reinterpret_cast(action_client.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "take_goal_response", &ActionClient::take_goal_response, + "Take an action goal response.") + .def( + "send_result_request", &ActionClient::send_result_request, + "Send an action result request.") + .def( + "take_cancel_response", &ActionClient::take_cancel_response, + "Take an action cancel response.") + .def( + "take_feedback", &ActionClient::take_feedback, + "Take a feedback message from a given action client.") + .def( + "send_cancel_request", &ActionClient::send_cancel_request, + "Send an action cancel request.") + .def( + "send_goal_request", &ActionClient::send_goal_request, + "Send an action goal request.") + .def( + "take_result_response", &ActionClient::take_result_response, + "Take an action result response.") + .def( + "get_num_entities", &ActionClient::get_num_entities, + "Get the number of wait set entities that make up an action entity.") + .def( + "is_action_server_available", &ActionClient::is_action_server_available, + "Check if an action server is available for the given action client.") + .def( + "add_to_waitset", &ActionClient::add_to_waitset, + "Add an action entity to a wait set.") + .def( + "is_ready", &ActionClient::is_ready, + "Check if an action entity has any ready wait set entities.") + .def( + "take_status", &ActionClient::take_status, + "Take an action status response."); +} +} // namespace rclpy diff --git a/rclpy/src/rclpy/action_client.hpp b/rclpy/src/rclpy/action_client.hpp new file mode 100644 index 000000000..e8a24229c --- /dev/null +++ b/rclpy/src/rclpy/action_client.hpp @@ -0,0 +1,239 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLPY__ACTION_CLIENT_HPP_ +#define RCLPY__ACTION_CLIENT_HPP_ + +#include + +#include + +#include + +#include "destroyable.hpp" +#include "node.hpp" +#include "wait_set.hpp" + +namespace py = pybind11; + +namespace rclpy +{ +/** + * This class will create an action client for the given action name. + * This client will use the typesupport defined in the action module + * provided as pyaction_type to send messages. + */ +class ActionClient : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create an action client. + /* + * Raises ValueError if action name is invalid. + * Raises RuntimeError if the action client could not be created. + * + * \param[in] node Node to add the action client to. + * \param[in] pyaction_type Action module associated with the action client. + * \param[in] pyaction_name Python object containing the action name. + * \param[in] goal_service_qos rmw_qos_profile_t object for the goal service. + * \param[in] result_service_qos rmw_qos_profile_t object for the result service. + * \param[in] cancel_service_qos rmw_qos_profile_t object for the cancel service. + * \param[in] feedback_qos rmw_qos_profile_t object for the feedback subscriber. + * \param[in] status_qos rmw_qos_profile_t object for the status subscriber. + */ + ActionClient( + Node & node, + py::object pyaction_type, + const char * action_name, + const rmw_qos_profile_t & goal_service_qos, + const rmw_qos_profile_t & result_service_qos, + const rmw_qos_profile_t & cancel_service_qos, + const rmw_qos_profile_t & feedback_topic_qos, + const rmw_qos_profile_t & status_topic_qos); + + /// Take an action goal response. + /** + * Raises AttributeError if there is an issue parsing the pygoal_response_type. + * Raises RuntimeError if the underlying rcl library returns an error when taking the response. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pygoal_response_type An instance of the response message type to take. + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or + * \return (None, None) if there is a failure in the rcl API call. + */ + py::tuple + take_goal_response(py::object pymsg_type); + + /// Send an action result request. + /** + * Raises AttributeError if there is an issue parsing the pyresult_request. + * Raises RuntimeError if the underlying rcl library returns an error when sending the request. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pyresult_request The request message to send. + * \return sequence_number the index of the sent request + */ + int64_t + send_result_request(py::object pyrequest); + + /// Take an action cancel response. + /** + * Raises AttributeError if there is an issue parsing the pycancel_response_type. + * Raises RuntimeError if the underlying rcl library returns an error when taking the response. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pycancel_response_type An instance of the response message type to take. + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or + * \return (None, None) if there is a failure in the rcl API call. + */ + py::tuple + take_cancel_response(py::object pymsg_type); + + /// Send an action cancel request. + /** + * Raises AttributeError if there is an issue parsing the pycancel_request. + * Raises RuntimeError if the underlying rcl library returns an error when sending the request. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pycancel_request The request message to send. + * \return sequence_number the index of the sent request + */ + int64_t + send_cancel_request(py::object pyrequest); + + /// Take a feedback message from a given action client. + /** + * Raises AttributeError if there is an issue parsing the pyfeedback_type. + * Raises RuntimeError on failure while taking a feedback message. Note, this does not include + * the case where there are no messages available. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pyfeedback_type Instance of the feedback message type to take. + * \return Python message with all fields populated with received message, or + * \return None if there is nothing to take, or + * \return None if there is a failure in the rcl API call. + */ + py::object + take_feedback(py::object pymsg_type); + + /// Take a status message from a given action client. + /** + * Raises AttributeError if there is an issue parsing the pystatus_type. + * Raises RuntimeError on failure while taking a status message. Note, this does not include + * the case where there are no messages available. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pystatus_type Instance of the status message type to take. + * \return Python message with all fields populated with received message, or + * \return None if there is nothing to take, or + * \return None if there is a failure in the rcl API call. + */ + py::object + take_status(py::object pymsg_type); + + /// Send an action goal request. + /** + * Raises AttributeError if there is an issue parsing the pygoal_request. + * Raises RuntimeError on failure. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pygoal_request The request message to send. + * \return sequence_number PyLong object representing the index of the sent request + */ + int64_t + send_goal_request(py::object pyrequest); + + /// Take an action result response. + /** + * Raises AttributeError if there is an issue parsing the pyresult_response_type. + * Raises RuntimeError if the underlying rcl library returns an error when taking the response. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pyresult_response_type An instance of the response message type to take. + * \return 2-tuple (sequence number, received response), or + * \return 2-tuple (None, None) if there is no response, or + * \return (None, None) if there is a failure in the rcl API call. + */ + py::tuple + take_result_response(py::object pymsg_type); + + /// Get the number of wait set entities that make up an action entity. + /** + * \return Tuple containing the number of wait set entities: + * (num_subscriptions, + * num_guard_conditions, + * num_timers, + * num_clients, + * num_services) + */ + py::tuple + get_num_entities(); + + /// Check if an action server is available for the given action client. + /** + * Raises RuntimeError on failure. + * + * \return True if an action server is available, False otherwise. + */ + bool + is_action_server_available(); + + /// Check if an action entity has any ready wait set entities. + /** + * This must be called after waiting on the wait set. + * Raises RuntimeError on failure. + * + * \param[in] wait_set Capsule pointing to the wait set structure. + * \return A tuple of booleans representing the sub-entities ready: + * (is_feedback_ready, + * is_status_ready, + * is_goal_response_ready, + * is_cancel_response_ready, + * is_result_response_ready) + */ + py::tuple + is_ready(WaitSet & wait_set); + + /// Add an action entitiy to a wait set. + /** + * Raises RuntimeError on failure. + * \param[in] wait_set Capsule pointer to an rcl_wait_set_t. + */ + void + add_to_waitset(WaitSet & wait_set); + + /// Get rcl_action_client_t pointer + rcl_action_client_t * + rcl_ptr() const + { + return rcl_action_client_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + Node node_; + std::shared_ptr rcl_action_client_; +}; +/// Define a pybind11 wrapper for an rcl_time_point_t +/** + * \param[in] module a pybind11 module to add the definition to + */ +void define_action_client(py::object module); +} // namespace rclpy + +#endif // RCLPY__ACTION_CLIENT_HPP_ diff --git a/rclpy/src/rclpy/action_goal_handle.cpp b/rclpy/src/rclpy/action_goal_handle.cpp new file mode 100644 index 000000000..771cde2a1 --- /dev/null +++ b/rclpy/src/rclpy/action_goal_handle.cpp @@ -0,0 +1,121 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +#include "rclpy_common/common.h" +#include "rclpy_common/exceptions.hpp" + +#include "utils.hpp" +#include "action_goal_handle.hpp" + +namespace py = pybind11; + +namespace rclpy +{ +ActionGoalHandle::ActionGoalHandle( + rclpy::ActionServer & action_server, py::object pygoal_info_msg) +: action_server_(action_server) +{ + destroy_ros_message_signature * destroy_ros_message = NULL; + auto goal_info_msg = static_cast( + rclpy_convert_from_py(pygoal_info_msg.ptr(), &destroy_ros_message)); + + if (!goal_info_msg) { + throw py::error_already_set(); + } + + auto goal_info_msg_ptr = std::unique_ptr( + goal_info_msg, destroy_ros_message); + + auto rcl_handle = rcl_action_accept_new_goal( + action_server.rcl_ptr(), goal_info_msg); + if (!rcl_handle) { + throw rclpy::RCLError("Failed to accept new goal"); + } + + rcl_action_goal_handle_ = std::shared_ptr( + new rcl_action_goal_handle_t, + [](rcl_action_goal_handle_t * goal_handle) + { + rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Error destroying action goal handle: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete goal_handle; + }); + // Copy out goal handle since action server storage disappears when it is fini'd + *rcl_action_goal_handle_ = *rcl_handle; +} + +void +ActionGoalHandle::destroy() +{ + rcl_action_goal_handle_.reset(); + action_server_.destroy(); +} + +rcl_action_goal_state_t +ActionGoalHandle::get_status() +{ + rcl_action_goal_state_t status; + rcl_ret_t ret = rcl_action_goal_handle_get_status(rcl_action_goal_handle_.get(), &status); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get goal status"); + } + + return status; +} + +void +ActionGoalHandle::update_goal_state(rcl_action_goal_event_t event) +{ + rcl_ret_t ret = rcl_action_update_goal_state(rcl_action_goal_handle_.get(), event); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to update goal state"); + } +} + +void +define_action_goal_handle(py::module module) +{ + py::class_>( + module, "ActionGoalHandle") + .def(py::init()) + .def_property_readonly( + "pointer", [](const ActionGoalHandle & handle) { + return reinterpret_cast(handle.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "get_status", &ActionGoalHandle::get_status, + "Get the status of a goal.") + .def( + "update_goal_state", &ActionGoalHandle::update_goal_state, + "Update a goal state.") + .def( + "is_active", &ActionGoalHandle::is_active, + "Check if a goal is active."); +} +} // namespace rclpy diff --git a/rclpy/src/rclpy/action_goal_handle.hpp b/rclpy/src/rclpy/action_goal_handle.hpp new file mode 100644 index 000000000..bb9da8819 --- /dev/null +++ b/rclpy/src/rclpy/action_goal_handle.hpp @@ -0,0 +1,90 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLPY__ACTION_GOAL_HANDLE_HPP_ +#define RCLPY__ACTION_GOAL_HANDLE_HPP_ + +#include + +#include + +#include + +#include "action_server.hpp" +#include "destroyable.hpp" +#include "handle.hpp" + +namespace py = pybind11; + +namespace rclpy +{ + +class ActionServer; + +class ActionGoalHandle : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create an action goal handle + /** + * This function will create an action goal handle for the given info message from the action server. + * This action goal handle will use the typesupport defined in the service module + * provided as pysrv_type to send messages. + * + * Raises RCLError if the action goal handle could not be created + * + * \param[in] pyaction_server handle to the action server that is accepting the goal + * \param[in] pygoal_info_msg a message containing info about the goal being accepted + */ + ActionGoalHandle(rclpy::ActionServer & action_server, py::object pygoal_info_msg); + + ~ActionGoalHandle() = default; + + rcl_action_goal_state_t + get_status(); + + void + update_goal_state(rcl_action_goal_event_t event); + + /// Check if the goal is still active + bool + is_active() + { + return rcl_action_goal_handle_is_active(rcl_ptr()); + } + + /// Get rcl_action goal handle_t pointer + rcl_action_goal_handle_t * + rcl_ptr() const + { + return rcl_action_goal_handle_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + ActionServer action_server_; + std::shared_ptr rcl_action_goal_handle_; +}; + +/// Define a pybind11 wrapper for an rclpy::ActionGoalHandle +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +define_action_goal_handle(py::module module); +} // namespace rclpy + +#endif // RCLPY__ACTION_GOAL_HANDLE_HPP_ diff --git a/rclpy/src/rclpy/action_server.cpp b/rclpy/src/rclpy/action_server.cpp new file mode 100644 index 000000000..a0c1ee302 --- /dev/null +++ b/rclpy/src/rclpy/action_server.cpp @@ -0,0 +1,403 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Include pybind11 before rclpy_common/handle.h includes Python.h +#include + +#include +#include + +#include +#include + +#include "rclpy_common/common.h" +#include "rclpy_common/exceptions.hpp" +#include "rclpy_common/handle.h" + +#include "action_server.hpp" + +namespace rclpy +{ + +void +ActionServer::destroy() +{ + rcl_action_server_.reset(); + node_.destroy(); +} + +ActionServer::ActionServer( + Node & node, + const rclpy::Clock & rclpy_clock, + py::object pyaction_type, + const char * action_name, + const rmw_qos_profile_t & goal_service_qos, + const rmw_qos_profile_t & result_service_qos, + const rmw_qos_profile_t & cancel_service_qos, + const rmw_qos_profile_t & feedback_topic_qos, + const rmw_qos_profile_t & status_topic_qos, + double result_timeout) +: node_(node) +{ + rcl_clock_t * clock = rclpy_clock.rcl_ptr(); + + rosidl_action_type_support_t * ts = static_cast( + rclpy_common_get_type_support(pyaction_type.ptr())); + if (!ts) { + throw py::error_already_set(); + } + + rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); + + action_server_ops.goal_service_qos = goal_service_qos; + action_server_ops.result_service_qos = result_service_qos; + action_server_ops.cancel_service_qos = cancel_service_qos; + action_server_ops.feedback_topic_qos = feedback_topic_qos; + action_server_ops.status_topic_qos = status_topic_qos; + action_server_ops.result_timeout.nanoseconds = (rcl_duration_value_t)RCL_S_TO_NS(result_timeout); + + rcl_action_server_ = std::shared_ptr( + new rcl_action_server_t, + [node](rcl_action_server_t * action_server) + { + // Intentionally capture node by value so shared_ptr can be transferred to copies + rcl_ret_t ret = rcl_action_server_fini(action_server, node.rcl_ptr()); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini publisher: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete action_server; + }); + + *rcl_action_server_ = rcl_action_get_zero_initialized_server(); + + rcl_ret_t ret = rcl_action_server_init( + rcl_action_server_.get(), + node_.rcl_ptr(), + clock, + ts, + action_name, + &action_server_ops); + if (RCL_RET_ACTION_NAME_INVALID == ret) { + std::string error_text{"Failed to create action server due to invalid topic name '"}; + error_text += action_name; + error_text += "' : "; + throw py::value_error(append_rcl_error(error_text)); + } else if (RCL_RET_OK != ret) { + throw py::value_error(append_rcl_error("Failed to create action server")); + } +} + +#define TAKE_SERVICE_REQUEST(Type) \ + /* taken_msg is always destroyed in this function */ \ + auto taken_msg = create_from_py(pymsg_type); \ + rmw_request_id_t header; \ + rcl_ret_t ret = \ + rcl_action_take_ ## Type ## _request(rcl_action_server_.get(), &header, taken_msg.get()); \ + /* Create the tuple to return */ \ + if (ret == RCL_RET_ACTION_CLIENT_TAKE_FAILED || ret == RCL_RET_ACTION_SERVER_TAKE_FAILED) { \ + return py::make_tuple(py::none(), py::none()); \ + } else if (RCL_RET_OK != ret) { \ + throw rclpy::RCLError("Failed to take " #Type); \ + } \ + return py::make_tuple(header, convert_to_py(taken_msg.get(), pymsg_type)); \ + +py::tuple +ActionServer::take_goal_request(py::object pymsg_type) +{ + TAKE_SERVICE_REQUEST(goal) +} + +py::tuple +ActionServer::take_result_request(py::object pymsg_type) +{ + TAKE_SERVICE_REQUEST(result) +} + +#define SEND_SERVICE_RESPONSE(Type) \ + auto ros_response = convert_from_py(pyresponse); \ + rcl_ret_t ret = rcl_action_send_ ## Type ## _response( \ + rcl_action_server_.get(), header, ros_response.get()); \ + if (RCL_RET_OK != ret) { \ + throw rclpy::RCLError("Failed to send " #Type " response"); \ + } + +void +ActionServer::send_goal_response( + rmw_request_id_t * header, py::object pyresponse) +{ + SEND_SERVICE_RESPONSE(goal) +} + +void +ActionServer::send_result_response( + rmw_request_id_t * header, py::object pyresponse) +{ + SEND_SERVICE_RESPONSE(result) +} + +py::tuple +ActionServer::take_cancel_request(py::object pymsg_type) +{ + TAKE_SERVICE_REQUEST(cancel) +} + +void +ActionServer::send_cancel_response( + rmw_request_id_t * header, py::object pyresponse) +{ + SEND_SERVICE_RESPONSE(cancel) +} + +void +ActionServer::publish_feedback(py::object pymsg) +{ + auto ros_message = convert_from_py(pymsg); + rcl_ret_t ret = rcl_action_publish_feedback(rcl_action_server_.get(), ros_message.get()); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to publish feedback with an action server"); + } +} + +void +ActionServer::publish_status() +{ + rcl_action_goal_status_array_t status_message = + rcl_action_get_zero_initialized_goal_status_array(); + rcl_ret_t ret = rcl_action_get_goal_status_array(rcl_action_server_.get(), &status_message); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed get goal status array"); + } + + ret = rcl_action_publish_status(rcl_action_server_.get(), &status_message); + + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed publish goal status array"); + } +} + +void +ActionServer::notify_goal_done() +{ + rcl_ret_t ret = rcl_action_notify_goal_done(rcl_action_server_.get()); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to notfiy action server of goal done"); + } +} + +bool +ActionServer::goal_exists(py::object pygoal_info) +{ + auto goal_info = convert_from_py(pygoal_info); + rcl_action_goal_info_t * goal_info_type = static_cast(goal_info.get()); + return rcl_action_server_goal_exists(rcl_action_server_.get(), goal_info_type); +} + +py::tuple +ActionServer::get_num_entities() +{ + size_t num_subscriptions = 0u; + size_t num_guard_conditions = 0u; + size_t num_timers = 0u; + size_t num_clients = 0u; + size_t num_services = 0u; + + rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( + rcl_action_server_.get(), + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get number of entities for 'rcl_action_server_t'"); + } + + py::tuple result_tuple(5); + result_tuple[0] = py::int_(num_subscriptions); + result_tuple[1] = py::int_(num_guard_conditions); + result_tuple[2] = py::int_(num_timers); + result_tuple[3] = py::int_(num_clients); + result_tuple[4] = py::int_(num_services); + return result_tuple; +} + +py::tuple +ActionServer::is_ready(WaitSet & wait_set) +{ + bool is_goal_request_ready = false; + bool is_cancel_request_ready = false; + bool is_result_request_ready = false; + bool is_goal_expired = false; + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set.rcl_ptr(), + rcl_action_server_.get(), + &is_goal_request_ready, + &is_cancel_request_ready, + &is_result_request_ready, + &is_goal_expired); + + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get number of ready entities for action server"); + } + + py::tuple result_tuple(4); + result_tuple[0] = py::bool_(is_goal_request_ready); + result_tuple[1] = py::bool_(is_cancel_request_ready); + result_tuple[2] = py::bool_(is_result_request_ready); + result_tuple[3] = py::bool_(is_goal_expired); + return result_tuple; +} + +void +ActionServer::add_to_waitset(WaitSet & wait_set) +{ + rcl_ret_t ret = rcl_action_wait_set_add_action_server( + wait_set.rcl_ptr(), rcl_action_server_.get(), NULL); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to add 'rcl_action_server_t' to wait set"); + } +} + +py::object +ActionServer::process_cancel_request( + py::object pycancel_request, py::object pycancel_response_type) +{ + auto cancel_request = convert_from_py(pycancel_request); + rcl_action_cancel_request_t * cancel_request_tmp = static_cast( + cancel_request.get()); + + rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); + rcl_ret_t ret = rcl_action_process_cancel_request( + rcl_action_server_.get(), cancel_request_tmp, &cancel_response); + + if (RCL_RET_OK != ret) { + std::string error_text = append_rcl_error("Failed to process cancel request"); + ret = rcl_action_cancel_response_fini(&cancel_response); + if (RCL_RET_OK != ret) { + error_text = append_rcl_error(". Also failed to cleanup response"); + } + throw std::runtime_error(error_text); + } + + py::object return_value = convert_to_py(&cancel_response.msg, pycancel_response_type); + RCPPUTILS_SCOPE_EXIT( + { + ret = rcl_action_cancel_response_fini(&cancel_response); + + if (RCL_RET_OK != ret) { + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to finalize cancel response: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); + return return_value; +} + +py::tuple +ActionServer::expire_goals(int64_t max_num_goals) +{ + auto expired_goals = + std::unique_ptr(new rcl_action_goal_info_t[max_num_goals]); + size_t num_expired; + rcl_ret_t ret = rcl_action_expire_goals( + rcl_action_server_.get(), expired_goals.get(), max_num_goals, &num_expired); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to expire goals"); + } + + // Get Python GoalInfo type + py::module pyaction_msgs_module = py::module::import("action_msgs.msg"); + py::object pygoal_info_class = pyaction_msgs_module.attr("GoalInfo"); + py::object pygoal_info_type = pygoal_info_class(); + + // Create a tuple of GoalInfo instances to return + py::tuple result_tuple(num_expired); + + for (size_t i = 0; i < num_expired; ++i) { + result_tuple[i] = + convert_to_py(&(expired_goals.get()[i]), pygoal_info_type); + } + + return result_tuple; +} + +void +define_action_server(py::object module) +{ + py::class_>(module, "ActionServer") + .def( + py::init()) + .def_property_readonly( + "pointer", [](const ActionServer & action_server) { + return reinterpret_cast(action_server.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "take_goal_request", &ActionServer::take_goal_request, + "Take an action goal request.") + .def( + "send_goal_response", &ActionServer::send_goal_response, + "Send an action goal response.") + .def( + "send_result_response", &ActionServer::send_result_response, + "Send an action result response.") + .def( + "take_cancel_request", &ActionServer::take_cancel_request, + "Take an action cancel request.") + .def( + "take_result_request", &ActionServer::take_result_request, + "Take an action result request.") + .def( + "send_cancel_response", &ActionServer::send_cancel_response, + "Send an action cancel response.") + .def( + "publish_feedback", &ActionServer::publish_feedback, + " Publish a feedback message from a given action server.") + .def( + "publish_status", &ActionServer::publish_status, + "Publish a status message from a given action server.") + .def( + "notify_goal_done", &ActionServer::notify_goal_done, + "Notify goal is done.") + .def( + "goal_exists", &ActionServer::goal_exists, + "Check is a goal exists in the server.") + .def( + "process_cancel_request", &ActionServer::process_cancel_request, + "Process a cancel request") + .def( + "expire_goals", &ActionServer::expire_goals, + "Expired goals.") + .def( + "get_num_entities", &ActionServer::get_num_entities, + "Get the number of wait set entities that make up an action entity.") + .def( + "is_ready", &ActionServer::is_ready, + "Check if an action entity has any ready wait set entities.") + .def( + "add_to_waitset", &ActionServer::add_to_waitset, + "Add an action entitiy to a wait set."); +} + +} // namespace rclpy diff --git a/rclpy/src/rclpy/action_server.hpp b/rclpy/src/rclpy/action_server.hpp new file mode 100644 index 000000000..6482bbd06 --- /dev/null +++ b/rclpy/src/rclpy/action_server.hpp @@ -0,0 +1,258 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLPY__ACTION_SERVER_HPP_ +#define RCLPY__ACTION_SERVER_HPP_ + +#include + +#include + +#include + +#include "clock.hpp" +#include "destroyable.hpp" +#include "node.hpp" +#include "wait_set.hpp" + +namespace py = pybind11; + +namespace rclpy +{ +/* + * This class will create an action server for the given action name. + * This client will use the typesupport defined in the action module + * provided as pyaction_type to send messages. + */ +class ActionServer : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create an action server. + /** + * Raises AttributeError if action type is invalid + * Raises ValueError if action name is invalid + * Raises RuntimeError if the action server could not be created. + * + * \param[in] node Node to add the action server to. + * \param[in] rclpy_clock Clock use to create the action server. + * \param[in] pyaction_type Action module associated with the action server. + * \param[in] pyaction_name Python object containing the action name. + * \param[in] goal_service_qos rmw_qos_profile_t object for the goal service. + * \param[in] result_service_qos rmw_qos_profile_t object for the result service. + * \param[in] cancel_service_qos rmw_qos_profile_t object for the cancel service. + * \param[in] feedback_qos rmw_qos_profile_t object for the feedback subscriber. + * \param[in] status_qos rmw_qos_profile_t object for the status subscriber. + */ + ActionServer( + Node & node, + const rclpy::Clock & rclpy_clock, + py::object pyaction_type, + const char * action_name, + const rmw_qos_profile_t & goal_service_qos, + const rmw_qos_profile_t & result_service_qos, + const rmw_qos_profile_t & cancel_service_qos, + const rmw_qos_profile_t & feedback_topic_qos, + const rmw_qos_profile_t & status_topic_qos, + double result_timeout); + + /// Take an action goal request. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pymsg_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is an + * "rclpy.rmw_request_id_t" type, or + * \return 2-tuple (None, None) if there as no message to take + */ + py::tuple + take_goal_request(py::object pymsg_type); + + /// Send an action goal response. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] header Pointer to the message header. + * \param[in] pyresponse The response message to send. + */ + void + send_goal_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Send an action result response. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pyheader Pointer to the message header. + * \param[in] pyresponse The response message to send. + */ + void + send_result_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Take an action cancel request. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pymsg_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is an + * "rmw_request_id_t" type, or + * \return 2-tuple (None, None) if there as no message to take + */ + py::tuple + take_cancel_request(py::object pymsg_type); + + /// Take an action result request. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pymsg_type An instance of the type of request message to take. + * \return 2-tuple (header, received request message) where the header is an + * "rclpy.rmw_request_id_t" type, or + * \return 2-tuple (None, None) if there as no message to take + */ + py::tuple + take_result_request(py::object pymsg_type); + + /// Send an action cancel response. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure. + * + * \param[in] pyheader Pointer to the message header. + * \param[in] pyresponse The response message to send. + */ + void + send_cancel_response( + rmw_request_id_t * header, py::object pyresponse); + + /// Publish a feedback message from a given action server. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure while publishing a feedback message. + * + * \param[in] pymsg The feedback message to publish. + */ + void + publish_feedback(py::object pymsg); + + /// Publish a status message from a given action server. + /** + * Raises RCLError if an error occurs in rcl + * Raises RuntimeError on failure while publishing a status message. + */ + void + publish_status(); + + /// Notifies action server that a goal handle reached a terminal state. + /** + * Raises RCLError if an error occurs in rcl + */ + void + notify_goal_done(); + + /// Check if a goal is already being tracked by an action server. + /* + * Raises AttributeError if there is an issue parsing the pygoal_info. + * + * \param[in] pygoal_info the identifiers of goals that expired, or set to `NULL` if unused + * \return True if the goal exists, false otherwise. + */ + bool + goal_exists(py::object pygoal_info); + + /// Process a cancel request using an action server. + /** + * This is a non-blocking call. + * + * Raises RuntimeError on failure while publishing a status message. + * Raises RCLError if an error occurs in rcl + * + * \return the cancel response message + */ + py::object + process_cancel_request( + py::object pycancel_request, py::object pycancel_response_type); + + /// Expires goals associated with an action server. + py::tuple + expire_goals(int64_t max_num_goals); + + /// Get the number of wait set entities that make up an action entity. + /** + * Raises RCLError if an error occurs in rcl + * + * \return Tuple containing the number of wait set entities: + * (num_subscriptions, + * num_guard_conditions, + * num_timers, + * num_clients, + * num_services) + */ + py::tuple + get_num_entities(); + + /// Check if an action entity has any ready wait set entities. + /** + * This must be called after waiting on the wait set. + * Raises RuntimeError on failure. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pywait_set Capsule pointing to the wait set structure. + * \return A tuple of booleans representing ready sub-entities. + * (is_goal_request_ready, + * is_cancel_request_ready, + * is_result_request_ready, + * is_goal_expired) + */ + py::tuple + is_ready(WaitSet & wait_set); + + /// Add an action entitiy to a wait set. + /** + * Raises RuntimeError on failure. + * Raises RCLError if an error occurs in rcl + * + * \param[in] pywait_set Capsule pointer to an rcl_wait_set_t. + */ + void + add_to_waitset(WaitSet & wait_set); + + /// Get rcl_action_server_t pointer + rcl_action_server_t * + rcl_ptr() const + { + return rcl_action_server_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + Node node_; + std::shared_ptr rcl_action_server_; +}; +/// Define a pybind11 wrapper for an rcl_time_point_t +/** + * \param[in] module a pybind11 module to add the definition to + */ +void define_action_server(py::object module); +} // namespace rclpy + +#endif // RCLPY__ACTION_SERVER_HPP_ diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index 8587f527d..4cba6cd88 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -34,15 +34,13 @@ void Client::destroy() { rcl_client_.reset(); - node_handle_.reset(); + node_.destroy(); } Client::Client( - py::capsule pynode, py::object pysrv_type, const char * service_name, py::object pyqos_profile) -: node_handle_(std::make_shared(pynode)) + Node & node, py::object pysrv_type, const char * service_name, py::object pyqos_profile) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - auto srv_type = static_cast( rclpy_common_get_type_support(pysrv_type.ptr())); if (!srv_type) { @@ -59,11 +57,10 @@ Client::Client( // Create a client rcl_client_ = std::shared_ptr( PythonAllocator().allocate(1), - [this](rcl_client_t * client) + [node](rcl_client_t * client) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_client_fini(client, node); + // Intentionally capture node by value so shared_ptr can be transferred to copies + rcl_ret_t ret = rcl_client_fini(client, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -78,7 +75,7 @@ Client::Client( *rcl_client_ = rcl_get_zero_initialized_client(); rcl_ret_t ret = rcl_client_init( - rcl_client_.get(), node, srv_type, service_name, &client_ops); + rcl_client_.get(), node_.rcl_ptr(), srv_type, service_name, &client_ops); if (RCL_RET_OK != ret) { if (RCL_RET_SERVICE_NAME_INVALID == ret) { std::string error_text{"failed to create client due to invalid service name '"}; @@ -114,9 +111,8 @@ Client::send_request(py::object pyrequest) bool Client::service_server_is_available() { - auto node = node_handle_->cast("rcl_node_t"); bool is_ready; - rcl_ret_t ret = rcl_service_server_is_available(node, rcl_client_.get(), &is_ready); + rcl_ret_t ret = rcl_service_server_is_available(node_.rcl_ptr(), rcl_client_.get(), &is_ready); if (RCL_RET_OK != ret) { throw RCLError("failed to check service availability"); } @@ -155,8 +151,8 @@ Client::take_response(py::object pyresponse_type) void define_client(py::object module) { - py::class_(module, "Client") - .def(py::init()) + py::class_>(module, "Client") + .def(py::init()) .def_property_readonly( "pointer", [](const Client & client) { return reinterpret_cast(client.rcl_ptr()); diff --git a/rclpy/src/rclpy/client.hpp b/rclpy/src/rclpy/client.hpp index f9f5a9f34..1280bd099 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -22,13 +22,13 @@ #include #include "destroyable.hpp" -#include "handle.hpp" +#include "node.hpp" namespace py = pybind11; namespace rclpy { -class Client : public Destroyable +class Client : public Destroyable, public std::enable_shared_from_this { public: /// Create a client @@ -40,12 +40,12 @@ class Client : public Destroyable * Raises ValueError if the capsules are not the correct types * Raises RuntimeError if the client could not be created * - * \param[in] pynode Capsule pointing to the node to add the client to + * \param[in] node Node to add the client to * \param[in] pysrv_type Service module associated with the client * \param[in] service_name Python object containing the service name * \param[in] pyqos rmw_qos_profile_t object for this client */ - Client(py::capsule pynode, py::object pysrv_type, const char * service_name, py::object pyqos); + Client(Node & node, py::object pysrv_type, const char * service_name, py::object pyqos); ~Client() = default; @@ -91,8 +91,7 @@ class Client : public Destroyable destroy() override; private: - // TODO(sloretz) replace with std::shared_ptr when rclpy::Node exists - std::shared_ptr node_handle_; + Node node_; std::shared_ptr rcl_client_; }; diff --git a/rclpy/src/rclpy/clock.cpp b/rclpy/src/rclpy/clock.cpp index bf91c84c7..f74947997 100644 --- a/rclpy/src/rclpy/clock.cpp +++ b/rclpy/src/rclpy/clock.cpp @@ -181,7 +181,7 @@ void Clock::remove_clock_callback(py::object pyjump_handle) void define_clock(py::object module) { - py::class_(module, "Clock") + py::class_>(module, "Clock") .def(py::init()) .def_property_readonly( "pointer", [](const Clock & clock) { diff --git a/rclpy/src/rclpy/clock.hpp b/rclpy/src/rclpy/clock.hpp index 06bd5076b..37b8dbb75 100644 --- a/rclpy/src/rclpy/clock.hpp +++ b/rclpy/src/rclpy/clock.hpp @@ -31,7 +31,7 @@ namespace py = pybind11; namespace rclpy { -class Clock : public Destroyable +class Clock : public Destroyable, public std::enable_shared_from_this { public: /// Create a clock @@ -105,13 +105,7 @@ class Clock : public Destroyable void remove_clock_callback(py::object pyjump_handle); - /// Get rcl_client_t pointer - std::shared_ptr get_shared_ptr() - { - return rcl_clock_; - } - - /// Get rcl_client_t pointer + /// Get rcl_clock_t pointer rcl_clock_t * rcl_ptr() const { return rcl_clock_.get(); diff --git a/rclpy/src/rclpy/context.cpp b/rclpy/src/rclpy/context.cpp index 794c29a78..e1c155c45 100644 --- a/rclpy/src/rclpy/context.cpp +++ b/rclpy/src/rclpy/context.cpp @@ -20,103 +20,137 @@ #include #include -#include +#include +#include +#include #include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" #include "context.hpp" +#include "utils.hpp" namespace rclpy { -size_t -context_get_domain_id(py::capsule pycontext) +Context::Context(py::list pyargs, size_t domain_id) { - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); + rcl_context_ = std::shared_ptr( + new rcl_context_t, + [](rcl_context_t * context) + { + if (NULL != context->impl) { + rcl_ret_t ret; + if (rcl_context_is_valid(context)) { + // shutdown first, if still valid + ret = rcl_shutdown(context); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to shutdown rcl_context_t: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + rcl_reset_error(); + } + } + ret = rcl_context_fini(context); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini rcl_context_t: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + rcl_reset_error(); + } + } + delete context; + }); + *rcl_context_ = rcl_get_zero_initialized_context(); + + // turn the arguments into an array of C-style strings + std::vector arg_c_values(pyargs.size()); + for (size_t i = 0; i < pyargs.size(); ++i) { + // CPython owns const char * memory - no need to free it + arg_c_values[i] = PyUnicode_AsUTF8(pyargs[i].ptr()); + if (!arg_c_values[i]) { + throw py::error_already_set(); + } } - size_t domain_id; - rcl_ret_t ret = rcl_context_get_domain_id(context, &domain_id); + InitOptions init_options(rcl_get_default_allocator()); + + // Set domain id + rcl_ret_t ret = rcl_init_options_set_domain_id(&init_options.rcl_options, domain_id); if (RCL_RET_OK != ret) { - throw RCLError("Failed to get domain id"); + throw RCLError("failed to set domain id to init options"); } - return domain_id; + if (arg_c_values.size() > static_cast(std::numeric_limits::max())) { + throw std::range_error("Too many cli arguments"); + } + int argc = static_cast(arg_c_values.size()); + const char ** argv = argc > 0 ? &(arg_c_values[0]) : nullptr; + ret = rcl_init(argc, argv, &init_options.rcl_options, rcl_context_.get()); + if (RCL_RET_OK != ret) { + throw RCLError("failed to initialize rcl"); + } + + throw_if_unparsed_ros_args(pyargs, rcl_context_.get()->global_arguments); } void -_rclpy_context_handle_destructor(void * p) +Context::destroy() { - auto context = static_cast(p); - if (!context) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_context_handle_destructor failed to get pointer"); - return; - } - if (NULL != context->impl) { - rcl_ret_t ret; - if (rcl_context_is_valid(context)) { - // shutdown first, if still valid - ret = rcl_shutdown(context); - if (RCL_RET_OK != ret) { - fprintf( - stderr, - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to shutdown rcl_context_t (%d) during PyCapsule destructor: %s\n", - ret, - rcl_get_error_string().str); - rcl_reset_error(); - } - } - ret = rcl_context_fini(context); - if (RCL_RET_OK != ret) { - fprintf( - stderr, - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini rcl_context_t (%d) during PyCapsule destructor: %s\n", - ret, - rcl_get_error_string().str); - rcl_reset_error(); - } - } - PyMem_FREE(context); + rcl_context_.reset(); } -py::capsule -create_context() +size_t +Context::get_domain_id() { - auto context = static_cast(PyMem_Malloc(sizeof(rcl_context_t))); - if (!context) { - throw std::bad_alloc(); - } - *context = rcl_get_zero_initialized_context(); - PyObject * capsule = rclpy_create_handle_capsule( - context, "rcl_context_t", _rclpy_context_handle_destructor); - if (!capsule) { - throw py::error_already_set(); + size_t domain_id; + rcl_ret_t ret = rcl_context_get_domain_id(rcl_context_.get(), &domain_id); + if (RCL_RET_OK != ret) { + throw RCLError("Failed to get domain id"); } - return py::reinterpret_steal(capsule); + + return domain_id; } -/// Status of the the client library -/** - * \return True if rcl is running properly, False otherwise - */ bool -context_is_valid(py::capsule pycontext) +Context::ok() { - auto context = static_cast(rclpy_handle_get_pointer_from_capsule( - pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); + return rcl_context_is_valid(rcl_context_.get()); +} + +void +Context::shutdown() +{ + rcl_ret_t ret = rcl_shutdown(rcl_context_.get()); + if (RCL_RET_OK != ret) { + throw RCLError("failed to shutdown"); } +} - return rcl_context_is_valid(context); +void define_context(py::object module) +{ + py::class_>(module, "Context") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Context & context) { + return reinterpret_cast(context.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "get_domain_id", &Context::get_domain_id, + "Retrieves domain id from init_options of context.") + .def( + "ok", &Context::ok, + "Status of the the client library") + .def( + "shutdown", &Context::shutdown, + "Shutdown context"); } + } // namespace rclpy diff --git a/rclpy/src/rclpy/context.hpp b/rclpy/src/rclpy/context.hpp index f6f4181a1..ed00992ef 100644 --- a/rclpy/src/rclpy/context.hpp +++ b/rclpy/src/rclpy/context.hpp @@ -17,36 +17,91 @@ #include +#include + +#include +#include + +#include "destroyable.hpp" +#include "rclpy_common/exceptions.hpp" + namespace py = pybind11; namespace rclpy { -/// Retrieves domain id from init_options of context -/** - * \param[in] pycontext Capsule containing rcl_context_t - * \return domain id - */ -size_t -context_get_domain_id(py::capsule pycontext); - -/// Create a capsule with an rcl_context_t instance. -/** - * The returned context is zero-initialized for use with rclpy_init(). - * - * Raises MemoryError if allocating memory fails. - * Raises RuntimeError if creating the context fails. - * - * \return capsule with the rcl_context_t instance - */ -py::capsule -create_context(); - -/// Status of the the client library -/** - * \return True if rcl is running properly, False otherwise - */ -bool -context_is_valid(py::capsule context); +struct InitOptions +{ + explicit InitOptions(rcl_allocator_t allocator) + { + rcl_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&rcl_options, allocator); + if (RCL_RET_OK != ret) { + throw RCLError("Failed to initialize init options"); + } + } + + ~InitOptions() + { + rcl_ret_t ret = rcl_init_options_fini(&rcl_options); + if (RCL_RET_OK != ret) { + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini rcl_init_options_t in destructor: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + rcl_reset_error(); + } + } + + rcl_init_options_t rcl_options; +}; + +class Context : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create a Context instance. + /** + * Raises MemoryError if allocating memory fails. + * Raises RuntimeError if creating the context fails. + * + * \param[in] pyargs List of command line arguments + * \param[in] domain_id domain id to be set in this context + */ + Context(py::list pyargs, size_t domain_id); + + /// Retrieves domain id from init_options of context + /** + * \param[in] pycontext Capsule containing rcl_context_t + * \return domain id + */ + size_t + get_domain_id(); + + /// Status of the the client library + /** + * \return True if rcl is running properly, False otherwise + */ + bool + ok(); + + void + shutdown(); + + /// Get rcl_context_t pointer + rcl_context_t * rcl_ptr() const + { + return rcl_context_.get(); + } + + /// Force an early destruction of this object + void destroy() override; + +private: + std::shared_ptr rcl_context_; +}; + +/// Define a pybind11 wrapper for an rclpy::Service +void define_context(py::object module); } // namespace rclpy #endif // RCLPY__CONTEXT_HPP_ diff --git a/rclpy/src/rclpy/destroyable.cpp b/rclpy/src/rclpy/destroyable.cpp index fa6ced3a1..2865983e8 100644 --- a/rclpy/src/rclpy/destroyable.cpp +++ b/rclpy/src/rclpy/destroyable.cpp @@ -19,8 +19,17 @@ #include "destroyable.hpp" #include "rclpy_common/exceptions.hpp" + namespace rclpy { +Destroyable::Destroyable(const Destroyable &) +{ + // When a destroyable is copied, it does not matter if someone asked + // to destroy the original. The copy has its own lifetime. + use_count = 0; + please_destroy_ = false; +} + void Destroyable::enter() { @@ -53,6 +62,10 @@ Destroyable::destroy() void Destroyable::destroy_when_not_in_use() { + if (please_destroy_) { + // already asked to destroy + return; + } please_destroy_ = true; if (0u == use_count) { destroy(); @@ -62,7 +75,7 @@ Destroyable::destroy_when_not_in_use() void define_destroyable(py::object module) { - py::class_(module, "Destroyable") + py::class_>(module, "Destroyable") .def("__enter__", &Destroyable::enter) .def("__exit__", &Destroyable::exit) .def( diff --git a/rclpy/src/rclpy/destroyable.hpp b/rclpy/src/rclpy/destroyable.hpp index 333db0916..92d6b0ad3 100644 --- a/rclpy/src/rclpy/destroyable.hpp +++ b/rclpy/src/rclpy/destroyable.hpp @@ -25,6 +25,12 @@ namespace rclpy class Destroyable { public: + /// Default constructor + Destroyable() = default; + + /// Copy constructor + Destroyable(const Destroyable & other); + /// Context manager __enter__ - block destruction void enter(); diff --git a/rclpy/src/rclpy/graph.cpp b/rclpy/src/rclpy/graph.cpp index b63c8f366..b9af8fa28 100644 --- a/rclpy/src/rclpy/graph.cpp +++ b/rclpy/src/rclpy/graph.cpp @@ -36,19 +36,13 @@ namespace rclpy py::list graph_get_publisher_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t publisher_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_publisher_names_and_types_by_node( - node, &allocator, no_demangle, node_name.c_str(), + node.rcl_ptr(), &allocator, no_demangle, node_name.c_str(), node_namespace.c_str(), &publisher_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { @@ -75,19 +69,13 @@ graph_get_publisher_names_and_types_by_node( py::list graph_get_subscriber_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t subscriber_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_subscriber_names_and_types_by_node( - node, &allocator, no_demangle, node_name.c_str(), + node.rcl_ptr(), &allocator, no_demangle, node_name.c_str(), node_namespace.c_str(), &subscriber_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { @@ -114,18 +102,13 @@ graph_get_subscriber_names_and_types_by_node( py::list graph_get_service_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace) + Node & node, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_service_names_and_types_by_node( - node, &allocator, node_name.c_str(), node_namespace.c_str(), &service_names_and_types); + node.rcl_ptr(), &allocator, node_name.c_str(), node_namespace.c_str(), + &service_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { throw NodeNameNonExistentError( @@ -151,18 +134,12 @@ graph_get_service_names_and_types_by_node( py::list graph_get_client_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace) + Node & node, std::string node_name, std::string node_namespace) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t client_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_get_client_names_and_types_by_node( - node, &allocator, node_name.c_str(), node_namespace.c_str(), &client_names_and_types); + node.rcl_ptr(), &allocator, node_name.c_str(), node_namespace.c_str(), &client_names_and_types); if (RCL_RET_OK != ret) { if (RCL_RET_NODE_NAME_NON_EXISTENT == ret) { throw NodeNameNonExistentError( @@ -187,18 +164,12 @@ graph_get_client_names_and_types_by_node( } py::list -graph_get_topic_names_and_types(py::capsule pynode, bool no_demangle) +graph_get_topic_names_and_types(Node & node, bool no_demangle) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = - rcl_get_topic_names_and_types(node, &allocator, no_demangle, &topic_names_and_types); + rcl_get_topic_names_and_types(node.rcl_ptr(), &allocator, no_demangle, &topic_names_and_types); if (RCL_RET_OK != ret) { throw RCLError("failed to get topic names and types"); } @@ -220,17 +191,12 @@ graph_get_topic_names_and_types(py::capsule pynode, bool no_demangle) } py::list -graph_get_service_names_and_types(py::capsule pynode) +graph_get_service_names_and_types(Node & node) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_names_and_types_t service_names_and_types = rcl_get_zero_initialized_names_and_types(); rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_get_service_names_and_types(node, &allocator, &service_names_and_types); + rcl_ret_t ret = rcl_get_service_names_and_types( + node.rcl_ptr(), &allocator, &service_names_and_types); if (RCL_RET_OK != ret) { throw RCLError("failed to get service names and types"); } @@ -259,18 +225,12 @@ typedef rcl_ret_t (* rcl_get_info_by_topic_func_t)( py::list _get_info_by_topic( - py::capsule pynode, + Node & node, const char * topic_name, bool no_mangle, const char * type, rcl_get_info_by_topic_func_t rcl_get_info_by_topic) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcutils_allocator_t allocator = rcutils_get_default_allocator(); rcl_topic_endpoint_info_array_t info_array = rcl_get_zero_initialized_topic_endpoint_info_array(); @@ -287,7 +247,8 @@ _get_info_by_topic( } }); - rcl_ret_t ret = rcl_get_info_by_topic(node, &allocator, topic_name, no_mangle, &info_array); + rcl_ret_t ret = rcl_get_info_by_topic( + node.rcl_ptr(), &allocator, topic_name, no_mangle, &info_array); if (RCL_RET_OK != ret) { if (RCL_RET_UNSUPPORTED == ret) { throw NotImplementedError( @@ -304,19 +265,19 @@ _get_info_by_topic( py::list graph_get_publishers_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle) + Node & node, const char * topic_name, bool no_mangle) { return _get_info_by_topic( - pynode, topic_name, no_mangle, "publishers", + node, topic_name, no_mangle, "publishers", rcl_get_publishers_info_by_topic); } py::list graph_get_subscriptions_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle) + Node & node, const char * topic_name, bool no_mangle) { return _get_info_by_topic( - pynode, topic_name, no_mangle, "subscriptions", + node, topic_name, no_mangle, "subscriptions", rcl_get_subscriptions_info_by_topic); } diff --git a/rclpy/src/rclpy/graph.hpp b/rclpy/src/rclpy/graph.hpp index 05ca47604..22967626f 100644 --- a/rclpy/src/rclpy/graph.hpp +++ b/rclpy/src/rclpy/graph.hpp @@ -22,6 +22,8 @@ #include #include +#include "node.hpp" + namespace py = pybind11; namespace rclpy @@ -29,11 +31,10 @@ namespace rclpy /// Get topic names and types for which a remote node has publishers. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node Node to get publisher topic names and types. * \param[in] no_demangle Whether to demangle topic names following ROS * conventions or not. * \param[in] node_name Name of the remote node to query. @@ -45,16 +46,15 @@ namespace rclpy */ py::list graph_get_publisher_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace); /// Get topic names and types for which a remote node has subscribers. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if the remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node node to get subscriber topic names and types * \param[in] no_demangle Whether to demangle topic names following ROS * conventions or not. * \param[in] node_name Name of the remote node to query. @@ -66,16 +66,15 @@ graph_get_publisher_names_and_types_by_node( */ py::list graph_get_subscriber_names_and_types_by_node( - py::capsule pynode, bool no_demangle, + Node & node, bool no_demangle, std::string node_name, std::string node_namespace); /// Get service names and types for which a remote node has servers. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if the remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node Node to get service topic names and types * \param[in] node_name Name of the remote node to query. * \param[in] node_namespace Namespace of the remote node to query. * \return List of tuples, where the first element of each tuple is the service @@ -85,15 +84,14 @@ graph_get_subscriber_names_and_types_by_node( */ py::list graph_get_service_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace); + Node & node, std::string node_name, std::string node_namespace); /// Get service names and types for which a remote node has clients. /** - * Raises ValueError if pynode is not a node capsule * Raises NodeNameNonExistentError if the remote node was not found * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node. + * \param[in] node node to get client topic names and types * \param[in] node_name Name of the remote node to query. * \param[in] node_namespace Namespace of the remote node to query. * \return List of tuples, where the first element of each tuple is the service @@ -103,14 +101,13 @@ graph_get_service_names_and_types_by_node( */ py::list graph_get_client_names_and_types_by_node( - py::capsule pynode, std::string node_name, std::string node_namespace); + Node & node, std::string node_name, std::string node_namespace); /// Get all topic names and types in the ROS graph. /** - * Raises ValueError if pynode is not a node capsule * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to query the ROS graph. + * \param[in] node node to get topic names and types * \param[in] no_demangle Whether to demangle topic names following ROS * conventions or not. * \return List of tuples, where the first element of each tuple is the topic @@ -119,21 +116,20 @@ graph_get_client_names_and_types_by_node( * \see rcl_get_topic_names_and_types */ py::list -graph_get_topic_names_and_types(py::capsule pynode, bool no_demangle); +graph_get_topic_names_and_types(Node & node, bool no_demangle); /// Get all service names and types in the ROS graph. /** - * Raises ValueError if pynode is not a node capsule * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to query the ROS graph. + * \param[in] node Node to get all topic service names and types * \return List of tuples, where the first element of each tuple is the service * name (string) and the second element is a list of service types (list of * strings). * \see rcl_get_service_names_and_types */ py::list -graph_get_service_names_and_types(py::capsule pynode); +graph_get_service_names_and_types(Node & node); /// Return a list of publishers on a given topic. /** @@ -143,7 +139,7 @@ graph_get_service_names_and_types(py::capsule pynode); * Raises NotImplementedError if the call is not supported by RMW * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to get the namespace from. + * \param[in] node Node to get topic publisher info * \param[in] topic_name the topic name to get the publishers for. * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, * otherwise it should be a valid ROS topic name. @@ -151,7 +147,7 @@ graph_get_service_names_and_types(py::capsule pynode); */ py::list graph_get_publishers_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle); + Node & node, const char * topic_name, bool no_mangle); /// Return a list of subscriptions on a given topic. /** @@ -161,7 +157,7 @@ graph_get_publishers_info_by_topic( * Raises NotImplementedError if the call is not supported by RMW * Raises RCLError if there is an rcl error * - * \param[in] pynode Capsule pointing to the node to get the namespace from. + * \param[in] node node to get topic subscriber info * \param[in] topic_name the topic name to get the subscriptions for. * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, * otherwise it should be a valid ROS topic name. @@ -169,7 +165,7 @@ graph_get_publishers_info_by_topic( */ py::list graph_get_subscriptions_info_by_topic( - py::capsule pynode, const char * topic_name, bool no_mangle); + Node & node, const char * topic_name, bool no_mangle); } // namespace rclpy diff --git a/rclpy/src/rclpy/guard_condition.cpp b/rclpy/src/rclpy/guard_condition.cpp index d5f6e43f6..4c41cd4c8 100644 --- a/rclpy/src/rclpy/guard_condition.cpp +++ b/rclpy/src/rclpy/guard_condition.cpp @@ -31,89 +31,66 @@ namespace rclpy { -/// Handle destructor for guard condition -static void -_rclpy_destroy_guard_condition(void * p) +GuardCondition::GuardCondition(Context & context) +: context_(context) { - auto gc = static_cast(p); - if (!gc) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_guard_condition got NULL pointer"); - return; - } + rcl_guard_condition_ = std::shared_ptr( + new rcl_guard_condition_t, + [](rcl_guard_condition_t * guard_condition) + { + rcl_ret_t ret = rcl_guard_condition_fini(guard_condition); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini guard condition: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete guard_condition; + }); + + *rcl_guard_condition_ = rcl_get_zero_initialized_guard_condition(); + rcl_guard_condition_options_t gc_options = rcl_guard_condition_get_default_options(); - rcl_ret_t ret = rcl_guard_condition_fini(gc); + rcl_ret_t ret = rcl_guard_condition_init( + rcl_guard_condition_.get(), context.rcl_ptr(), gc_options); if (RCL_RET_OK != ret) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "Failed to fini guard condition: %s", - rcl_get_error_string().str); + throw RCLError("failed to create guard_condition"); } - PyMem_Free(gc); } -py::capsule -guard_condition_create(py::capsule pycontext) +void +GuardCondition::destroy() { - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - - // Use smart pointer to make sure memory is free'd on error - auto deleter = [](rcl_guard_condition_t * ptr) {_rclpy_destroy_guard_condition(ptr);}; - auto gc = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_guard_condition_t))), - deleter); - if (!gc) { - throw std::bad_alloc(); - } - - *gc = rcl_get_zero_initialized_guard_condition(); - rcl_guard_condition_options_t gc_options = rcl_guard_condition_get_default_options(); - - rcl_ret_t ret = rcl_guard_condition_init(gc.get(), context, gc_options); - if (ret != RCL_RET_OK) { - throw RCLError("failed to create guard_condition"); - } - - PyObject * pygc_c = - rclpy_create_handle_capsule(gc.get(), "rcl_guard_condition_t", _rclpy_destroy_guard_condition); - if (!pygc_c) { - throw py::error_already_set(); - } - auto pygc = py::reinterpret_steal(pygc_c); - // pygc now owns the rcl_guard_contition_t - gc.release(); - - auto gc_handle = static_cast(pygc); - auto context_handle = static_cast(pycontext); - _rclpy_handle_add_dependency(gc_handle, context_handle); - if (PyErr_Occurred()) { - _rclpy_handle_dec_ref(gc_handle); - throw py::error_already_set(); - } - - return pygc; + rcl_guard_condition_.reset(); + context_.destroy(); } void -guard_condition_trigger(py::capsule pygc) +GuardCondition::trigger_guard_condition() { - auto gc = static_cast( - rclpy_handle_get_pointer_from_capsule(pygc.ptr(), "rcl_guard_condition_t")); - if (!gc) { - throw py::error_already_set(); - } + rcl_ret_t ret = rcl_trigger_guard_condition(rcl_guard_condition_.get()); - rcl_ret_t ret = rcl_trigger_guard_condition(gc); - - if (ret != RCL_RET_OK) { + if (RCL_RET_OK != ret) { throw RCLError("failed to trigger guard condition"); } } + +void define_guard_condition(py::object module) +{ + py::class_>(module, "GuardCondition") + .def(py::init()) + .def_property_readonly( + "pointer", [](const GuardCondition & guard_condition) { + return reinterpret_cast(guard_condition.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "trigger_guard_condition", &GuardCondition::trigger_guard_condition, + "Trigger a general purpose guard condition") + .def( + "pycapsule", &GuardCondition::pycapsule, + "Return a pycapsule object with the guardcondition pointer"); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/guard_condition.hpp b/rclpy/src/rclpy/guard_condition.hpp index 4a61c9be9..d77f9f0a8 100644 --- a/rclpy/src/rclpy/guard_condition.hpp +++ b/rclpy/src/rclpy/guard_condition.hpp @@ -17,31 +17,75 @@ #include +#include +#include + +#include + +#include "context.hpp" +#include "destroyable.hpp" +#include "rclpy_common/exceptions.hpp" +#include "rclpy_common/handle.h" +#include "utils.hpp" + namespace py = pybind11; namespace rclpy { /// Create a general purpose guard condition -/** - * A successful call will return a Capsule with the pointer of the created - * rcl_guard_condition_t * structure - * - * Raises RuntimeError if initializing the guard condition fails - * - * \return a guard condition capsule - */ -py::capsule -guard_condition_create(py::capsule pycontext); - -/// Trigger a general purpose guard condition -/** - * Raises ValueError if pygc is not a guard condition capsule - * Raises RCLError if the guard condition could not be triggered - * - * \param[in] pygc Capsule pointing to guard condtition - */ -void -guard_condition_trigger(py::capsule pygc); +class GuardCondition : public Destroyable, public std::enable_shared_from_this +{ +public: + /** + * Raises RuntimeError if initializing the guard condition fails + */ + explicit GuardCondition(Context & context); + + /// Trigger a general purpose guard condition + /** + * Raises ValueError if pygc is not a guard condition capsule + * Raises RCLError if the guard condition could not be triggered + */ + void + trigger_guard_condition(); + + /// Get rcl_guard_condition_t pointer + rcl_guard_condition_t * rcl_ptr() const + { + return rcl_guard_condition_.get(); + } + + // TODO(ahcorde): Remove the pycapsule method when #728 is in + /// Return a pycapsule object to be able to handle the signal in C. + py::capsule + pycapsule() const + { + PyObject * capsule = rclpy_create_handle_capsule( + rcl_guard_condition_.get(), "rcl_guard_condition_t", _rclpy_destroy_guard_condition); + if (!capsule) { + throw py::error_already_set(); + } + return py::reinterpret_steal(capsule); + } + + /// Force an early destruction of this object + void destroy() override; + +private: + Context context_; + std::shared_ptr rcl_guard_condition_; + + /// Handle destructor for guard condition + static void + _rclpy_destroy_guard_condition(void * p) + { + (void)p; + // Empty destructor, the class should take care of the lifecycle. + } +}; + +/// Define a pybind11 wrapper for an rclpy::Service +void define_guard_condition(py::object module); } // namespace rclpy #endif // RCLPY__GUARD_CONDITION_HPP_ diff --git a/rclpy/src/rclpy/init.cpp b/rclpy/src/rclpy/init.cpp deleted file mode 100644 index 73019815c..000000000 --- a/rclpy/src/rclpy/init.cpp +++ /dev/null @@ -1,154 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// Include pybind11 before rclpy_common/handle.h includes Python.h -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "rclpy_common/handle.h" - -#include "rclpy_common/exceptions.hpp" - -#include "init.hpp" - -namespace rclpy -{ -struct InitOptions -{ - explicit InitOptions(rcl_allocator_t allocator) - { - rcl_options = rcl_get_zero_initialized_init_options(); - rcl_ret_t ret = rcl_init_options_init(&rcl_options, allocator); - if (RCL_RET_OK != ret) { - throw RCLError("Failed to initialize init options"); - } - } - - ~InitOptions() - { - rcl_ret_t ret = rcl_init_options_fini(&rcl_options); - if (RCL_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini rcl_init_options_t in destructor:"); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); - rcl_reset_error(); - } - } - - rcl_init_options_t rcl_options; -}; - -void -init(py::list pyargs, py::capsule pycontext, size_t domain_id) -{ - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - - // turn the arguments into an array of C-style strings - std::vector arg_c_values(pyargs.size()); - for (size_t i = 0; i < pyargs.size(); ++i) { - // CPython owns const char * memory - no need to free it - arg_c_values[i] = PyUnicode_AsUTF8(pyargs[i].ptr()); - if (!arg_c_values[i]) { - throw py::error_already_set(); - } - } - - InitOptions init_options(rcl_get_default_allocator()); - - // Set domain id - rcl_ret_t ret = rcl_init_options_set_domain_id(&init_options.rcl_options, domain_id); - if (RCL_RET_OK != ret) { - throw RCLError("failed to set domain id to init options"); - } - - if (arg_c_values.size() > static_cast(std::numeric_limits::max())) { - throw std::range_error("Too many cli arguments"); - } - int argc = static_cast(arg_c_values.size()); - const char ** argv = argc > 0 ? &(arg_c_values[0]) : nullptr; - ret = rcl_init(argc, argv, &init_options.rcl_options, context); - if (RCL_RET_OK != ret) { - throw RCLError("failed to initialize rcl"); - } - - throw_if_unparsed_ros_args(pyargs, context->global_arguments); -} - -void -shutdown(py::capsule pycontext) -{ - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - - rcl_ret_t ret = rcl_shutdown(context); - if (RCL_RET_OK != ret) { - throw RCLError("failed to shutdown"); - } -} - -void -throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args) -{ - int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(&rcl_args); - - if (unparsed_ros_args_count < 0) { - throw std::runtime_error("failed to count unparsed arguments"); - } - if (0 == unparsed_ros_args_count) { - return; - } - - rcl_allocator_t allocator = rcl_get_default_allocator(); - - int * unparsed_indices_c = nullptr; - rcl_ret_t ret = rcl_arguments_get_unparsed_ros(&rcl_args, allocator, &unparsed_indices_c); - if (RCL_RET_OK != ret) { - throw RCLError("failed to get unparsed arguments"); - } - - auto deallocator = [&](int ptr[]) {allocator.deallocate(ptr, allocator.state);}; - auto unparsed_indices = std::unique_ptr( - unparsed_indices_c, deallocator); - - py::list unparsed_args; - for (int i = 0; i < unparsed_ros_args_count; ++i) { - int index = unparsed_indices_c[i]; - if (index < 0 || static_cast(index) >= pyargs.size()) { - throw std::runtime_error("got invalid unparsed ROS arg index"); - } - unparsed_args.append(pyargs[index]); - } - - throw UnknownROSArgsError(static_cast(py::repr(unparsed_args))); -} -} // namespace rclpy diff --git a/rclpy/src/rclpy/init.hpp b/rclpy/src/rclpy/init.hpp deleted file mode 100644 index e5c4f829d..000000000 --- a/rclpy/src/rclpy/init.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCLPY__INIT_HPP_ -#define RCLPY__INIT_HPP_ - -#include - -#include - -#include -#include - -namespace py = pybind11; - -namespace rclpy -{ -/// Initialize rcl with default options, ignoring parameters -/** - * Raises RCLError if rcl could not be initialized - * Raises UnknownROSArgsError if any CLI arguments could not be parsed - * Raises RuntimeError if an internal error happens - */ -void -init(py::list pyargs, py::capsule pycontext, size_t domain_id); - -/// Request shutdown of the client library -/** - * Raises RCLError if the library could not be shutdown - */ -void -shutdown(py::capsule pycontext); - -/// Throw UnparsedROSArgsError with a message saying which args are unparsed. -void -throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args); -} // namespace rclpy - -#endif // RCLPY__INIT_HPP_ diff --git a/rclpy/src/rclpy/logging.cpp b/rclpy/src/rclpy/logging.cpp index f95fb8503..f6d642149 100644 --- a/rclpy/src/rclpy/logging.cpp +++ b/rclpy/src/rclpy/logging.cpp @@ -62,18 +62,13 @@ rclpy_thread_safe_logging_output_handler( } void -logging_configure(py::object pycontext) +logging_configure(Context & context) { - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } rcl_allocator_t allocator = rcl_get_default_allocator(); rclpy::LoggingGuard scoped_logging_guard; rcl_ret_t ret = rcl_logging_configure_with_output_handler( - &context->global_arguments, + &context.rcl_ptr()->global_arguments, &allocator, rclpy_thread_safe_logging_output_handler); if (RCL_RET_OK != ret) { diff --git a/rclpy/src/rclpy/logging.hpp b/rclpy/src/rclpy/logging.hpp index c5152df8f..f97c9cb34 100644 --- a/rclpy/src/rclpy/logging.hpp +++ b/rclpy/src/rclpy/logging.hpp @@ -19,6 +19,8 @@ #include +#include "context.hpp" + namespace py = pybind11; namespace rclpy @@ -40,7 +42,7 @@ class LoggingGuard * \param[in] pycontext a context instance to use to retrieve global CLI arguments */ void -logging_configure(py::object pycontext); +logging_configure(Context & _context); /// Finalize rcl logging void diff --git a/rclpy/src/rclpy/names.cpp b/rclpy/src/rclpy/names.cpp index 5f4fdf23a..e265af6ce 100644 --- a/rclpy/src/rclpy/names.cpp +++ b/rclpy/src/rclpy/names.cpp @@ -209,23 +209,17 @@ expand_topic_name(const char * topic, const char * node_name, const char * node_ } std::string -remap_topic_name(py::capsule pynode, const char * topic_name) +remap_topic_name(Node & node, const char * topic_name) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - // Get the node options - const rcl_node_options_t * node_options = rcl_node_get_options(node); + const rcl_node_options_t * node_options = rcl_node_get_options(node.rcl_ptr()); if (nullptr == node_options) { throw RCLError("failed to get node options"); } const rcl_arguments_t * global_args = nullptr; if (node_options->use_global_arguments) { - global_args = &(node->context->global_arguments); + global_args = &(node.rcl_ptr()->context->global_arguments); } char * output_cstr = nullptr; @@ -233,8 +227,8 @@ remap_topic_name(py::capsule pynode, const char * topic_name) &(node_options->arguments), global_args, topic_name, - rcl_node_get_name(node), - rcl_node_get_namespace(node), + rcl_node_get_name(node.rcl_ptr()), + rcl_node_get_namespace(node.rcl_ptr()), node_options->allocator, &output_cstr); if (RCL_RET_OK != ret) { @@ -256,22 +250,16 @@ remap_topic_name(py::capsule pynode, const char * topic_name) } std::string -resolve_name(py::capsule pynode, const char * topic_name, bool only_expand, bool is_service) +resolve_name(Node & node, const char * topic_name, bool only_expand, bool is_service) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const rcl_node_options_t * node_options = rcl_node_get_options(node); + const rcl_node_options_t * node_options = rcl_node_get_options(node.rcl_ptr()); if (nullptr == node_options) { throw RCLError("failed to get node options"); } char * output_cstr = nullptr; rcl_ret_t ret = rcl_node_resolve_name( - node, + node.rcl_ptr(), topic_name, node_options->allocator, is_service, diff --git a/rclpy/src/rclpy/names.hpp b/rclpy/src/rclpy/names.hpp index e79e33440..6ae90e822 100644 --- a/rclpy/src/rclpy/names.hpp +++ b/rclpy/src/rclpy/names.hpp @@ -19,6 +19,8 @@ #include +#include "node.hpp" + namespace py = pybind11; namespace rclpy @@ -94,26 +96,26 @@ expand_topic_name(const char * topic, const char * node_name, const char * node_ * Raises ValueError if the capsule is not the correct type * Raises RCLError if an unexpected error happens * - * \param[in] pynode Capsule pointing to the node + * \param[in] Node node to remap the topic name * \param[in] topic_name topic string to be remapped * \return remapped topic name */ std::string -remap_topic_name(py::capsule pynode, const char * topic_name); +remap_topic_name(Node & node, const char * topic_name); /// Expand and remap a topic name /** * Raises ValueError if the capsule is not the correct type * Raises RCLError if an unexpected error happens * - * \param[in] pynode Capsule pointing to the node + * \param[in] node node to expand and remap a topic name * \param[in] topic_name topic string to be remapped * \param[in] only_expand when `false`, remapping rules are ignored * \param[in] is_service `true` for service names, `false` for topic names * \return expanded and remapped topic name */ std::string -resolve_name(py::capsule pynode, const char * topic_name, bool only_expand, bool is_service); +resolve_name(Node & node, const char * topic_name, bool only_expand, bool is_service); } // namespace rclpy #endif // RCLPY__NAMES_HPP_ diff --git a/rclpy/src/rclpy/node.cpp b/rclpy/src/rclpy/node.cpp index 43d233012..c41138c92 100644 --- a/rclpy/src/rclpy/node.cpp +++ b/rclpy/src/rclpy/node.cpp @@ -15,6 +15,7 @@ // Include pybind11 before rclpy_common/handle.h includes Python.h #include +#include #include #include #include @@ -34,24 +35,16 @@ #include "rclpy_common/exceptions.hpp" #include "rclpy_common/handle.h" -#include "init.hpp" #include "logging.hpp" #include "node.hpp" #include "utils.hpp" - namespace rclpy { const char * -get_node_fully_qualified_name(py::capsule pynode) +Node::get_fully_qualified_name() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * fully_qualified_node_name = rcl_node_get_fully_qualified_name(node); + const char * fully_qualified_node_name = rcl_node_get_fully_qualified_name(rcl_node_.get()); if (!fully_qualified_node_name) { throw RCLError("Fully qualified name not set"); } @@ -60,15 +53,9 @@ get_node_fully_qualified_name(py::capsule pynode) } const char * -get_node_logger_name(py::capsule pynode) +Node::logger_name() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * node_logger_name = rcl_node_get_logger_name(node); + const char * node_logger_name = rcl_node_get_logger_name(rcl_node_.get()); if (!node_logger_name) { throw RCLError("Logger name not set"); } @@ -77,15 +64,9 @@ get_node_logger_name(py::capsule pynode) } const char * -get_node_name(py::capsule pynode) +Node::get_node_name() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * node_name = rcl_node_get_name(node); + const char * node_name = rcl_node_get_name(rcl_node_.get()); if (!node_name) { throw RCLError("Node name not set"); } @@ -94,15 +75,9 @@ get_node_name(py::capsule pynode) } const char * -get_node_namespace(py::capsule pynode) +Node::get_namespace() { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - - const char * node_namespace = rcl_node_get_namespace(node); + const char * node_namespace = rcl_node_get_namespace(rcl_node_.get()); if (!node_namespace) { throw RCLError("Node namespace not set"); } @@ -111,16 +86,10 @@ get_node_namespace(py::capsule pynode) } size_t -get_count_publishers(py::capsule pynode, const char * topic_name) +Node::get_count_publishers(const char * topic_name) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - size_t count = 0; - rcl_ret_t ret = rcl_count_publishers(node, topic_name, &count); + rcl_ret_t ret = rcl_count_publishers(rcl_node_.get(), topic_name, &count); if (RCL_RET_OK != ret) { throw RCLError("Error in rcl_count_publishers"); } @@ -129,16 +98,10 @@ get_count_publishers(py::capsule pynode, const char * topic_name) } size_t -get_count_subscribers(py::capsule pynode, const char * topic_name) +Node::get_count_subscribers(const char * topic_name) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - size_t count = 0; - rcl_ret_t ret = rcl_count_subscribers(node, topic_name, &count); + rcl_ret_t ret = rcl_count_subscribers(rcl_node_.get(), topic_name, &count); if (RCL_RET_OK != ret) { throw RCLError("Error in rcl_count_subscribers"); } @@ -147,14 +110,8 @@ get_count_subscribers(py::capsule pynode, const char * topic_name) } py::list -get_node_names_impl(py::capsule pynode, bool get_enclaves) +Node::get_names_impl(bool get_enclaves) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - rcl_allocator_t allocator = rcl_get_default_allocator(); rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); @@ -163,10 +120,10 @@ get_node_names_impl(py::capsule pynode, bool get_enclaves) rcl_ret_t ret = RCL_RET_OK; if (get_enclaves) { ret = rcl_get_node_names_with_enclaves( - node, allocator, &node_names, &node_namespaces, &enclaves); + rcl_node_.get(), allocator, &node_names, &node_namespaces, &enclaves); } else { ret = rcl_get_node_names( - node, allocator, &node_names, &node_namespaces); + rcl_node_.get(), allocator, &node_names, &node_namespaces); } if (RCL_RET_OK != ret) { throw RCLError("Failed to get node names"); @@ -176,29 +133,32 @@ get_node_names_impl(py::capsule pynode, bool get_enclaves) { rcutils_ret_t fini_ret = rcutils_string_array_fini(&node_names); if (RCUTILS_RET_OK != fini_ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini node names during error handling: "); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini node names during error handling: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); rcl_reset_error(); } fini_ret = rcutils_string_array_fini(&node_namespaces); if (RCUTILS_RET_OK != fini_ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini node namespaces during error handling: "); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini node namespaces during error handling: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); rcl_reset_error(); } fini_ret = rcutils_string_array_fini(&enclaves); if (RCUTILS_RET_OK != fini_ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "[rclpy|" RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__LINE__) "]: " - "failed to fini enclaves string array during error handling: "); - RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); - RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, + "[rclpy| %s : %s ]: failed to fini enclaves string array during error handling: %s", + RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); rcl_reset_error(); } }); @@ -221,15 +181,15 @@ get_node_names_impl(py::capsule pynode, bool get_enclaves) } py::list -get_node_names_and_namespaces(py::capsule pynode) +Node::get_node_names_and_namespaces() { - return get_node_names_impl(pynode, false); + return get_names_impl(false); } py::list -get_node_names_and_namespaces_with_enclaves(py::capsule pynode) +Node::get_node_names_and_namespaces_with_enclaves() { - return get_node_names_impl(pynode, true); + return get_names_impl(true); } /// Create an rclpy.parameter.Parameter from an rcl_variant_t @@ -362,22 +322,16 @@ _parse_param_overrides( } py::dict -get_node_parameters(py::object pyparameter_cls, py::capsule pynode) +Node::get_parameters(py::object pyparameter_cls) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - py::dict params_by_node_name; py::object parameter_type_cls = pyparameter_cls.attr("Type"); - const rcl_node_options_t * node_options = rcl_node_get_options(node); + const rcl_node_options_t * node_options = rcl_node_get_options(rcl_node_.get()); if (node_options->use_global_arguments) { _parse_param_overrides( - &(node->context->global_arguments), pyparameter_cls, + &(rcl_node_.get()->context->global_arguments), pyparameter_cls, parameter_type_cls, params_by_node_name); } @@ -385,7 +339,7 @@ get_node_parameters(py::object pyparameter_cls, py::capsule pynode) &(node_options->arguments), pyparameter_cls, parameter_type_cls, params_by_node_name); - const char * node_fqn = rcl_node_get_fully_qualified_name(node); + const char * node_fqn = rcl_node_get_fully_qualified_name(rcl_node_.get()); if (!node_fqn) { throw RCLError("failed to get node fully qualified name"); } @@ -409,49 +363,22 @@ get_node_parameters(py::object pyparameter_cls, py::capsule pynode) return node_params; } -/// Handle destructor for node -static -void -_rclpy_destroy_node(void * p) +void Node::destroy() { - rclpy::LoggingGuard scoped_logging_guard; - auto node = static_cast(p); - if (!node) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_node got a NULL pointer"); - return; - } - - rcl_ret_t ret = rcl_node_fini(node); - if (RCL_RET_OK != ret) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "Failed to fini node: %s", - rcl_get_error_string().str); - } - PyMem_Free(node); + rcl_node_.reset(); + context_.destroy(); } -py::capsule -create_node( +Node::Node( const char * node_name, const char * namespace_, - py::capsule pycontext, + Context & context, py::object pycli_args, bool use_global_arguments, bool enable_rosout) +: context_(context) { rcl_ret_t ret; - - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - rcl_arguments_t arguments = rcl_get_zero_initialized_arguments(); // turn the arguments into an array of C-style strings @@ -503,14 +430,23 @@ create_node( throw_if_unparsed_ros_args(pyargs, arguments); - auto deleter = [](rcl_node_t * ptr) {_rclpy_destroy_node(ptr);}; - auto node = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_node_t))), - deleter); - if (!node) { - throw std::bad_alloc(); - } - *node = rcl_get_zero_initialized_node(); + rcl_node_ = std::shared_ptr( + new rcl_node_t, + [](rcl_node_t * node) + { + rcl_ret_t ret = rcl_node_fini(node); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini node: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete node; + }); + + *rcl_node_ = rcl_get_zero_initialized_node(); rcl_node_options_t options = rcl_node_get_default_options(); options.use_global_arguments = use_global_arguments; options.arguments = arguments; @@ -518,7 +454,8 @@ create_node( { rclpy::LoggingGuard scoped_logging_guard; - ret = rcl_node_init(node.get(), node_name, namespace_, context, &options); + ret = rcl_node_init( + rcl_node_.get(), node_name, namespace_, context.rcl_ptr(), &options); } if (RCL_RET_BAD_ALLOC == ret) { @@ -534,22 +471,107 @@ create_node( if (RCL_RET_OK != ret) { throw RCLError("error creating node"); } +} + +py::list +Node::get_action_client_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace) +{ + rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_action_get_client_names_and_types_by_node( + rcl_node_.get(), + &allocator, + remote_node_name, + remote_node_namespace, + &names_and_types); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get action client names and type"); + } + + return convert_to_py_names_and_types(&names_and_types); +} - PyObject * pynode_c = rclpy_create_handle_capsule(node.get(), "rcl_node_t", _rclpy_destroy_node); - if (!pynode_c) { - throw py::error_already_set(); +py::list +Node::get_action_server_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace) +{ + rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_action_get_server_names_and_types_by_node( + rcl_node_.get(), + &allocator, + remote_node_name, + remote_node_namespace, + &names_and_types); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get action server names and type"); } - auto pynode = py::reinterpret_steal(pynode_c); - // pynode now owns the rcl_node_t - node.release(); - - auto node_handle = static_cast(pynode); - auto context_handle = static_cast(pycontext); - _rclpy_handle_add_dependency(node_handle, context_handle); - if (PyErr_Occurred()) { - throw py::error_already_set(); + + return convert_to_py_names_and_types(&names_and_types); +} + +py::list +Node::get_action_names_and_types() +{ + rcl_names_and_types_t names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_action_get_names_and_types(rcl_node_.get(), &allocator, &names_and_types); + if (RCL_RET_OK != ret) { + throw rclpy::RCLError("Failed to get action names and type"); } - return pynode; + return convert_to_py_names_and_types(&names_and_types); +} + +void +define_node(py::object module) +{ + py::class_>(module, "Node") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Node & node) { + return reinterpret_cast(node.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "get_fully_qualified_name", &Node::get_fully_qualified_name, + "Get the fully qualified name of the node.") + .def( + "logger_name", &Node::logger_name, + "Get the name of the logger associated with a node.") + .def( + "get_node_name", &Node::get_node_name, + "Get the name of a node.") + .def( + "get_namespace", &Node::get_namespace, + "Get the namespace of a node.") + .def( + "get_count_publishers", &Node::get_count_publishers, + "Returns the count of all the publishers known for that topic in the entire ROS graph.") + .def( + "get_count_subscribers", &Node::get_count_subscribers, + "Returns the count of all the subscribers known for that topic in the entire ROS graph.") + .def( + "get_node_names_and_namespaces", &Node::get_node_names_and_namespaces, + "Get the list of nodes discovered by the provided node") + .def( + "get_node_names_and_namespaces_with_enclaves", + &Node::get_node_names_and_namespaces_with_enclaves, + "Get the list of nodes discovered by the provided node, with their respective enclaves.") + .def( + "get_action_client_names_and_types_by_node", + &Node::get_action_client_names_and_types_by_node, + "Get action client names and types by node.") + .def( + "get_action_server_names_and_types_by_node", + &Node::get_action_server_names_and_types_by_node, + "Get action server names and types by node.") + .def( + "get_action_names_and_types", &Node::get_action_names_and_types, + "Get action names and types.") + .def( + "get_parameters", &Node::get_parameters, + "Get a list of parameters for the current node"); } } // namespace rclpy diff --git a/rclpy/src/rclpy/node.hpp b/rclpy/src/rclpy/node.hpp index e09a463b3..fb6eeb289 100644 --- a/rclpy/src/rclpy/node.hpp +++ b/rclpy/src/rclpy/node.hpp @@ -17,157 +17,189 @@ #include +#include // rcl_names_and_types_t + +#include + +#include "context.hpp" +#include "destroyable.hpp" + namespace py = pybind11; namespace rclpy { -/// Get the fully qualified name of the node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if name is not set - * - * \param[in] pynode Capsule pointing to the node - * \return String containing the fully qualified name of the node - */ -const char * -get_node_fully_qualified_name(py::capsule pynode); - -/// Get the name of the logger associated with a node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if logger name is not set - * - * \param[in] pynode Capsule pointing to the node to get the logger name of - * \return logger_name - */ -const char * -get_node_logger_name(py::capsule pynode); - -/// Get the name of a node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if name is not set - * - * \param[in] pynode Capsule pointing to the node to get the name from - * \return name of the node - */ -const char * -get_node_name(py::capsule pynode); - -/// Get the namespace of a node. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if namespace is not set - * - * \param[in] pynode Capsule pointing to the node to get the namespace from - * \return namespace - */ -const char * -get_node_namespace(py::capsule pynode); - -/// Returns the count of all the publishers known for that topic in the entire ROS graph. -/* - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if an error occurs in rcl - * - * \param[in] pynode pynode Capsule pointing to a node - * \param[in] topic_name Name of the topic to count the number of publishers - * \return the count of all the publishers known for that topic in the entire ROS graph. - */ -size_t -get_count_publishers(py::capsule pynode, const char * topic_name); - -/// Returns the count of all the subscribers known for that topic in the entire ROS graph -/* - * - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if an error occurs in rcl - * - * \param[in] pynode pynode Capsule pointing to a node - * \param[in] topic_name Name of the topic to count the number of subscribers - * \return the count of all the subscribers known for that topic in the entire ROS graph - */ -size_t -get_count_subscribers(py::capsule pynode, const char * topic_name); - -/// Get the list of nodes discovered by the provided node -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if the names are unavailable. - * - * \param[in] module the Python module this function is part of - * \param[in] pynode Capsule pointing to the node - * \param[in] get_enclaves specifies if the output includes the enclaves names - * or not - * \return Python list of tuples, containing: - * node name, node namespace, and - * enclave if `get_enclaves` is true. - */ -py::list -get_node_names_impl(py::capsule pynode, bool get_enclaves); - -/// Get the list of nodes discovered by the provided node -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if the names are unavailable. - * - * \param[in] pynode Capsule pointing to the node - * \return Python list of tuples where each tuple contains the two strings: - * the node name and node namespace - */ -py::list -get_node_names_and_namespaces(py::capsule pynode); - -/// Get the list of nodes discovered by the provided node, with their respective enclaves. -/** - * Raises ValueError if pynode is not a node capsule - * Raises RCLError if the names are unavailable. - * - * \param[in] pynode Capsule pointing to the node - * \return Python list of tuples where each tuple contains three strings: - * node name, node namespace, and enclave. - */ -py::list -get_node_names_and_namespaces_with_enclaves(py::capsule pynode); - -/// Get a list of parameters for the current node -/** - * Raises ValueError if the argument is not a node handle - * Raises RCLError if any rcl function call fails - * Raises AttributeError if pyparameter_cls doesn't have a 'Type' attribute - * Raises RuntimeError if assumptions about rcl structures are violated - * - * \param[in] pyparameter_cls The rclpy.parameter.Parameter class object. - * \param[in] node_capsule Capsule pointing to the node handle - * \return A dict mapping parameter names to rclpy.parameter.Parameter (may be empty). - */ -py::dict -get_node_parameters(py::object pyparameter_cls, py::capsule pynode); - -/// Create a node -/** - * Raises ValueError if the node name or namespace is invalid - * Raises RCLError if the node could not be initialized for an unexpected reason - * Raises RCLInvalidROSArgsError if the given CLI arguments could not be parsed - * Raises UnknownROSArgsError if there are unknown CLI arguments given - * Raises MemoryError if memory could not be allocated for the node - * - * \param[in] node_name name of the node to be created - * \param[in] namespace namespace for the node - * \param[in] pycontext Capsule for an rcl_context_t - * \param[in] pycli_args a sequence of command line arguments for just this node, or None - * \param[in] use_global_arguments if true then the node will also use cli arguments on pycontext - * \param[in] enable rosout if true then enable rosout logging - * \return Capsule of the pointer to the created rcl_node_t * structure - */ -py::capsule -create_node( - const char * node_name, - const char * namespace_, - py::capsule pycontext, - py::object pycli_args, - bool use_global_arguments, - bool enable_rosout); -} // namespace rclpy +class Node : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create a node + /** + * Raises ValueError if the node name or namespace is invalid + * Raises RCLError if the node could not be initialized for an unexpected reason + * Raises RCLInvalidROSArgsError if the given CLI arguments could not be parsed + * Raises UnknownROSArgsError if there are unknown CLI arguments given + * Raises MemoryError if memory could not be allocated for the node + * + * \param[in] node_name name of the node to be created + * \param[in] namespace namespace for the node + * \param[in] context Context + * \param[in] pycli_args a sequence of command line arguments for just this node, or None + * \param[in] use_global_arguments if true then the node will also use cli arguments on context + * \param[in] enable rosout if true then enable rosout logging + */ + Node( + const char * node_name, + const char * namespace_, + Context & context, + py::object pycli_args, + bool use_global_arguments, + bool enable_rosout); + + /// Get the fully qualified name of the node. + /** + * Raises RCLError if name is not set + * + * \return String containing the fully qualified name of the node + */ + const char * + get_fully_qualified_name(); + + /// Get the name of the logger associated with a node. + /** + * Raises RCLError if logger name is not set + * + * \return logger_name + */ + const char * + logger_name(); + + /// Get the name of a node. + /** + * Raises RCLError if name is not set + * + * \return name of the node + */ + const char * + get_node_name(); + + /// Get the namespace of a node. + /** + * Raises RCLError if namespace is not set + * + * \return namespace + */ + const char * + get_namespace(); + + /// Returns the count of all the publishers known for that topic in the entire ROS graph. + /* + * Raises RCLError if an error occurs in rcl + * + * \param[in] topic_name Name of the topic to count the number of publishers + * \return the count of all the publishers known for that topic in the entire ROS graph. + */ + size_t + get_count_publishers(const char * topic_name); + + /// Returns the count of all the subscribers known for that topic in the entire ROS graph + /* + * + * Raises RCLError if an error occurs in rcl + * + * \param[in] topic_name Name of the topic to count the number of subscribers + * \return the count of all the subscribers known for that topic in the entire ROS graph + */ + size_t + get_count_subscribers(const char * topic_name); + /// Get the list of nodes discovered by the provided node + /** + * Raises RCLError if the names are unavailable. + * + * \return Python list of tuples where each tuple contains the two strings: + * the node name and node namespace + */ + py::list + get_node_names_and_namespaces(); + + /// Get the list of nodes discovered by the provided node, with their respective enclaves. + /** + * Raises RCLError if the names are unavailable. + * + * \return Python list of tuples where each tuple contains three strings: + * node name, node namespace, and enclave. + */ + py::list + get_node_names_and_namespaces_with_enclaves(); + + /// Get a list of parameters for the current node + /** + * Raises RCLError if any rcl function call fails + * Raises AttributeError if pyparameter_cls doesn't have a 'Type' attribute + * Raises RuntimeError if assumptions about rcl structures are violated + * + * \param[in] pyparameter_cls The rclpy.parameter.Parameter class object. + * \return A dict mapping parameter names to rclpy.parameter.Parameter (may be empty). + */ + py::dict + get_parameters(py::object pyparameter_cls); + + /// Get action client names and types by node. + /* + * \param[in] remote_node_name the node name of the actions to return + * \param[in] remote_node_namespace the node namespace of the actions to return + * \return list of action client names and their types + */ + py::list + get_action_client_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace); + + /// Get action server names and types by node. + /* + * \param[in] remote_node_name the node name of the actions to return + * \param[in] remote_node_namespace the node namespace of the actions to return + * \return list of action server names and their types + */ + py::list + get_action_server_names_and_types_by_node( + const char * remote_node_name, const char * remote_node_namespace); + + /// Get action names and types. + /* + * \return list of action names and types + */ + py::list + get_action_names_and_types(); + + /// Get rcl_node_t pointer + rcl_node_t * + rcl_ptr() const + { + return rcl_node_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + /// Get the list of nodes discovered by the provided node + /** + * Raises RCLError if the names are unavailable. + * + * \param[in] get_enclaves specifies if the output includes the enclaves names + * or not + * \return Python list of tuples, containing: + * node name, node namespace, and + * enclave if `get_enclaves` is true. + */ + py::list + get_names_impl(bool get_enclaves); + + Context context_; + std::shared_ptr rcl_node_; +}; + +void +define_node(py::object module); +} // namespace rclpy #endif // RCLPY__NODE_HPP_ diff --git a/rclpy/src/rclpy/publisher.cpp b/rclpy/src/rclpy/publisher.cpp index 5a73882fd..467d8f56f 100644 --- a/rclpy/src/rclpy/publisher.cpp +++ b/rclpy/src/rclpy/publisher.cpp @@ -21,7 +21,6 @@ #include #include "rclpy_common/common.h" -#include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" @@ -29,40 +28,11 @@ namespace rclpy { -static void -_rclpy_destroy_publisher(void * p) -{ - auto pub = static_cast(p); - if (!pub) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_publisher got NULL pointer"); - return; - } - - rcl_ret_t ret = rcl_publisher_fini(&(pub->publisher), pub->node); - if (RCL_RET_OK != ret) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "Failed to fini publisher: %s", - rcl_get_error_string().str); - } - PyMem_Free(pub); -} - -py::capsule -publisher_create( - py::capsule pynode, py::object pymsg_type, std::string topic, +Publisher::Publisher( + Node & node, py::object pymsg_type, std::string topic, py::object pyqos_profile) +: node_(node) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - auto msg_type = static_cast( rclpy_common_get_type_support(pymsg_type.ptr())); if (!msg_type) { @@ -75,22 +45,30 @@ publisher_create( publisher_ops.qos = pyqos_profile.cast(); } - // Use smart pointer to make sure memory is free'd on error - auto deleter = [](rclpy_publisher_t * ptr) {_rclpy_destroy_publisher(ptr);}; - auto pub = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rclpy_publisher_t))), - deleter); - if (!pub) { - throw std::bad_alloc(); - } - pub->publisher = rcl_get_zero_initialized_publisher(); - pub->node = node; + rcl_publisher_ = std::shared_ptr( + new rcl_publisher_t, + [node](rcl_publisher_t * publisher) + { + // Intentionally capturing node by value so shared_ptr can be transfered to copies + rcl_ret_t ret = rcl_publisher_fini(publisher, node.rcl_ptr()); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini publisher: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete publisher; + }); + + *rcl_publisher_ = rcl_get_zero_initialized_publisher(); rcl_ret_t ret = rcl_publisher_init( - &(pub->publisher), node, msg_type, + rcl_publisher_.get(), node_.rcl_ptr(), msg_type, topic.c_str(), &publisher_ops); - if (ret != RCL_RET_OK) { - if (ret == RCL_RET_TOPIC_NAME_INVALID) { + if (RCL_RET_OK != ret) { + if (RCL_RET_TOPIC_NAME_INVALID == ret) { std::string error_text{"Failed to create publisher due to invalid topic name '"}; error_text += topic; error_text += "'"; @@ -98,37 +76,18 @@ publisher_create( } throw RCLError("Failed to create publisher"); } +} - PyObject * pypub_c = - rclpy_create_handle_capsule(pub.get(), "rclpy_publisher_t", _rclpy_destroy_publisher); - if (!pypub_c) { - throw py::error_already_set(); - } - auto pypub = py::reinterpret_steal(pypub_c); - // pypub now owns the rclpy_publisher_t - pub.release(); - - auto pub_handle = static_cast(pypub); - auto node_handle = static_cast(pynode); - _rclpy_handle_add_dependency(pub_handle, node_handle); - if (PyErr_Occurred()) { - _rclpy_handle_dec_ref(pub_handle); - throw py::error_already_set(); - } - - return pypub; +void Publisher::destroy() +{ + rcl_publisher_.reset(); + node_.destroy(); } const char * -publisher_get_logger_name(py::capsule pypublisher) +Publisher::get_logger_name() { - auto pub = static_cast( - rclpy_handle_get_pointer_from_capsule(pypublisher.ptr(), "rclpy_publisher_t")); - if (!pub) { - throw py::error_already_set(); - } - - const char * node_logger_name = rcl_node_get_logger_name(pub->node); + const char * node_logger_name = rcl_node_get_logger_name(node_.rcl_ptr()); if (!node_logger_name) { throw RCLError("Node logger name not set"); } @@ -137,17 +96,11 @@ publisher_get_logger_name(py::capsule pypublisher) } size_t -publisher_get_subscription_count(py::capsule pypublisher) +Publisher::get_subscription_count() { - auto pub = static_cast( - rclpy_handle_get_pointer_from_capsule(pypublisher.ptr(), "rclpy_publisher_t")); - if (!pub) { - throw py::error_already_set(); - } - size_t count = 0; - rcl_ret_t ret = rcl_publisher_get_subscription_count(&pub->publisher, &count); - if (ret != RCL_RET_OK) { + rcl_ret_t ret = rcl_publisher_get_subscription_count(rcl_publisher_.get(), &count); + if (RCL_RET_OK != ret) { throw RCLError("failed to get subscription count"); } @@ -155,15 +108,9 @@ publisher_get_subscription_count(py::capsule pypublisher) } std::string -publisher_get_topic_name(py::capsule pypublisher) +Publisher::get_topic_name() { - auto pub = static_cast( - rclpy_handle_get_pointer_from_capsule(pypublisher.ptr(), "rclpy_publisher_t")); - if (!pub) { - throw py::error_already_set(); - } - - const char * topic_name = rcl_publisher_get_topic_name(&pub->publisher); + const char * topic_name = rcl_publisher_get_topic_name(rcl_publisher_.get()); if (!topic_name) { throw RCLError("failed to get topic name"); } @@ -172,44 +119,59 @@ publisher_get_topic_name(py::capsule pypublisher) } void -publisher_publish_message(py::capsule pypublisher, py::object pymsg) +Publisher::publish(py::object pymsg) { - auto pub = static_cast( - rclpy_handle_get_pointer_from_capsule(pypublisher.ptr(), "rclpy_publisher_t")); - if (!pub) { - throw py::error_already_set(); - } - destroy_ros_message_signature * destroy_ros_message = NULL; void * raw_ros_message = rclpy_convert_from_py(pymsg.ptr(), &destroy_ros_message); if (!raw_ros_message) { throw py::error_already_set(); } - rcl_ret_t ret = rcl_publish(&(pub->publisher), raw_ros_message, NULL); + rcl_ret_t ret = rcl_publish(rcl_publisher_.get(), raw_ros_message, NULL); destroy_ros_message(raw_ros_message); - if (ret != RCL_RET_OK) { + if (RCL_RET_OK != ret) { throw RCLError("Failed to publish"); } } void -publisher_publish_raw(py::capsule pypublisher, std::string msg) +Publisher::publish_raw(std::string msg) { - auto pub = static_cast( - rclpy_handle_get_pointer_from_capsule(pypublisher.ptr(), "rclpy_publisher_t")); - if (!pub) { - throw py::error_already_set(); - } - rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); serialized_msg.buffer_capacity = msg.size(); serialized_msg.buffer_length = msg.size(); serialized_msg.buffer = reinterpret_cast(const_cast(msg.c_str())); - rcl_ret_t ret = rcl_publish_serialized_message(&(pub->publisher), &serialized_msg, NULL); - if (ret != RCL_RET_OK) { + rcl_ret_t ret = rcl_publish_serialized_message(rcl_publisher_.get(), &serialized_msg, NULL); + if (RCL_RET_OK != ret) { throw RCLError("Failed to publish"); } } + +void +define_publisher(py::object module) +{ + py::class_>(module, "Publisher") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Publisher & publisher) { + return reinterpret_cast(publisher.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "get_logger_name", &Publisher::get_logger_name, + "Get the name of the logger associated with the node of the publisher") + .def( + "get_subscription_count", &Publisher::get_subscription_count, + "Count subscribers from a publisher.") + .def( + "get_topic_name", &Publisher::get_topic_name, + "Retrieve the topic name from a Publisher.") + .def( + "publish", &Publisher::publish, + "Publish a message") + .def( + "publish_raw", &Publisher::publish_raw, + "Publish a serialized message."); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/publisher.hpp b/rclpy/src/rclpy/publisher.hpp index a42bfb377..b40b9a2d6 100644 --- a/rclpy/src/rclpy/publisher.hpp +++ b/rclpy/src/rclpy/publisher.hpp @@ -17,87 +17,104 @@ #include +#include +#include + +#include #include +#include "destroyable.hpp" +#include "node.hpp" + namespace py = pybind11; namespace rclpy { -/// Create a publisher /** - * This function will create a publisher and attach it to the provided topic name + * This class will create a publisher and attach it to the provided topic name * This publisher will use the typesupport defined in the message module - * provided as pymsg_type to send messages over the wire. - * - * Raises ValueError if the topic name is invalid - * Raises ValueError if the capsules are not the correct types - * Raises RCLError if the publisher cannot be created - * - * \param[in] pynode Capsule pointing to the node to add the publisher to - * \param[in] pymsg_type Message type associated with the publisher - * \param[in] topic The name of the topic to attach the publisher to - * \param[in] pyqos_profile rmw_qos_profile_t object for this publisher - * \return Capsule of the pointer to the created rcl_publisher_t * structure, or + * provided as pymsg_type to send messages. */ -py::capsule -publisher_create( - py::capsule pynode, py::object pymsg_type, std::string topic, - py::object pyqos_profile); +class Publisher : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create a publisher + /* + * Raises ValueError if the topic name is invalid + * Raises ValueError if the capsules are not the correct types + * Raises RCLError if the publisher cannot be created + * + * \param[in] node node to add the publisher to + * \param[in] pymsg_type Message type associated with the publisher + * \param[in] topic The name of the topic to attach the publisher to + * \param[in] pyqos_profile rmw_qos_profile_t object for this publisher + */ + Publisher( + Node & node, py::object pymsg_type, std::string topic, + py::object pyqos_profile); -/// Get the name of the logger associated with the node of the publisher. -/** - * Raises ValueError if pypublisher is not a publisher capsule - * Raises RCLError if logger name not set - * - * \param[in] pypublisher Capsule pointing to the publisher - * \return logger_name of pypublisher - */ -const char * -publisher_get_logger_name(py::capsule pypublisher); + /// Get the name of the logger associated with the node of the publisher. + /** + * Raises RCLError if logger name not set + * + * \return the logger name + */ + const char * + get_logger_name(); -/// Count subscribers from a publisher. -/** - * Raise ValueError if capsule is not a publisher - * Raises RCLError if the subscriber count cannot be determined - * - * \param[in] pypublisher Capsule pointing to the publisher - * \return count of subscribers - */ -size_t -publisher_get_subscription_count(py::capsule pypublisher); + /// Count subscribers from a publisher. + /** + * Raises RCLError if the subscriber count cannot be determined + * + * \return number of subscribers + */ + size_t + get_subscription_count(); -/// Retrieve the topic name from a rclpy_publisher_t -/** - * Raise ValueError if capsule is not a publisher - * Raises RCLError if the name cannot be determined - * - * \param[in] pypublisher Capsule pointing to the publisher - * \return name of the publisher's topic - */ -std::string -publisher_get_topic_name(py::capsule pypublisher); + /// Retrieve the topic name from a rclpy_publisher_t + /** + * Raises RCLError if the name cannot be determined + * + * \return name of the publisher's topic + */ + std::string + get_topic_name(); -/// Publish a message -/** - * Raises ValueError if pypublisher is not a publisher capsule - * Raises RCLError if the message cannot be published - * - * \param[in] pypublisher Capsule pointing to the publisher - * \param[in] pymsg Message to send - */ -void -publisher_publish_message(py::capsule pypublisher, py::object pymsg); + /// Publish a message + /** + * Raises RCLError if the message cannot be published + * + * \param[in] pymsg Message to send + */ + void + publish(py::object pymsg); -/// Publish a serialized message -/** - * Raises ValueError if pypublisher is not a publisher capsule - * Raises RCLError if the message cannot be published - * - * \param[in] pypublisher Capsule pointing to the publisher - * \param[in] msg serialized message to send - */ -void -publisher_publish_raw(py::capsule pypublisher, std::string msg); + /// Publish a serialized message + /** + * Raises RCLError if the message cannot be published + * + * \param[in] msg serialized message to send + */ + void + publish_raw(std::string msg); + + /// Get rcl_publisher_t pointer + rcl_publisher_t * + rcl_ptr() const + { + return rcl_publisher_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + Node node_; + std::shared_ptr rcl_publisher_; +}; +/// Define a pybind11 wrapper for an rclpy::Service +void define_publisher(py::object module); } // namespace rclpy #endif // RCLPY__PUBLISHER_HPP_ diff --git a/rclpy/src/rclpy/qos_event.cpp b/rclpy/src/rclpy/qos_event.cpp index ffd4c798c..10abd191f 100644 --- a/rclpy/src/rclpy/qos_event.cpp +++ b/rclpy/src/rclpy/qos_event.cpp @@ -21,6 +21,7 @@ #include #include +#include #include // NOLINT #include "rclpy_common/common.h" @@ -58,20 +59,19 @@ void QoSEvent::destroy() { rcl_event_.reset(); - parent_handle_.reset(); + std::visit([](auto & t) {t.destroy();}, grandparent_); } QoSEvent::QoSEvent( - py::capsule pysubscription, rcl_subscription_event_type_t event_type) -: parent_handle_(std::make_shared(pysubscription)), event_type_(event_type) + rclpy::Subscription & subscription, rcl_subscription_event_type_t event_type) +: event_type_(event_type), grandparent_(subscription) { - auto wrapper = parent_handle_->cast("rclpy_subscription_t"); - // Create a subscription event rcl_event_ = create_zero_initialized_event(); rcl_ret_t ret = rcl_subscription_event_init( - rcl_event_.get(), &(wrapper->subscription), event_type); + rcl_event_.get(), + std::get(grandparent_).rcl_ptr(), event_type); if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); throw std::bad_alloc(); @@ -85,16 +85,15 @@ QoSEvent::QoSEvent( } QoSEvent::QoSEvent( - py::capsule pypublisher, rcl_publisher_event_type_t event_type) -: parent_handle_(std::make_shared(pypublisher)), event_type_(event_type) + rclpy::Publisher & publisher, rcl_publisher_event_type_t event_type) +: event_type_(event_type), grandparent_(publisher) { - auto wrapper = parent_handle_->cast("rclpy_publisher_t"); - // Create a publisher event rcl_event_ = create_zero_initialized_event(); rcl_ret_t ret = rcl_publisher_event_init( - rcl_event_.get(), &(wrapper->publisher), event_type); + rcl_event_.get(), + std::get(grandparent_).rcl_ptr(), event_type); if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); throw std::bad_alloc(); @@ -168,9 +167,9 @@ QoSEvent::take_event() void define_qos_event(py::module module) { - py::class_(module, "QoSEvent") - .def(py::init()) - .def(py::init()) + py::class_>(module, "QoSEvent") + .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const QoSEvent & event) { return reinterpret_cast(event.rcl_ptr()); diff --git a/rclpy/src/rclpy/qos_event.hpp b/rclpy/src/rclpy/qos_event.hpp index 7c886758c..7d7bbcd96 100644 --- a/rclpy/src/rclpy/qos_event.hpp +++ b/rclpy/src/rclpy/qos_event.hpp @@ -24,27 +24,30 @@ #include "destroyable.hpp" #include "handle.hpp" +#include "publisher.hpp" +#include "subscription.hpp" namespace py = pybind11; namespace rclpy { -class QoSEvent : public Destroyable +/* + * This class will create an event handle for the given subscription. + */ +class QoSEvent : public Destroyable, public std::enable_shared_from_this { public: /// Create a subscription event /** - * This function will create an event handle for the given subscription. - * * Raises UnsupportedEventTypeError if the event type is not supported * Raises TypeError if arguments are not of the correct types i.e. a subscription capsule * Raises MemoryError if the event can't be allocated * Raises RCLError if event initialization failed in rcl * - * \param[in] pysubscription Capsule containing the subscription + * \param[in] ssubscription Subscription wrapping the underlying ``rcl_subscription_t`` object. * \param[in] event_type Type of event to create */ - QoSEvent(py::capsule pysubscription, rcl_subscription_event_type_t event_type); + QoSEvent(rclpy::Subscription & subscriber, rcl_subscription_event_type_t event_type); /// Create a publisher event /** @@ -55,10 +58,10 @@ class QoSEvent : public Destroyable * Raises MemoryError if the event can't be allocated * Raises RCLError if event initialization failed in rcl * - * \param[in] pypublisher Capsule containing the publisher + * \param[in] publisher Publisher wrapping the underlying ``rcl_publisher_t`` object. * \param[in] event_type Type of event to create */ - QoSEvent(py::capsule pypublisher, rcl_publisher_event_type_t event_type); + QoSEvent(rclpy::Publisher & publisher, rcl_publisher_event_type_t event_type); ~QoSEvent() = default; @@ -88,9 +91,9 @@ class QoSEvent : public Destroyable destroy() override; private: - std::shared_ptr parent_handle_; - std::shared_ptr rcl_event_; std::variant event_type_; + std::variant grandparent_; + std::shared_ptr rcl_event_; }; /// Define a pybind11 wrapper for an rclpy::QoSEvent diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index 1002fbf73..2f4acf093 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -35,16 +35,14 @@ void Service::destroy() { rcl_service_.reset(); - node_handle_.reset(); + node_.destroy(); } Service::Service( - py::capsule pynode, py::object pysrv_type, std::string service_name, + Node & node, py::object pysrv_type, std::string service_name, py::object pyqos_profile) -: node_handle_(std::make_shared(pynode)) +: node_(node) { - auto node = node_handle_->cast("rcl_node_t"); - auto srv_type = static_cast( rclpy_common_get_type_support(pysrv_type.ptr())); if (!srv_type) { @@ -60,11 +58,10 @@ Service::Service( // Create a client rcl_service_ = std::shared_ptr( new rcl_service_t, - [this](rcl_service_t * service) + [node](rcl_service_t * service) { - auto node = node_handle_->cast_or_warn("rcl_node_t"); - - rcl_ret_t ret = rcl_service_fini(service, node); + // Intentionally capture node by copy so shared_ptr can be transfered to copies + rcl_ret_t ret = rcl_service_fini(service, node.rcl_ptr()); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -79,7 +76,7 @@ Service::Service( *rcl_service_ = rcl_get_zero_initialized_service(); rcl_ret_t ret = rcl_service_init( - rcl_service_.get(), node, srv_type, + rcl_service_.get(), node_.rcl_ptr(), srv_type, service_name.c_str(), &service_ops); if (RCL_RET_OK != ret) { if (ret == RCL_RET_SERVICE_NAME_INVALID) { @@ -120,8 +117,7 @@ Service::service_take_request(py::object pyrequest_type) rmw_service_info_t header; py::tuple result_tuple(2); - rcl_ret_t ret = rcl_take_request_with_info( - rcl_service_.get(), &header, taken_request.get()); + rcl_ret_t ret = rcl_take_request_with_info(rcl_service_.get(), &header, taken_request.get()); if (ret == RCL_RET_SERVICE_TAKE_FAILED) { result_tuple[0] = py::none(); result_tuple[1] = py::none(); @@ -139,8 +135,8 @@ Service::service_take_request(py::object pyrequest_type) void define_service(py::object module) { - py::class_(module, "Service") - .def(py::init()) + py::class_>(module, "Service") + .def(py::init()) .def_property_readonly( "pointer", [](const Service & service) { return reinterpret_cast(service.rcl_ptr()); diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index ec87880c7..6d8b0f298 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -25,6 +25,7 @@ #include "destroyable.hpp" #include "handle.hpp" +#include "node.hpp" #include "rclpy_common/exceptions.hpp" #include "utils.hpp" @@ -33,7 +34,7 @@ namespace py = pybind11; namespace rclpy { -class Service : public Destroyable +class Service : public Destroyable, public std::enable_shared_from_this { public: /// Create a service server @@ -45,14 +46,14 @@ class Service : public Destroyable * Raises ValueError if the capsules are not the correct types * Raises RCLError if the service could not be created * - * \param[in] pynode Capsule pointing to the node to add the service to + * \param[in] node node to add the service to * \param[in] pysrv_type Service module associated with the service * \param[in] service_name Python object for the service name * \param[in] pyqos_profile QoSProfile Python object for this service * \return capsule containing the rcl_service_t */ Service( - py::capsule pynode, py::object pysrv_type, std::string service_name, + Node & node, py::object pysrv_type, std::string service_name, py::object pyqos_profile); ~Service() = default; @@ -81,7 +82,7 @@ class Service : public Destroyable py::tuple service_take_request(py::object pyrequest_type); - /// Get rcl_client_t pointer + /// Get rcl_service_t pointer rcl_service_t * rcl_ptr() const { @@ -93,8 +94,7 @@ class Service : public Destroyable destroy() override; private: - // TODO(ahcorde) replace with std::shared_ptr when rclpy::Node exists - std::shared_ptr node_handle_; + Node node_; std::shared_ptr rcl_service_; }; diff --git a/rclpy/src/rclpy/subscription.cpp b/rclpy/src/rclpy/subscription.cpp index 6a20d8f26..2b2f11cff 100644 --- a/rclpy/src/rclpy/subscription.cpp +++ b/rclpy/src/rclpy/subscription.cpp @@ -23,7 +23,6 @@ #include #include "rclpy_common/common.h" -#include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" @@ -35,40 +34,11 @@ using pybind11::literals::operator""_a; namespace rclpy { -static void -_rclpy_destroy_subscription(void * p) -{ - auto sub = static_cast(p); - if (!sub) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "_rclpy_destroy_subscription got NULL pointer"); - return; - } - - rcl_ret_t ret = rcl_subscription_fini(&(sub->subscription), sub->node); - if (RCL_RET_OK != ret) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, "Failed to fini subscription: %s", - rcl_get_error_string().str); - } - PyMem_Free(sub); -} - -py::capsule -subscription_create( - py::capsule pynode, py::object pymsg_type, std::string topic, +Subscription::Subscription( + Node & node, py::object pymsg_type, std::string topic, py::object pyqos_profile) +: node_(node) { - auto node = static_cast( - rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); - if (!node) { - throw py::error_already_set(); - } - auto msg_type = static_cast( rclpy_common_get_type_support(pymsg_type.ptr())); if (!msg_type) { @@ -81,19 +51,27 @@ subscription_create( subscription_ops.qos = pyqos_profile.cast(); } - // Use smart pointer to make sure memory is free'd on error - auto deleter = [](rclpy_subscription_t * ptr) {_rclpy_destroy_subscription(ptr);}; - auto sub = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rclpy_subscription_t))), - deleter); - if (!sub) { - throw std::bad_alloc(); - } - sub->subscription = rcl_get_zero_initialized_subscription(); - sub->node = node; + rcl_subscription_ = std::shared_ptr( + new rcl_subscription_t, + [node](rcl_subscription_t * subscription) + { + // Intentionally capture node by copy so shared_ptr can be transfered to copies + rcl_ret_t ret = rcl_subscription_fini(subscription, node.rcl_ptr()); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini subscription: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete subscription; + }); + + *rcl_subscription_ = rcl_get_zero_initialized_subscription(); rcl_ret_t ret = rcl_subscription_init( - &(sub->subscription), node, msg_type, + rcl_subscription_.get(), node_.rcl_ptr(), msg_type, topic.c_str(), &subscription_ops); if (ret != RCL_RET_OK) { if (ret == RCL_RET_TOPIC_NAME_INVALID) { @@ -104,41 +82,23 @@ subscription_create( } throw RCLError("Failed to create subscription"); } +} - PyObject * pysub_c = - rclpy_create_handle_capsule(sub.get(), "rclpy_subscription_t", _rclpy_destroy_subscription); - if (!pysub_c) { - throw py::error_already_set(); - } - auto pysub = py::reinterpret_steal(pysub_c); - // pysub now owns the rclpy_subscription_t - sub.release(); - - auto sub_handle = static_cast(pysub); - auto node_handle = static_cast(pynode); - _rclpy_handle_add_dependency(sub_handle, node_handle); - if (PyErr_Occurred()) { - throw py::error_already_set(); - } - - return pysub; +void Subscription::destroy() +{ + rcl_subscription_.reset(); + node_.destroy(); } py::object -subscription_take_message(py::capsule pysubscription, py::object pymsg_type, bool raw) +Subscription::take_message(py::object pymsg_type, bool raw) { - auto wrapper = static_cast( - rclpy_handle_get_pointer_from_capsule(pysubscription.ptr(), "rclpy_subscription_t")); - if (!wrapper) { - throw py::error_already_set(); - } - py::object pytaken_msg; rmw_message_info_t message_info; if (raw) { SerializedMessage taken{rcutils_get_default_allocator()}; rcl_ret_t ret = rcl_take_serialized_message( - &(wrapper->subscription), &taken.rcl_msg, &message_info, NULL); + rcl_subscription_.get(), &taken.rcl_msg, &message_info, NULL); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); @@ -156,7 +116,7 @@ subscription_take_message(py::capsule pysubscription, py::object pymsg_type, boo auto taken_msg = create_from_py(pymsg_type); rcl_ret_t ret = rcl_take( - &(wrapper->subscription), taken_msg.get(), &message_info, NULL); + rcl_subscription_.get(), taken_msg.get(), &message_info, NULL); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); @@ -177,37 +137,45 @@ subscription_take_message(py::capsule pysubscription, py::object pymsg_type, boo "received_timestamp"_a = message_info.received_timestamp)); } -py::object -subscription_get_logger_name(py::capsule pysubscription) +const char * +Subscription::get_logger_name() { - auto sub = static_cast( - rclpy_handle_get_pointer_from_capsule(pysubscription.ptr(), "rclpy_subscription_t")); - if (!sub) { - throw py::error_already_set(); - } - - const char * node_logger_name = rcl_node_get_logger_name(sub->node); - if (nullptr == node_logger_name) { - return py::none(); + const char * node_logger_name = rcl_node_get_logger_name(node_.rcl_ptr()); + if (!node_logger_name) { + throw RCLError("Node logger name not set"); } - return py::str(node_logger_name); + return node_logger_name; } std::string -subscription_get_topic_name(py::capsule pysubscription) +Subscription::get_topic_name() { - auto sub = static_cast( - rclpy_handle_get_pointer_from_capsule(pysubscription.ptr(), "rclpy_subscription_t")); - if (!sub) { - throw py::error_already_set(); - } - - const char * subscription_name = rcl_subscription_get_topic_name(&(sub->subscription)); + const char * subscription_name = rcl_subscription_get_topic_name(rcl_subscription_.get()); if (nullptr == subscription_name) { throw RCLError("failed to get subscription topic name"); } return std::string(subscription_name); } +void +define_subscription(py::object module) +{ + py::class_>(module, "Subscription") + .def(py::init()) + .def_property_readonly( + "pointer", [](const Subscription & subscription) { + return reinterpret_cast(subscription.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "take_message", &Subscription::take_message, + "Take a message and its metadata from a subscription") + .def( + "get_logger_name", &Subscription::get_logger_name, + "Get the name of the logger associated with the node of the subscription.") + .def( + "get_topic_name", &Subscription::get_topic_name, + "Return the resolved topic name of a subscription."); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/subscription.hpp b/rclpy/src/rclpy/subscription.hpp index 5c31aa757..21640a16e 100644 --- a/rclpy/src/rclpy/subscription.hpp +++ b/rclpy/src/rclpy/subscription.hpp @@ -17,74 +17,92 @@ #include +#include +#include + +#include #include +#include "destroyable.hpp" +#include "node.hpp" + namespace py = pybind11; namespace rclpy { /// Create a subscription /** - * This function will create a subscription for the given topic name. + * This class will create a subscription for the given topic name. * This subscription will use the typesupport defined in the message module - * provided as pymsg_type to send messages over the wire. - * - * On a successful call a Capsule pointing to the pointer of the created - * rcl_subscription_t * structure is returned - * - * Raises ValueError if the capsules are not the correct types - * Raises RCLError if the subscription could not be created - * - * \param[in] pynode Capsule pointing to the node to add the subscriber to - * \param[in] pymsg_type Message module associated with the subscriber - * \param[in] topic The topic name - * \param[in] pyqos_profile rmw_qos_profile_t object for this subscription - * \return capsule containing the rclpy_subscription_t + * provided as pymsg_type to send messages. */ -py::capsule -subscription_create( - py::capsule pynode, py::object pymsg_type, std::string topic, - py::object pyqos_profile); +class Subscription : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Create a subscription + /* + * Raises RCLError if the subscription could not be created + * + * \param[in] node node to add the subscriber to + * \param[in] pymsg_type Message module associated with the subscriber + * \param[in] topic The topic name + * \param[in] pyqos_profile rmw_qos_profile_t object for this subscription + */ + Subscription( + Node & node, py::object pymsg_type, std::string topic, + py::object pyqos_profile); -/// Take a message and its metadata from a subscription -/** - * Raises ValueError if pysubscription is not subscription capsule - * Raises MemoryError if there was error allocating memory - * Raises RCLError if there was an error within rcl - * - * \param[in] pysubscription Capsule pointing to the subscription. - * \param[in] pymsg_type Message type to be taken (i.e. a class). - * \param[in] raw If True, return the message without de-serializing it. - * \return Tuple of (message, metadata) or None if there was no message to take. - * Message is a \p pymsg_type instance if \p raw is True, otherwise a byte string. - * Metadata is a plain dictionary. - */ -py::object -subscription_take_message(py::capsule pysubscription, py::object pymsg_type, bool raw); + /// Take a message and its metadata from a subscription + /** + * Raises MemoryError if there was an error allocating memory + * Raises RCLError if there was an error within rcl + * + * \param[in] pymsg_type Message type to be taken (i.e. a class). + * \param[in] raw If True, return the message without de-serializing it. + * \return Tuple of (message, metadata) or None if there was no message to take. + * Message is a \p pymsg_type instance if \p raw is True, otherwise a byte string. + * Metadata is a plain dictionary. + */ + py::object + take_message(py::object pymsg_type, bool raw); -/// Get the name of the logger associated with the node of the subscription. -/** - * Raises ValueError if pysubscription is not a subscription capsule - * - * \param[in] pysubscription Capsule pointing to the subscription to get the logger name of - * \return logger_name, or - * \return None on failure - */ -py::object -subscription_get_logger_name(py::capsule pysubscription); + /// Get the name of the logger associated with the node of the subscription. + /** + * + * \return logger_name, or + * \return None on failure + */ + const char * + get_logger_name(); -/// Return the resolved topic name of a subscription. -/** - * The returned string is the resolved topic name after remappings have be applied. - * - * Raises ValueError if pysubscription is not a subscription capsule - * Raises RCLError if the topic name could not be determined - * - * \param[in] pysubscription Capsule pointing to the subscription to get the topic name of - * \return a string with the topic name - */ -std::string -subscription_get_topic_name(py::capsule pysubscription); + /// Return the resolved topic name of a subscription. + /** + * The returned string is the resolved topic name after remappings have be applied. + * + * Raises RCLError if the topic name could not be determined + * + * \return a string with the topic name + */ + std::string + get_topic_name(); + + /// Get rcl_subscription_t pointer + rcl_subscription_t * + rcl_ptr() const + { + return rcl_subscription_.get(); + } + + /// Force an early destruction of this object + void + destroy() override; + +private: + Node node_; + std::shared_ptr rcl_subscription_; +}; +/// Define a pybind11 wrapper for an rclpy::Service +void define_subscription(py::object module); } // namespace rclpy #endif // RCLPY__SUBSCRIPTION_HPP_ diff --git a/rclpy/src/rclpy/timer.cpp b/rclpy/src/rclpy/timer.cpp index b4e192e57..a424dd013 100644 --- a/rclpy/src/rclpy/timer.cpp +++ b/rclpy/src/rclpy/timer.cpp @@ -37,20 +37,14 @@ void Timer::destroy() { rcl_timer_.reset(); - clock_handle_.reset(); + clock_.destroy(); + context_.destroy(); } Timer::Timer( - Clock & rclcy_clock, py::capsule pycontext, int64_t period_nsec) + Clock & clock, Context & context, int64_t period_nsec) +: context_(context), clock_(clock) { - clock_handle_ = rclcy_clock.get_shared_ptr(); - - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } - // Create a client rcl_timer_ = std::shared_ptr( new rcl_timer_t, @@ -72,7 +66,7 @@ Timer::Timer( rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_timer_init( - rcl_timer_.get(), clock_handle_.get(), context, + rcl_timer_.get(), clock_.rcl_ptr(), context.rcl_ptr(), period_nsec, NULL, allocator); if (RCL_RET_OK != ret) { @@ -168,8 +162,8 @@ bool Timer::is_timer_canceled() void define_timer(py::object module) { - py::class_(module, "Timer") - .def(py::init()) + py::class_>(module, "Timer") + .def(py::init()) .def_property_readonly( "pointer", [](const Timer & timer) { return reinterpret_cast(timer.rcl_ptr()); diff --git a/rclpy/src/rclpy/timer.hpp b/rclpy/src/rclpy/timer.hpp index 57e5dde7e..55c37b292 100644 --- a/rclpy/src/rclpy/timer.hpp +++ b/rclpy/src/rclpy/timer.hpp @@ -22,6 +22,7 @@ #include #include "clock.hpp" +#include "context.hpp" #include "destroyable.hpp" #include "handle.hpp" @@ -30,7 +31,7 @@ namespace py = pybind11; namespace rclpy { -class Timer : public Destroyable +class Timer : public Destroyable, public std::enable_shared_from_this { public: /// Create a timer @@ -46,7 +47,7 @@ class Timer : public Destroyable * \param[in] period_nsec the period of the timer in nanoseconds * \return a timer capsule */ - Timer(Clock & clock, py::capsule pycontext, int64_t period_nsec); + Timer(Clock & clock, Context & context, int64_t period_nsec); ~Timer() = default; @@ -122,7 +123,7 @@ class Timer : public Destroyable */ bool is_timer_canceled(); - /// Get rcl_client_t pointer + /// Get rcl_timer_t pointer rcl_timer_t * rcl_ptr() const { @@ -133,8 +134,8 @@ class Timer : public Destroyable void destroy() override; private: - // TODO(ahcorde) replace with std::shared_ptr when rclpy::Clock exists - std::shared_ptr clock_handle_; + Context context_; + Clock clock_; std::shared_ptr rcl_timer_; }; diff --git a/rclpy/src/rclpy/utils.cpp b/rclpy/src/rclpy/utils.cpp index 8eb013c87..6f2cf2c0d 100644 --- a/rclpy/src/rclpy/utils.cpp +++ b/rclpy/src/rclpy/utils.cpp @@ -17,17 +17,18 @@ #include #include +#include #include #include #include +#include #include #include "rclpy_common/common.h" #include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" -#include "init.hpp" #include "utils.hpp" namespace rclpy @@ -81,6 +82,31 @@ create_from_py(py::object pymessage) void, destroy_ros_message_function *>(message, destroy_ros_message); } +std::unique_ptr +convert_from_py(py::object pymessage) +{ + typedef bool convert_from_py_signature (PyObject *, void *); + + std::unique_ptr message = + create_from_py(pymessage); + + py::object pymetaclass = pymessage.attr("__class__"); + + auto capsule_ptr = static_cast( + pymetaclass.attr("_CONVERT_FROM_PY").cast()); + auto convert = + reinterpret_cast(capsule_ptr); + if (!convert) { + throw py::error_already_set(); + } + + if (!convert(pymessage.ptr(), message.get())) { + throw py::error_already_set(); + } + + return message; +} + py::object convert_to_py(void * message, py::object pyclass) { @@ -104,20 +130,10 @@ get_rmw_implementation_identifier() } void -assert_liveliness(py::capsule pyentity) +assert_liveliness(rclpy::Publisher * publisher) { - if (0 == strcmp("rclpy_publisher_t", pyentity.name())) { - auto publisher = static_cast( - rclpy_handle_get_pointer_from_capsule( - pyentity.ptr(), "rclpy_publisher_t")); - if (nullptr == publisher) { - throw py::error_already_set(); - } - if (RCL_RET_OK != rcl_publisher_assert_liveliness(&publisher->publisher)) { - throw RCLError("Failed to assert liveliness on the Publisher"); - } - } else { - throw py::type_error("Passed capsule is not a valid Publisher."); + if (RCL_RET_OK != rcl_publisher_assert_liveliness(publisher->rcl_ptr())) { + throw RCLError("Failed to assert liveliness on the Publisher"); } } @@ -210,4 +226,54 @@ remove_ros_args(py::object pycli_args) return result_args; } + +void +throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args) +{ + int unparsed_ros_args_count = rcl_arguments_get_count_unparsed_ros(&rcl_args); + + if (unparsed_ros_args_count < 0) { + throw std::runtime_error("failed to count unparsed arguments"); + } + if (0 == unparsed_ros_args_count) { + return; + } + + rcl_allocator_t allocator = rcl_get_default_allocator(); + + int * unparsed_indices_c = nullptr; + rcl_ret_t ret = rcl_arguments_get_unparsed_ros(&rcl_args, allocator, &unparsed_indices_c); + if (RCL_RET_OK != ret) { + throw RCLError("failed to get unparsed arguments"); + } + + auto deallocator = [&](int ptr[]) {allocator.deallocate(ptr, allocator.state);}; + auto unparsed_indices = std::unique_ptr( + unparsed_indices_c, deallocator); + + py::list unparsed_args; + for (int i = 0; i < unparsed_ros_args_count; ++i) { + int index = unparsed_indices_c[i]; + if (index < 0 || static_cast(index) >= pyargs.size()) { + throw std::runtime_error("got invalid unparsed ROS arg index"); + } + unparsed_args.append(pyargs[index]); + } + + throw UnknownROSArgsError(static_cast(py::repr(unparsed_args))); +} + +py::dict +rclpy_action_get_rmw_qos_profile(const char * rmw_profile) +{ + PyObject * pyqos_profile = NULL; + if (0 == strcmp(rmw_profile, "rcl_action_qos_profile_status_default")) { + pyqos_profile = rclpy_common_convert_to_qos_dict(&rcl_action_qos_profile_status_default); + } else { + std::string error_text = "Requested unknown rmw_qos_profile: "; + error_text += rmw_profile; + throw std::runtime_error(error_text); + } + return py::reinterpret_steal(pyqos_profile); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/utils.hpp b/rclpy/src/rclpy/utils.hpp index 58fa0b24a..0f4ae104e 100644 --- a/rclpy/src/rclpy/utils.hpp +++ b/rclpy/src/rclpy/utils.hpp @@ -21,6 +21,9 @@ #include +#include "rclpy_common/common.h" +#include "publisher.hpp" + namespace py = pybind11; namespace rclpy @@ -47,6 +50,16 @@ convert_to_py_names_and_types(const rcl_names_and_types_t * topic_names_and_type std::unique_ptr create_from_py(py::object pyclass); +/// Convert a ROS message from a Python type to a C type. +/** + * Raises AttributeError if the Python message type is missing a required attribute. + * + * \param[in] pyclass ROS message Python type to extract data from. + * \return unique pointer with the C version of the input ROS message. + */ +std::unique_ptr +convert_from_py(py::object pyclass); + /// Convert a ROS message from a C type to a Python type. /** * Raises AttributeError if \p pyclass is missing a required attribute. @@ -78,7 +91,7 @@ get_rmw_implementation_identifier(); * \return None */ void -assert_liveliness(py::capsule pyentity); +assert_liveliness(rclpy::Publisher * publisher); /// Remove ROS specific args from a list of args. /** @@ -93,6 +106,21 @@ assert_liveliness(py::capsule pyentity); py::list remove_ros_args(py::object pycli_args); +/// Throw UnparsedROSArgsError with a message saying which args are unparsed. +void +throw_if_unparsed_ros_args(py::list pyargs, const rcl_arguments_t & rcl_args); + +/// Fetch a predefined qos_profile from rcl_action and convert it to a Python QoSProfile object. +/** + * Raises RuntimeError if the QoS profile is unknown. + * + * This function takes a string defining a rmw_qos_profile_t and returns the + * corresponding Python QoSProfile object. + * \param[in] rmw_profile String with the name of the profile to load. + * \return QoSProfile object. + */ +py::dict +rclpy_action_get_rmw_qos_profile(const char * rmw_profile); } // namespace rclpy #endif // RCLPY__UTILS_HPP_ diff --git a/rclpy/src/rclpy/wait_set.cpp b/rclpy/src/rclpy/wait_set.cpp index bc15e432c..2e837be15 100644 --- a/rclpy/src/rclpy/wait_set.cpp +++ b/rclpy/src/rclpy/wait_set.cpp @@ -32,157 +32,93 @@ namespace rclpy { -/// Destructor for a wait set -void -_rclpy_destroy_wait_set(PyObject * pycapsule) -{ - auto wait_set = static_cast(PyCapsule_GetPointer(pycapsule, "rcl_wait_set_t")); - - rcl_ret_t ret = rcl_wait_set_fini(wait_set); - if (ret != RCL_RET_OK) { - // Warning should use line number of the current stack frame - int stack_level = 1; - PyErr_WarnFormat( - PyExc_RuntimeWarning, stack_level, - "Failed to fini wait set: %s", rcl_get_error_string().str); - rcl_reset_error(); - } - - PyMem_Free(wait_set); -} - -py::capsule -get_zero_initialized_wait_set() -{ - auto deleter = [](rcl_wait_set_t * ptr) {PyMem_FREE(ptr);}; - auto wait_set = std::unique_ptr( - static_cast(PyMem_Malloc(sizeof(rcl_wait_set_t))), - deleter); - if (!wait_set) { - throw std::bad_alloc(); - } - - *wait_set = rcl_get_zero_initialized_wait_set(); - return py::capsule(wait_set.release(), "rcl_wait_set_t", _rclpy_destroy_wait_set); -} - -void -wait_set_init( - py::capsule pywait_set, +WaitSet::WaitSet( size_t number_of_subscriptions, size_t number_of_guard_conditions, size_t number_of_timers, size_t number_of_clients, size_t number_of_services, size_t number_of_events, - py::capsule pycontext) + Context & context) +: context_(context) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - - auto context = static_cast( - rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); - if (!context) { - throw py::error_already_set(); - } + // Create a client + rcl_wait_set_ = std::shared_ptr( + new rcl_wait_set_t, + [](rcl_wait_set_t * waitset) + { + rcl_ret_t ret = rcl_wait_set_fini(waitset); + if (RCL_RET_OK != ret) { + // Warning should use line number of the current stack frame + int stack_level = 1; + PyErr_WarnFormat( + PyExc_RuntimeWarning, stack_level, "Failed to fini wait set: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + delete waitset; + }); + *rcl_wait_set_ = rcl_get_zero_initialized_wait_set(); rcl_ret_t ret = rcl_wait_set_init( - wait_set, + rcl_wait_set_.get(), number_of_subscriptions, number_of_guard_conditions, number_of_timers, number_of_clients, number_of_services, number_of_events, - context, + context.rcl_ptr(), rcl_get_default_allocator()); - if (ret != RCL_RET_OK) { + if (RCL_RET_OK != ret) { throw RCLError("failed to initialize wait set"); } } void -wait_set_clear_entities(py::capsule pywait_set) +WaitSet::destroy() { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); + rcl_wait_set_.reset(); + context_.destroy(); +} - rcl_ret_t ret = rcl_wait_set_clear(wait_set); +void +WaitSet::clear_entities() +{ + rcl_ret_t ret = rcl_wait_set_clear(rcl_wait_set_.get()); if (ret != RCL_RET_OK) { throw RCLError("failed to clear wait set"); } } size_t -wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py::capsule pyentity) +WaitSet::add_service(const Service & service) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - - rcl_ret_t ret = RCL_RET_ERROR; size_t index; - - if ("subscription" == entity_type) { - auto sub = static_cast( - rclpy_handle_get_pointer_from_capsule(pyentity.ptr(), "rclpy_subscription_t")); - if (!sub) { - throw py::error_already_set(); - } - ret = rcl_wait_set_add_subscription(wait_set, &(sub->subscription), &index); - } else if ("guard_condition" == entity_type) { - auto guard_condition = static_cast( - rclpy_handle_get_pointer_from_capsule(pyentity.ptr(), "rcl_guard_condition_t")); - if (!guard_condition) { - throw py::error_already_set(); - } - ret = rcl_wait_set_add_guard_condition(wait_set, guard_condition, &index); - } else { - std::string error_text{"'"}; - error_text += entity_type; - error_text += "' is not a known entity"; - throw std::runtime_error(error_text); - } - if (ret != RCL_RET_OK) { - std::string error_text{"failed to add'"}; - error_text += entity_type; - error_text += "' to waitset"; - throw RCLError(error_text); + rcl_ret_t ret = rcl_wait_set_add_service(rcl_wait_set_.get(), service.rcl_ptr(), &index); + if (RCL_RET_OK != ret) { + throw RCLError("failed to add service to wait set"); } return index; } size_t -wait_set_add_service(const py::capsule pywait_set, const Service & service) +WaitSet::add_subscription(const Subscription & subscription) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - size_t index; - rcl_ret_t ret = rcl_wait_set_add_service(wait_set, service.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_subscription( + rcl_wait_set_.get(), subscription.rcl_ptr(), &index); if (RCL_RET_OK != ret) { - throw RCLError("failed to add service to wait set"); + throw RCLError("failed to add subscription to wait set"); } return index; } size_t -wait_set_add_timer(const py::capsule pywait_set, const Timer & timer) +WaitSet::add_timer(const Timer & timer) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - size_t index; - rcl_ret_t ret = rcl_wait_set_add_timer(wait_set, timer.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_timer(rcl_wait_set_.get(), timer.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add client to wait set"); } @@ -190,15 +126,21 @@ wait_set_add_timer(const py::capsule pywait_set, const Timer & timer) } size_t -wait_set_add_client(const py::capsule pywait_set, const Client & client) +WaitSet::add_guard_condition(const GuardCondition & gc) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); + size_t index; + rcl_ret_t ret = rcl_wait_set_add_guard_condition(rcl_wait_set_.get(), gc.rcl_ptr(), &index); + if (RCL_RET_OK != ret) { + throw RCLError("failed to add guard condition to wait set"); } - auto wait_set = static_cast(pywait_set); + return index; +} +size_t +WaitSet::add_client(const Client & client) +{ size_t index; - rcl_ret_t ret = rcl_wait_set_add_client(wait_set, client.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_client(rcl_wait_set_.get(), client.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add client to wait set"); } @@ -206,15 +148,10 @@ wait_set_add_client(const py::capsule pywait_set, const Client & client) } size_t -wait_set_add_event(const py::capsule pywait_set, const QoSEvent & event) +WaitSet::add_event(const QoSEvent & event) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - size_t index; - rcl_ret_t ret = rcl_wait_set_add_event(wait_set, event.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_event(rcl_wait_set_.get(), event.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add event to wait set"); } @@ -222,33 +159,28 @@ wait_set_add_event(const py::capsule pywait_set, const QoSEvent & event) } bool -wait_set_is_ready(const std::string & entity_type, py::capsule pywait_set, size_t index) +WaitSet::is_ready(const std::string & entity_type, size_t index) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - const void ** entities = NULL; size_t num_entities = 0; if ("subscription" == entity_type) { - entities = reinterpret_cast(wait_set->subscriptions); - num_entities = wait_set->size_of_subscriptions; + entities = reinterpret_cast(rcl_wait_set_->subscriptions); + num_entities = rcl_wait_set_->size_of_subscriptions; } else if ("client" == entity_type) { - entities = reinterpret_cast(wait_set->clients); - num_entities = wait_set->size_of_clients; + entities = reinterpret_cast(rcl_wait_set_->clients); + num_entities = rcl_wait_set_->size_of_clients; } else if ("service" == entity_type) { - entities = reinterpret_cast(wait_set->services); - num_entities = wait_set->size_of_services; + entities = reinterpret_cast(rcl_wait_set_->services); + num_entities = rcl_wait_set_->size_of_services; } else if ("timer" == entity_type) { - entities = reinterpret_cast(wait_set->timers); - num_entities = wait_set->size_of_timers; + entities = reinterpret_cast(rcl_wait_set_->timers); + num_entities = rcl_wait_set_->size_of_timers; } else if ("guard_condition" == entity_type) { - entities = reinterpret_cast(wait_set->guard_conditions); - num_entities = wait_set->size_of_guard_conditions; + entities = reinterpret_cast(rcl_wait_set_->guard_conditions); + num_entities = rcl_wait_set_->size_of_guard_conditions; } else if ("event" == entity_type) { - entities = reinterpret_cast(wait_set->events); - num_entities = wait_set->size_of_events; + entities = reinterpret_cast(rcl_wait_set_->events); + num_entities = rcl_wait_set_->size_of_events; } else { std::string error_text{"'"}; error_text += entity_type; @@ -283,23 +215,23 @@ _get_ready_entities(const EntityArray ** entities, const size_t num_entities) } py::list -get_ready_entities(const std::string & entity_type, py::capsule pywait_set) +WaitSet::get_ready_entities(const std::string & entity_type) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - if ("subscription" == entity_type) { - return _get_ready_entities(wait_set->subscriptions, wait_set->size_of_subscriptions); + return _get_ready_entities( + rcl_wait_set_->subscriptions, rcl_wait_set_->size_of_subscriptions); } else if ("client" == entity_type) { - return _get_ready_entities(wait_set->clients, wait_set->size_of_clients); + return _get_ready_entities( + rcl_wait_set_->clients, rcl_wait_set_->size_of_clients); } else if ("service" == entity_type) { - return _get_ready_entities(wait_set->services, wait_set->size_of_services); + return _get_ready_entities( + rcl_wait_set_->services, rcl_wait_set_->size_of_services); } else if ("timer" == entity_type) { - return _get_ready_entities(wait_set->timers, wait_set->size_of_timers); + return _get_ready_entities( + rcl_wait_set_->timers, rcl_wait_set_->size_of_timers); } else if ("guard_condition" == entity_type) { - return _get_ready_entities(wait_set->guard_conditions, wait_set->size_of_guard_conditions); + return _get_ready_entities( + rcl_wait_set_->guard_conditions, rcl_wait_set_->size_of_guard_conditions); } std::string error_text{"'"}; @@ -309,23 +241,59 @@ get_ready_entities(const std::string & entity_type, py::capsule pywait_set) } void -wait(py::capsule pywait_set, int64_t timeout) +WaitSet::wait(int64_t timeout) { - if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { - throw py::value_error("capsule is not an rcl_wait_set_t"); - } - auto wait_set = static_cast(pywait_set); - rcl_ret_t ret; // Could be a long wait, release the GIL { py::gil_scoped_release gil_release; - ret = rcl_wait(wait_set, timeout); + ret = rcl_wait(rcl_wait_set_.get(), timeout); } - if (ret != RCL_RET_OK && ret != RCL_RET_TIMEOUT) { + if (RCL_RET_OK != ret && RCL_RET_TIMEOUT != ret) { throw RCLError("failed to wait on wait set"); } } + +void define_waitset(py::object module) +{ + py::class_>(module, "WaitSet") + .def(py::init()) + .def_property_readonly( + "pointer", [](const WaitSet & waitset) { + return reinterpret_cast(waitset.rcl_ptr()); + }, + "Get the address of the entity as an integer") + .def( + "clear_entities", &WaitSet::clear_entities, + "Clear all the pointers in the wait set") + .def( + "add_service", &WaitSet::add_service, + "Add a service to the wait set structure") + .def( + "add_subscription", &WaitSet::add_subscription, + "Add a subcription to the wait set structure") + .def( + "add_client", &WaitSet::add_client, + "Add a client to the wait set structure") + .def( + "add_guard_condition", &WaitSet::add_guard_condition, + "Add a guard condition to the wait set structure") + .def( + "add_timer", &WaitSet::add_timer, + "Add a timer to the wait set structure") + .def( + "add_event", &WaitSet::add_event, + "Add an event to the wait set structure") + .def( + "is_ready", &WaitSet::is_ready, + "Check if an entity in the wait set is ready by its index") + .def( + "get_ready_entities", &WaitSet::get_ready_entities, + "Get list of entities ready by entity type") + .def( + "wait", &WaitSet::wait, + "Wait until timeout is reached or event happened"); +} } // namespace rclpy diff --git a/rclpy/src/rclpy/wait_set.hpp b/rclpy/src/rclpy/wait_set.hpp index f73052560..7d74cb1fc 100644 --- a/rclpy/src/rclpy/wait_set.hpp +++ b/rclpy/src/rclpy/wait_set.hpp @@ -21,146 +21,162 @@ #include #include "client.hpp" +#include "context.hpp" +#include "destroyable.hpp" +#include "guard_condition.hpp" #include "qos_event.hpp" #include "service.hpp" +#include "subscription.hpp" #include "timer.hpp" namespace py = pybind11; namespace rclpy { -/// Return a Capsule pointing to a zero initialized rcl_wait_set_t structure -py::capsule -get_zero_initialized_wait_set(); - -/// Initialize a wait set -/** - * Raises RCLError if the wait set could not be initialized - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] node_name string name of the node to be created - * \param[in] number_of_subscriptions int - * \param[in] number_of_guard_conditions int - * \param[in] number_of_timers int - * \param[in] number_of_clients int - * \param[in] number_of_services int - * \param[in] pycontext Capsule pointing to an rcl_context_t - */ -void -wait_set_init( - py::capsule pywait_set, - size_t number_of_subscriptions, - size_t number_of_guard_conditions, - size_t number_of_timers, - size_t number_of_clients, - size_t number_of_services, - size_t number_of_events, - py::capsule pycontext); - -/// Clear all the pointers in the wait set -/** - * Raises RCLError if any rcl error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - */ -void -wait_set_clear_entities(py::capsule pywait_set); - -/// Add an entity to the wait set structure -/** - * Raises RuntimeError if the entity type is unknown - * Raises RCLError if any lower level error occurs - * - * \param[in] entity_type string defining the entity ["subscription, client, service"] - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] pyentity Capsule pointing to the entity to add - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py::capsule pyentity); - -/// Add a service to the wait set structure -/** - * Raises RCLError if any lower level error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] service a service to add to the wait set - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_service(const py::capsule pywait_set, const Service & service); - -/// Add a client to the wait set structure -/** - * Raises RCLError if any lower level error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] client a client to add to the wait set - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_client(const py::capsule pywait_set, const Client & client); - -/// Add a timer to the wait set structure -/** - * Raises RCLError if any lower level error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] timer a timer to add to the wait set - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_timer(const py::capsule pywait_set, const Timer & timer); - -/// Add an event to the wait set structure -/** - * Raises RCLError if any lower level error occurs - * - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] event a QoSEvent to add to the wait set - * \return Index in waitset entity was added at - */ -size_t -wait_set_add_event(const py::capsule pywait_set, const QoSEvent & event); - -/// Check if an entity in the wait set is ready by its index -/** - * This must be called after waiting on the wait set. - * Raises RuntimeError if the entity type is unknown - * Raises IndexError if the given index is beyond the number of entities in the set - * - * \param[in] entity_type string defining the entity ["subscription, client, service"] - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] index location in the wait set of the entity to check - * \return True if the entity at the index in the wait set is not NULL - */ -bool -wait_set_is_ready(const std::string & entity_type, py::capsule pywait_set, size_t index); - -/// Get list of non-null entities in wait set -/** - * Raises ValueError if pywait_set is not a wait set capsule - * Raises RuntimeError if the entity type is not known - * - * \param[in] entity_type string defining the entity ["subscription, client, service"] - * \param[in] pywait_set Capsule pointing to the wait set structure - * \return List of wait set entities pointers ready for take - */ -py::list -get_ready_entities(const std::string & entity_type, py::capsule pywait_set); - -/// Wait until timeout is reached or event happened -/** - * Raises ValueError if pywait_set is not a wait set capsule - * Raises RCLError if there was an error while waiting - * - * This function will wait for an event to happen or for the timeout to expire. - * A negative timeout means wait forever, a timeout of 0 means no wait - * \param[in] pywait_set Capsule pointing to the wait set structure - * \param[in] timeout optional time to wait before waking up (in nanoseconds) - */ -void -wait(py::capsule pywait_set, int64_t timeout); +class WaitSet : public Destroyable, public std::enable_shared_from_this +{ +public: + /// Initialize a wait set + WaitSet(); + + /// Initialize a wait set + /** + * Raises RCLError if the wait set could not be initialized + * + * \param[in] node_name string name of the node to be created + * \param[in] number_of_subscriptions a positive number or zero + * \param[in] number_of_guard_conditions int + * \param[in] number_of_timers int + * \param[in] number_of_clients int + * \param[in] number_of_services int + * \param[in] pycontext Capsule pointing to an rcl_context_t + */ + WaitSet( + size_t number_of_subscriptions, + size_t number_of_guard_conditions, + size_t number_of_timers, + size_t number_of_clients, + size_t number_of_services, + size_t number_of_events, + Context & context); + + /// Clear all the pointers in the wait set + /** + * Raises RCLError if any rcl error occurs + */ + void + clear_entities(); + + /// Add a service to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] service a service to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_service(const Service & service); + + /// Add a subcription to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] service a service to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_subscription(const Subscription & subscription); + + /// Add a client to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] client a client to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_client(const Client & client); + + /// Add a guard condition to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] timer a guard condition to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_guard_condition(const GuardCondition & gc); + + /// Add a timer to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] timer a timer to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_timer(const Timer & timer); + + /// Add an event to the wait set structure + /** + * Raises RCLError if any lower level error occurs + * + * \param[in] event a QoSEvent to add to the wait set + * \return Index in waitset entity was added at + */ + size_t + add_event(const QoSEvent & event); + + /// Check if an entity in the wait set is ready by its index + /** + * This must be called after waiting on the wait set. + * Raises RuntimeError if the entity type is unknown + * Raises IndexError if the given index is beyond the number of entities in the set + * + * \param[in] entity_type string defining the entity ["subscription, client, service"] + * \param[in] index location in the wait set of the entity to check + * \return True if the entity at the index in the wait set is not NULL + */ + bool + is_ready(const std::string & entity_type, size_t index); + + /// Get list of entities ready by entity type + /** + * Raises RuntimeError if the entity type is not known + * + * \param[in] entity_type string defining the entity ["subscription, client, service"] + * \return List of wait set entities pointers ready for take + */ + py::list + get_ready_entities(const std::string & entity_type); + + /// Wait until timeout is reached or event happened + /** + * Raises RCLError if there was an error while waiting + * + * This function will wait for an event to happen or for the timeout to expire. + * A negative timeout means wait forever, a timeout of 0 means no wait + * \param[in] timeout optional time to wait before waking up (in nanoseconds) + */ + void + wait(int64_t timeout); + + /// Get rcl_wait_set_t pointer + rcl_wait_set_t * rcl_ptr() const + { + return rcl_wait_set_.get(); + } + + /// Force an early destruction of this object + void destroy() override; + +private: + Context context_; + std::shared_ptr rcl_wait_set_; +}; + +/// Define a pybind11 wrapper for an rclpy::Service +void define_waitset(py::object module); } // namespace rclpy #endif // RCLPY__WAIT_SET_HPP_ diff --git a/rclpy/src/rclpy_common/include/rclpy_common/common.h b/rclpy/src/rclpy_common/include/rclpy_common/common.h index a5ba6f069..db18e9586 100644 --- a/rclpy/src/rclpy_common/include/rclpy_common/common.h +++ b/rclpy/src/rclpy_common/include/rclpy_common/common.h @@ -32,49 +32,6 @@ typedef void destroy_ros_message_signature (void *); typedef bool convert_from_py_signature (PyObject *, void *); typedef PyObject * convert_to_py_signature (void *); -typedef struct -{ - // Important: a pointer to a structure is also a pointer to its first member. - // The subscription must be first in the struct to compare sub.handle.pointer to an address - // in a wait set. - rcl_subscription_t subscription; - rcl_node_t * node; -} rclpy_subscription_t; - -typedef struct -{ - rcl_publisher_t publisher; - rcl_node_t * node; -} rclpy_publisher_t; - -typedef struct -{ - // Important: a pointer to a structure is also a pointer to its first member. - // The client must be first in the struct to compare cli.handle.pointer to an address - // in a wait set. - rcl_client_t client; - rcl_node_t * node; -} rclpy_client_t; - -typedef struct -{ - // Important: a pointer to a structure is also a pointer to its first member. - // The service must be first in the struct to compare srv.handle.pointer to an address - // in a wait set. - rcl_service_t service; - rcl_node_t * node; -} rclpy_service_t; - -/// Finalize names and types struct with error setting. -/** - * \param[in] names_and_types The struct to finalize. - * \return `true` if finalized successfully, `false` otherwise. - * If `false`, then a Python error is set. - */ -RCLPY_COMMON_PUBLIC -bool -rclpy_names_and_types_fini(rcl_names_and_types_t * names_and_types); - /// Get the type support structure for a Python ROS message type. /** * \param[in] pymsg_type The Python ROS message type. @@ -84,17 +41,6 @@ RCLPY_COMMON_PUBLIC void * rclpy_common_get_type_support(PyObject * pymsg_type); -/// Convert a C rcl_names_and_types_t into a Python list. -/** - * \param names_and_types The names and types struct to convert. - * \return A PyList of PyTuples. The first element of each tuple is a string for the - * name and the second element is a list of strings for the types. - * \return `NULL` if there is an error. No Python error is set. - */ -RCLPY_COMMON_PUBLIC -PyObject * -rclpy_convert_to_py_names_and_types(rcl_names_and_types_t * topic_names_and_types); - /// Convert a C rmw_qos_profile_t into a Python dictionary with qos profile args. /** * \param[in] profile Pointer to a rmw_qos_profile_t to convert diff --git a/rclpy/src/rclpy_common/src/common.c b/rclpy/src/rclpy_common/src/common.c index e0788fdd8..e96319360 100644 --- a/rclpy/src/rclpy_common/src/common.c +++ b/rclpy/src/rclpy_common/src/common.c @@ -55,23 +55,6 @@ cleanup_rclpy_qos_profile(rclpy_qos_profile_t * profile) Py_XDECREF(profile->avoid_ros_namespace_conventions); } -bool -rclpy_names_and_types_fini(rcl_names_and_types_t * names_and_types) -{ - if (!names_and_types) { - return true; - } - rcl_ret_t ret = rcl_names_and_types_fini(names_and_types); - if (ret != RCL_RET_OK) { - PyErr_Format( - PyExc_RuntimeError, - "Failed to destroy rcl_names_and_types_t: %s", rcl_get_error_string().str); - rcl_reset_error(); - return false; - } - return true; -} - void * rclpy_common_get_type_support(PyObject * pymsg_type) { @@ -91,55 +74,6 @@ rclpy_common_get_type_support(PyObject * pymsg_type) return ts; } -PyObject * -rclpy_convert_to_py_names_and_types(rcl_names_and_types_t * names_and_types) -{ - if (!names_and_types) { - return NULL; - } - - PyObject * pynames_and_types = PyList_New(names_and_types->names.size); - if (!pynames_and_types) { - return NULL; - } - - size_t i; - for (i = 0; i < names_and_types->names.size; ++i) { - PyObject * pytuple = PyTuple_New(2); - if (!pytuple) { - Py_DECREF(pynames_and_types); - return NULL; - } - PyObject * pyname = PyUnicode_FromString(names_and_types->names.data[i]); - if (!pyname) { - Py_DECREF(pynames_and_types); - Py_DECREF(pytuple); - return NULL; - } - PyTuple_SET_ITEM(pytuple, 0, pyname); - PyObject * pytypes_list = PyList_New(names_and_types->types[i].size); - if (!pytypes_list) { - Py_DECREF(pynames_and_types); - Py_DECREF(pytuple); - return NULL; - } - size_t j; - for (j = 0; j < names_and_types->types[i].size; ++j) { - PyObject * pytype = PyUnicode_FromString(names_and_types->types[i].data[j]); - if (!pytype) { - Py_DECREF(pynames_and_types); - Py_DECREF(pytuple); - Py_DECREF(pytypes_list); - return NULL; - } - PyList_SET_ITEM(pytypes_list, j, pytype); - } - PyTuple_SET_ITEM(pytuple, 1, pytypes_list); - PyList_SET_ITEM(pynames_and_types, i, pytuple); - } - return pynames_and_types; -} - static PyObject * _convert_rmw_time_to_py_duration(const rmw_time_t * duration) diff --git a/rclpy/test/test_destruction.py b/rclpy/test/test_destruction.py index 97ad26bc9..e16d11fe7 100644 --- a/rclpy/test/test_destruction.py +++ b/rclpy/test/test_destruction.py @@ -38,8 +38,7 @@ def test_destroy_node_twice(): try: node = rclpy.create_node('test_node2', context=context) node.destroy_node() - with pytest.raises(InvalidHandle): - node.destroy_node() + node.destroy_node() finally: rclpy.shutdown(context=context) @@ -144,11 +143,7 @@ def test_destroy_subscription_asap(): with sub.handle: pass - with sub.handle: - node.destroy_subscription(sub) - # handle valid because it's still being used - with sub.handle: - pass + node.destroy_subscription(sub) with pytest.raises(InvalidHandle): # handle invalid because it was destroyed when no one was using it @@ -166,11 +161,8 @@ def test_destroy_node_asap(): try: node = rclpy.create_node('test_destroy_subscription_asap', context=context) - with node.handle: - node.destroy_node() - # handle valid because it's still being used - with node.handle: - pass + + node.destroy_node() with pytest.raises(InvalidHandle): # handle invalid because it was destroyed when no one was using it @@ -193,11 +185,7 @@ def test_destroy_publisher_asap(): with pub.handle: pass - with pub.handle: - node.destroy_publisher(pub) - # handle valid because it's still being used - with pub.handle: - pass + node.destroy_publisher(pub) with pytest.raises(InvalidHandle): # handle invalid because it was destroyed when no one was using it diff --git a/rclpy/test/test_handle.py b/rclpy/test/test_handle.py index 6c9cab2e8..99b016280 100644 --- a/rclpy/test/test_handle.py +++ b/rclpy/test/test_handle.py @@ -23,7 +23,7 @@ def test_handle_destroyed_immediately(): try: node = rclpy.create_node('test_handle_destroyed_immediately', context=context) - node.handle.destroy() + node.destroy_node() with pytest.raises(InvalidHandle): with node.handle: pass @@ -38,9 +38,7 @@ def test_handle_destroyed_when_not_used(): try: node = rclpy.create_node('test_handle_destroyed_when_not_used', context=context) with node.handle: - node.handle.destroy() - with node.handle: - pass + node.destroy_node() with pytest.raises(InvalidHandle): with node.handle: diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index 9f94ce323..aab804711 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -40,7 +40,6 @@ from rclpy.exceptions import ParameterImmutableException from rclpy.exceptions import ParameterNotDeclaredException from rclpy.executors import SingleThreadedExecutor -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.parameter import Parameter from rclpy.qos import qos_profile_sensor_data from rclpy.qos import QoSDurabilityPolicy @@ -155,8 +154,8 @@ def test_take(self): basic_types_pub.publish(basic_types_msg) cycle_count = 0 while cycle_count < 5: - with sub.handle as capsule: - result = _rclpy.rclpy_take(capsule, sub.msg_type, False) + with sub.handle: + result = sub.handle.take_message(sub.msg_type, False) if result is not None: msg, info = result self.assertNotEqual(0, info['source_timestamp']) diff --git a/rclpy/test/test_qos_event.py b/rclpy/test/test_qos_event.py index 447b9ba62..a94b95865 100644 --- a/rclpy/test/test_qos_event.py +++ b/rclpy/test/test_qos_event.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import gc import unittest from unittest.mock import Mock @@ -50,6 +51,11 @@ def setUp(self): self.is_fastrtps = 'rmw_fastrtps' in get_rmw_implementation_identifier() def tearDown(self): + # These tests create a bunch of events by hand instead of using Node APIs, + # so they won't be cleaned up when calling `node.destroy_node()`, but they could still + # keep the node alive from one test to the next. + # Invoke the garbage collector to destroy them. + gc.collect() self.node.destroy_node() rclpy.shutdown(context=self.context) @@ -196,8 +202,8 @@ def warn(self, message, once=False): rclpy.logging._root_logger = original_logger def _create_event_handle(self, parent_entity, event_type): - with parent_entity.handle as parent_capsule: - event = _rclpy.QoSEvent(parent_capsule, event_type) + with parent_entity.handle: + event = _rclpy.QoSEvent(parent_entity.handle, event_type) self.assertIsNotNone(event) return event @@ -237,41 +243,40 @@ def test_call_publisher_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events publisher = self.node.create_publisher(EmptyMsg, self.topic_name, 10) - wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() - with self.context.handle as context_handle: - _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 3, context_handle) + with self.context.handle: + wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, self.context.handle) deadline_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) with deadline_event_handle: - deadline_event_index = _rclpy.rclpy_wait_set_add_event(wait_set, deadline_event_handle) + deadline_event_index = wait_set.add_event(deadline_event_handle) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST) with liveliness_event_handle: - liveliness_event_index = _rclpy.rclpy_wait_set_add_event( - wait_set, liveliness_event_handle) + liveliness_event_index = wait_set.add_event( + liveliness_event_handle) self.assertIsNotNone(liveliness_event_index) try: incompatible_qos_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS) with incompatible_qos_event_handle: - incompatible_qos_event_index = _rclpy.rclpy_wait_set_add_event( - wait_set, incompatible_qos_event_handle) + incompatible_qos_event_index = wait_set.add_event( + incompatible_qos_event_handle) self.assertIsNotNone(incompatible_qos_event_index) except UnsupportedEventTypeError: self.assertTrue(self.is_fastrtps) # We live in our own namespace and have created no other participants, so # there can't be any of these events. - _rclpy.rclpy_wait(wait_set, 0) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, deadline_event_index)) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, liveliness_event_index)) + wait_set.wait(0) + self.assertFalse(wait_set.is_ready('event', deadline_event_index)) + self.assertFalse(wait_set.is_ready('event', liveliness_event_index)) if not self.is_fastrtps: - self.assertFalse(_rclpy.rclpy_wait_set_is_ready( - 'event', wait_set, incompatible_qos_event_index)) + self.assertFalse(wait_set.is_ready( + 'event', incompatible_qos_event_index)) # Calling take data even though not ready should provide me an empty initialized message # Tests data conversion utilities in C side @@ -310,42 +315,39 @@ def test_call_subscription_rclpy_event_apis(self): # Go through the exposed apis and ensure that things don't explode when called # Make no assumptions about being able to actually receive the events subscription = self.node.create_subscription(EmptyMsg, self.topic_name, Mock(), 10) - wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() - with self.context.handle as context_handle: - _rclpy.rclpy_wait_set_init(wait_set, 0, 0, 0, 0, 0, 3, context_handle) + with self.context.handle: + wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, self.context.handle) deadline_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) with deadline_event_handle: - deadline_event_index = _rclpy.rclpy_wait_set_add_event( - wait_set, deadline_event_handle) + deadline_event_index = wait_set.add_event(deadline_event_handle) self.assertIsNotNone(deadline_event_index) liveliness_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_LIVELINESS_CHANGED) with liveliness_event_handle: - liveliness_event_index = _rclpy.rclpy_wait_set_add_event( - wait_set, liveliness_event_handle) + liveliness_event_index = wait_set.add_event(liveliness_event_handle) self.assertIsNotNone(liveliness_event_index) try: incompatible_qos_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS) with incompatible_qos_event_handle: - incompatible_qos_event_index = _rclpy.rclpy_wait_set_add_event( - wait_set, incompatible_qos_event_handle) + incompatible_qos_event_index = wait_set.add_event( + incompatible_qos_event_handle) self.assertIsNotNone(incompatible_qos_event_index) except UnsupportedEventTypeError: self.assertTrue(self.is_fastrtps) # We live in our own namespace and have created no other participants, so # there can't be any of these events. - _rclpy.rclpy_wait(wait_set, 0) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, deadline_event_index)) - self.assertFalse(_rclpy.rclpy_wait_set_is_ready('event', wait_set, liveliness_event_index)) + wait_set.wait(0) + self.assertFalse(wait_set.is_ready('event', deadline_event_index)) + self.assertFalse(wait_set.is_ready('event', liveliness_event_index)) if not self.is_fastrtps: - self.assertFalse(_rclpy.rclpy_wait_set_is_ready( - 'event', wait_set, incompatible_qos_event_index)) + self.assertFalse(wait_set.is_ready( + 'event', incompatible_qos_event_index)) # Calling take data even though not ready should provide me an empty initialized message # Tests data conversion utilities in C side diff --git a/rclpy/test/test_subscription.py b/rclpy/test/test_subscription.py index e12b4e7a8..b3f994b71 100644 --- a/rclpy/test/test_subscription.py +++ b/rclpy/test/test_subscription.py @@ -16,6 +16,7 @@ import rclpy from rclpy.node import Node + from test_msgs.msg import Empty @@ -48,7 +49,13 @@ def make_mock_subscription(namespace, topic_name, cli_args=None): ('/example/topic', 'ns', '/example/topic'), ]) def test_get_subscription_topic_name(topic_name, namespace, expected): - sub = make_mock_subscription(namespace, topic_name) + node = Node('node_name', namespace=namespace, cli_args=None) + sub = node.create_subscription( + msg_type=Empty, + topic=topic_name, + callback=lambda _: None, + qos_profile=10, + ) assert sub.topic_name == expected @@ -61,5 +68,11 @@ def test_get_subscription_topic_name(topic_name, namespace, expected): '/ns/new_topic'), ]) def test_get_subscription_topic_name_after_remapping(topic_name, namespace, cli_args, expected): - sub = make_mock_subscription(namespace, topic_name, cli_args) + node = Node('node_name', namespace=namespace, cli_args=cli_args) + sub = node.create_subscription( + msg_type=Empty, + topic=topic_name, + callback=lambda _: None, + qos_profile=10, + ) assert sub.topic_name == expected diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 7e15fa2d6..f79841c51 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -41,9 +41,9 @@ class ClientWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle as node_capsule: + with node.handle: self.client = _rclpy.Client( - node_capsule, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) + node.handle, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) self.client_index = None self.client_is_ready = False @@ -52,7 +52,7 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('client', wait_set, self.client_index): + if wait_set.is_ready('client', self.client_index): self.client_is_ready = True return self.client_is_ready @@ -76,7 +76,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.client_index = _rclpy.rclpy_wait_set_add_client(wait_set, self.client) + self.client_index = wait_set.add_client(self.client) class ServerWaitable(Waitable): @@ -84,9 +84,9 @@ class ServerWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle as node_capsule: + with node.handle: self.server = _rclpy.Service( - node_capsule, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile()) + node.handle, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile()) self.server_index = None self.server_is_ready = False @@ -95,7 +95,7 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('service', wait_set, self.server_index): + if wait_set.is_ready('service', self.server_index): self.server_is_ready = True return self.server_is_ready @@ -119,7 +119,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.server_index = _rclpy.rclpy_wait_set_add_service(wait_set, self.server) + self.server_index = wait_set.add_service(self.server) class TimerWaitable(Waitable): @@ -129,9 +129,9 @@ def __init__(self, node): self._clock = Clock(clock_type=ClockType.STEADY_TIME) period_nanoseconds = 10000 - with self._clock.handle, node.context.handle as context_capsule: + with self._clock.handle, node.context.handle: self.timer = _rclpy.Timer( - self._clock.handle, context_capsule, period_nanoseconds) + self._clock.handle, node.context.handle, period_nanoseconds) self.timer_index = None self.timer_is_ready = False @@ -140,7 +140,7 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('timer', wait_set, self.timer_index): + if wait_set.is_ready('timer', self.timer_index): self.timer_is_ready = True return self.timer_is_ready @@ -165,7 +165,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.timer_index = _rclpy.rclpy_wait_set_add_timer(wait_set, self.timer) + self.timer_index = wait_set.add_timer(self.timer) class SubscriptionWaitable(Waitable): @@ -173,9 +173,9 @@ class SubscriptionWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle as node_capsule: - self.subscription = _rclpy.rclpy_create_subscription( - node_capsule, EmptyMsg, 'test_topic', QoSProfile(depth=10).get_c_qos_profile()) + with node.handle: + self.subscription = _rclpy.Subscription( + node.handle, EmptyMsg, 'test_topic', QoSProfile(depth=10).get_c_qos_profile()) self.subscription_index = None self.subscription_is_ready = False @@ -184,7 +184,7 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('subscription', wait_set, self.subscription_index): + if wait_set.is_ready('subscription', self.subscription_index): self.subscription_is_ready = True return self.subscription_is_ready @@ -192,7 +192,7 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" if self.subscription_is_ready: self.subscription_is_ready = False - msg_info = _rclpy.rclpy_take(self.subscription, EmptyMsg, False) + msg_info = self.subscription.take_message(EmptyMsg, False) if msg_info is not None: return msg_info[0] return None @@ -210,8 +210,8 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.subscription_index = _rclpy.rclpy_wait_set_add_entity( - 'subscription', wait_set, self.subscription) + self.subscription_index = wait_set.add_subscription( + self.subscription) class GuardConditionWaitable(Waitable): @@ -219,8 +219,8 @@ class GuardConditionWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.context.handle as context_capsule: - self.guard_condition = _rclpy.rclpy_create_guard_condition(context_capsule) + with node.context.handle: + self.guard_condition = _rclpy.GuardCondition(node.context.handle) self.guard_condition_index = None self.guard_is_ready = False @@ -229,7 +229,7 @@ def __init__(self, node): def is_ready(self, wait_set): """Return True if entities are ready in the wait set.""" - if _rclpy.rclpy_wait_set_is_ready('guard_condition', wait_set, self.guard_condition_index): + if wait_set.is_ready('guard_condition', self.guard_condition_index): self.guard_is_ready = True return self.guard_is_ready @@ -253,8 +253,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.guard_condition_index = _rclpy.rclpy_wait_set_add_entity( - 'guard_condition', wait_set, self.guard_condition) + self.guard_condition_index = wait_set.add_guard_condition(self.guard_condition) class MutuallyExclusiveWaitable(Waitable): @@ -369,7 +368,7 @@ def test_waitable_with_guard_condition(self): self.node.add_waitable(self.waitable) thr = self.start_spin_thread(self.waitable) - _rclpy.rclpy_trigger_guard_condition(self.waitable.guard_condition) + self.waitable.guard_condition.trigger_guard_condition() thr.join() assert self.waitable.future.done()