diff --git a/rclpy/CMakeLists.txt b/rclpy/CMakeLists.txt index 1481e6a05..9d0379dec 100644 --- a/rclpy/CMakeLists.txt +++ b/rclpy/CMakeLists.txt @@ -148,13 +148,11 @@ install(TARGETS rclpy_common # Split from main extension and converted to pybind11 pybind11_add_module(_rclpy_pybind11 SHARED + src/rclpy/_rclpy_action.cpp src/rclpy/_rclpy_handle.cpp src/rclpy/_rclpy_logging.cpp src/rclpy/_rclpy_pybind11.cpp src/rclpy/_rclpy_pycapsule.cpp - src/rclpy/action_client.cpp - src/rclpy/action_goal_handle.cpp - src/rclpy/action_server.cpp src/rclpy/client.cpp src/rclpy/clock.cpp src/rclpy/context.cpp @@ -163,6 +161,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED src/rclpy/graph.cpp src/rclpy/guard_condition.cpp src/rclpy/handle.cpp + src/rclpy/init.cpp src/rclpy/logging.cpp src/rclpy/names.cpp src/rclpy/node.cpp diff --git a/rclpy/rclpy/action/client.py b/rclpy/rclpy/action/client.py index d476dc131..0532ea24b 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: - self._client_handle = _rclpy.ActionClient( - node.handle, + with node.handle as node_capsule: + self._client_handle = _rclpy.rclpy_action_create_client( + node_capsule, action_type, action_name, goal_service_qos_profile.get_c_qos_profile(), @@ -220,7 +220,9 @@ def _remove_pending_result_request(self, future): # Start Waitable API def is_ready(self, wait_set): """Return True if one or more entities are ready in the wait set.""" - ready_entities = self._client_handle.is_ready(wait_set) + ready_entities = _rclpy.rclpy_action_wait_set_is_ready( + self._client_handle, + wait_set) self._is_feedback_ready = ready_entities[0] self._is_status_ready = ready_entities[1] self._is_goal_response_ready = ready_entities[2] @@ -232,36 +234,36 @@ def take_data(self): """Take stuff from lower level so the wait set doesn't immediately wake again.""" data = {} if self._is_goal_response_ready: - taken_data = self._client_handle.take_goal_response( - self._action_type.Impl.SendGoalService.Response) + taken_data = _rclpy.rclpy_action_take_goal_response( + self._client_handle, self._action_type.Impl.SendGoalService.Response) # If take fails, then we get (None, None) if all(taken_data): data['goal'] = taken_data if self._is_cancel_response_ready: - taken_data = self._client_handle.take_cancel_response( - self._action_type.Impl.CancelGoalService.Response) + taken_data = _rclpy.rclpy_action_take_cancel_response( + self._client_handle, self._action_type.Impl.CancelGoalService.Response) # If take fails, then we get (None, None) if all(taken_data): data['cancel'] = taken_data if self._is_result_response_ready: - taken_data = self._client_handle.take_result_response( - self._action_type.Impl.GetResultService.Response) + taken_data = _rclpy.rclpy_action_take_result_response( + self._client_handle, self._action_type.Impl.GetResultService.Response) # If take fails, then we get (None, None) if all(taken_data): data['result'] = taken_data if self._is_feedback_ready: - taken_data = self._client_handle.take_feedback( - self._action_type.Impl.FeedbackMessage) + taken_data = _rclpy.rclpy_action_take_feedback( + self._client_handle, self._action_type.Impl.FeedbackMessage) # If take fails, then we get None if taken_data is not None: data['feedback'] = taken_data if self._is_status_ready: - taken_data = self._client_handle.take_status( - self._action_type.Impl.GoalStatusMessage) + taken_data = _rclpy.rclpy_action_take_status( + self._client_handle, self._action_type.Impl.GoalStatusMessage) # If take fails, then we get None if taken_data is not None: data['status'] = taken_data @@ -345,12 +347,12 @@ async def execute(self, taken_data): def get_num_entities(self): """Return number of each type of entity used in the wait set.""" - num_entities = self._client_handle.get_num_entities() + num_entities = _rclpy.rclpy_action_wait_set_get_num_entities(self._client_handle) return NumberOfEntities(*num_entities) def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self._client_handle.add_to_waitset(wait_set) + _rclpy.rclpy_action_wait_set_add(self._client_handle, wait_set) # End Waitable API def send_goal(self, goal, **kwargs): @@ -421,7 +423,8 @@ def send_goal_async(self, goal, feedback_callback=None, goal_uuid=None): request = self._action_type.Impl.SendGoalService.Request() request.goal_id = self._generate_random_uuid() if goal_uuid is None else goal_uuid request.goal = goal - sequence_number = self._client_handle.send_goal_request(request) + sequence_number = _rclpy.rclpy_action_send_goal_request( + self._client_handle, request) if sequence_number in self._pending_goal_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending goal request'.format(sequence_number)) @@ -479,7 +482,9 @@ def _cancel_goal_async(self, goal_handle): cancel_request = CancelGoal.Request() cancel_request.goal_info.goal_id = goal_handle.goal_id - sequence_number = self._client_handle.send_cancel_request(cancel_request) + sequence_number = _rclpy.rclpy_action_send_cancel_request( + self._client_handle, + cancel_request) if sequence_number in self._pending_cancel_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending cancel request'.format(sequence_number)) @@ -531,7 +536,9 @@ def _get_result_async(self, goal_handle): result_request = self._action_type.Impl.GetResultService.Request() result_request.goal_id = goal_handle.goal_id - sequence_number = self._client_handle.send_result_request(result_request) + sequence_number = _rclpy.rclpy_action_send_result_request( + self._client_handle, + result_request) if sequence_number in self._pending_result_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending result request'.format(sequence_number)) @@ -550,8 +557,10 @@ def server_is_ready(self): :return: True if an action server is ready, False otherwise. """ - with self._node.handle: - return self._client_handle.is_action_server_available() + with self._node.handle as node_capsule: + return _rclpy.rclpy_action_server_is_available( + node_capsule, + self._client_handle) def wait_for_server(self, timeout_sec=None): """ @@ -578,8 +587,8 @@ def destroy(self): """Destroy the underlying action client handle.""" if self._client_handle is None: return - with self._node.handle: - self._client_handle.destroy_when_not_in_use() + with self._node.handle as node_capsule: + _rclpy.rclpy_action_destroy_entity(self._client_handle, node_capsule) self._node.remove_waitable(self) self._client_handle = None diff --git a/rclpy/rclpy/action/graph.py b/rclpy/rclpy/action/graph.py index 362e48b97..828fd9a8c 100644 --- a/rclpy/rclpy/action/graph.py +++ b/rclpy/rclpy/action/graph.py @@ -15,6 +15,7 @@ from typing import List from typing import Tuple +from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.node import Node @@ -33,9 +34,9 @@ def get_action_client_names_and_types_by_node( The first element of each tuple is the action name and the second element is a list of action types. """ - with node.handle: - return node.handle.get_action_client_names_and_types_by_node( - remote_node_name, remote_node_namespace) + with node.handle as node_capsule: + return _rclpy.rclpy_action_get_client_names_and_types_by_node( + node_capsule, remote_node_name, remote_node_namespace) def get_action_server_names_and_types_by_node( @@ -53,9 +54,9 @@ def get_action_server_names_and_types_by_node( The first element of each tuple is the action name and the second element is a list of action types. """ - with node.handle: - return node.handle.get_action_server_names_and_types_by_node( - remote_node_name, remote_node_namespace) + with node.handle as node_capsule: + return _rclpy.rclpy_action_get_server_names_and_types_by_node( + node_capsule, remote_node_name, remote_node_namespace) def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]: @@ -67,5 +68,5 @@ def get_action_names_and_types(node: Node) -> List[Tuple[str, List[str]]]: The first element of each tuple is the action name and the second element is a list of action types. """ - with node.handle: - return node.handle.get_action_names_and_types() + with node.handle as node_capsule: + return _rclpy.rclpy_action_get_names_and_types(node_capsule) diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py index 64e7aab72..7df498bfa 100644 --- a/rclpy/rclpy/action/server.py +++ b/rclpy/rclpy/action/server.py @@ -43,7 +43,14 @@ class CancelResponse(Enum): ACCEPT = 2 -GoalEvent = _rclpy.GoalEvent +class GoalEvent(Enum): + """Goal events that cause state transitions.""" + + EXECUTE = 1 + CANCEL_GOAL = 2 + SUCCEED = 3 + ABORT = 4 + CANCELED = 5 class ServerGoalHandle: @@ -62,7 +69,8 @@ def __init__(self, action_server, goal_info, goal_request): :param goal_info: GoalInfo message. :param goal_request: The user defined goal request message from an ActionClient. """ - self._goal_handle = _rclpy.ActionGoalHandle(action_server._handle, goal_info) + self._handle = _rclpy.rclpy_action_accept_new_goal( + action_server._handle, goal_info) self._action_server = action_server self._goal_info = goal_info self._goal_request = goal_request @@ -88,9 +96,9 @@ def goal_id(self): @property def is_active(self): with self._lock: - if self._goal_handle is None: + if self._handle is None: return False - return self._goal_handle.is_active() + return _rclpy.rclpy_action_goal_handle_is_active(self._handle) @property def is_cancel_requested(self): @@ -99,24 +107,24 @@ def is_cancel_requested(self): @property def status(self): with self._lock: - if self._goal_handle is None: + if self._handle is None: return GoalStatus.STATUS_UNKNOWN - return self._goal_handle.get_status() + return _rclpy.rclpy_action_goal_handle_get_status(self._handle) def _update_state(self, event): with self._lock: # Ignore updates for already destructed goal handles - if self._goal_handle is None: + if self._handle is None: return # Update state - self._goal_handle.update_goal_state(event) + _rclpy.rclpy_action_update_goal_state(self._handle, event.value) # Publish state change - self._action_server._handle.publish_status() + _rclpy.rclpy_action_publish_status(self._action_server._handle) # If it's a terminal state, then also notify the action server - if not self._goal_handle.is_active(): + if not _rclpy.rclpy_action_goal_handle_is_active(self._handle): self._action_server.notify_goal_done() def execute(self, execute_callback=None): @@ -124,7 +132,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(_rclpy.GoalEvent.EXECUTE) + self._update_state(GoalEvent.EXECUTE) self._action_server.notify_execute(self, execute_callback) def publish_feedback(self, feedback): @@ -133,7 +141,7 @@ def publish_feedback(self, feedback): with self._lock: # Ignore for already destructed goal handles - if self._goal_handle is None: + if self._handle is None: return # Populate the feedback message with metadata about this goal @@ -143,23 +151,24 @@ def publish_feedback(self, feedback): feedback_message.feedback = feedback # Publish - self._action_server._handle.publish_feedback(feedback_message) + _rclpy.rclpy_action_publish_feedback( + self._action_server._handle, feedback_message) def succeed(self): - self._update_state(_rclpy.GoalEvent.SUCCEED) + self._update_state(GoalEvent.SUCCEED) def abort(self): - self._update_state(_rclpy.GoalEvent.ABORT) + self._update_state(GoalEvent.ABORT) def canceled(self): - self._update_state(_rclpy.GoalEvent.CANCELED) + self._update_state(GoalEvent.CANCELED) def destroy(self): with self._lock: - if self._goal_handle is None: + if self._handle is None: return - self._goal_handle.destroy_when_not_in_use() - self._goal_handle = None + _rclpy.rclpy_action_destroy_server_goal_handle(self._handle) + self._handle = None self._action_server.remove_future(self._result_future) @@ -240,9 +249,9 @@ def __init__( check_for_type_support(action_type) self._node = node self._action_type = action_type - with node.handle, node.get_clock().handle: - self._handle = _rclpy.ActionServer( - node.handle, + with node.handle as node_capsule, node.get_clock().handle: + self._handle = _rclpy.rclpy_action_create_server( + node_capsule, node.get_clock().handle, action_type, action_name, @@ -270,7 +279,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 = self._handle.goal_exists(goal_info) + goal_id_exists = _rclpy.rclpy_action_server_goal_exists(self._handle, goal_info) accepted = False if not goal_id_exists: @@ -301,7 +310,11 @@ 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 - self._handle.send_goal_response(request_header, response_msg) + _rclpy.rclpy_action_send_goal_response( + self._handle, + request_header, + response_msg, + ) if not accepted: self._node.get_logger().debug('New goal rejected: {0}'.format(goal_uuid.uuid)) @@ -347,8 +360,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 = self._handle.process_cancel_request( - cancel_request, self._action_type.Impl.CancelGoalService.Response) + cancel_response = _rclpy.rclpy_action_process_cancel_request( + self._handle, cancel_request, self._action_type.Impl.CancelGoalService.Response) for goal_info in cancel_response.goals_canceling: goal_uuid = bytes(goal_info.goal_id.uuid) @@ -362,12 +375,16 @@ async def _execute_cancel_request(self, request_header_and_message): if CancelResponse.ACCEPT == response: # Notify goal handle - goal_handle._update_state(_rclpy.GoalEvent.CANCEL_GOAL) + goal_handle._update_state(GoalEvent.CANCEL_GOAL) else: # Remove from response cancel_response.goals_canceling.remove(goal_info) - self._handle.send_cancel_response(request_header, cancel_response) + _rclpy.rclpy_action_send_cancel_response( + self._handle, + request_header, + cancel_response, + ) async def _execute_get_result_request(self, request_header_and_message): request_header, result_request = request_header_and_message @@ -382,7 +399,11 @@ 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 - self._handle.send_result_response(request_header, result_response) + _rclpy.rclpy_action_send_result_response( + self._handle, + request_header, + result_response, + ) return # There is an accepted goal matching the goal ID, register a callback to send the @@ -396,7 +417,11 @@ async def _execute_expire_goals(self, expired_goals): del self._goal_handles[goal_uuid] def _send_result_response(self, request_header, future): - self._handle.send_result_response(request_header, future.result()) + _rclpy.rclpy_action_send_result_response( + self._handle, + request_header, + future.result(), + ) @property def action_type(self): @@ -406,7 +431,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 = self._handle.is_ready(wait_set) + ready_entities = _rclpy.rclpy_action_wait_set_is_ready(self._handle, 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] @@ -418,7 +443,8 @@ def take_data(self): data = {} if self._is_goal_request_ready: with self._lock: - taken_data = self._handle.take_goal_request( + taken_data = _rclpy.rclpy_action_take_goal_request( + self._handle, self._action_type.Impl.SendGoalService.Request, ) # If take fails, then we get (None, None) @@ -427,7 +453,8 @@ def take_data(self): if self._is_cancel_request_ready: with self._lock: - taken_data = self._handle.take_cancel_request( + taken_data = _rclpy.rclpy_action_take_cancel_request( + self._handle, self._action_type.Impl.CancelGoalService.Request, ) # If take fails, then we get (None, None) @@ -436,7 +463,8 @@ def take_data(self): if self._is_result_request_ready: with self._lock: - taken_data = self._handle.take_result_request( + taken_data = _rclpy.rclpy_action_take_result_request( + self._handle, self._action_type.Impl.GetResultService.Request, ) # If take fails, then we get (None, None) @@ -445,7 +473,10 @@ def take_data(self): if self._is_goal_expired: with self._lock: - data['expired'] = self._handle.expire_goals(len(self._goal_handles)) + data['expired'] = _rclpy.rclpy_action_expire_goals( + self._handle, + len(self._goal_handles), + ) return data @@ -470,7 +501,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 = self._handle.get_num_entities() + num_entities = _rclpy.rclpy_action_wait_set_get_num_entities(self._handle) return NumberOfEntities( num_entities[0], num_entities[1], @@ -481,7 +512,7 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" with self._lock: - self._handle.add_to_waitset(wait_set) + _rclpy.rclpy_action_wait_set_add(self._handle, wait_set) # End Waitable API def notify_execute(self, goal_handle, execute_callback): @@ -496,7 +527,7 @@ def notify_execute(self, goal_handle, execute_callback): def notify_goal_done(self): with self._lock: - self._handle.notify_goal_done() + _rclpy.rclpy_action_notify_goal_done(self._handle) def register_handle_accepted_callback(self, handle_accepted_callback): """ @@ -584,7 +615,8 @@ def destroy(self): for goal_handle in self._goal_handles.values(): goal_handle.destroy() - self._handle.destroy_when_not_in_use() + with self._node.handle as node_capsule: + _rclpy.rclpy_action_destroy_entity(self._handle, node_capsule) self._node.remove_waitable(self) self._handle = None diff --git a/rclpy/rclpy/context.py b/rclpy/rclpy/context.py index f7a45ff8b..d133ddb1d 100644 --- a/rclpy/rclpy/context.py +++ b/rclpy/rclpy/context.py @@ -34,6 +34,9 @@ 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() @@ -41,10 +44,7 @@ def __init__(self): @property def handle(self): - return self.__context - - def destroy(self): - self.__context.destroy_when_not_in_use() + return self._handle def init(self, args: Optional[List[str]] = None, @@ -57,36 +57,31 @@ 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 as _rclpy + from rclpy.impl.implementation_singleton import rclpy_implementation global g_logging_ref_count - with self._lock: + with self._handle as capsule, 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)) - 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 + 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 def ok(self): """Check if context hasn't been shut down.""" # imported locally to avoid loading extensions on module import - try: - with self.__context, self._lock: - return self.__context.ok() - except AttributeError: - return False + from rclpy.impl.implementation_singleton import rclpy_implementation + with self._handle as capsule, self._lock: + return rclpy_implementation.rclpy_ok(capsule) def _call_on_shutdown_callbacks(self): with self._callbacks_lock: @@ -99,17 +94,19 @@ def _call_on_shutdown_callbacks(self): def shutdown(self): """Shutdown this context.""" # imported locally to avoid loading extensions on module import - with self.__context, self._lock: - self.__context.shutdown() + from rclpy.impl.implementation_singleton import rclpy_implementation + with self._handle as capsule, self._lock: + rclpy_implementation.rclpy_shutdown(capsule) 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 - with self.__context, self._lock: - if self.__context.ok(): - self.__context.shutdown() + 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) self._call_on_shutdown_callbacks() def _remove_callback(self, weak_method): @@ -141,5 +138,6 @@ def _logging_fini(self): def get_domain_id(self): """Get domain id of context.""" - with self.__context, self._lock: - return self.__context.get_domain_id() + from rclpy.impl.implementation_singleton import rclpy_implementation + with self._handle as capsule, self._lock: + return rclpy_implementation.rclpy_context_get_domain_id(capsule) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 09a34d661..a4cc77eaa 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: - msg_info = sub.handle.take_message(sub.msg_type, sub.raw) + with sub.handle as capsule: + msg_info = _rclpy.rclpy_take(capsule, sub.msg_type, sub.raw) if msg_info is not None: return msg_info[0] return None @@ -507,13 +507,12 @@ def _wait_for_ready_callbacks( len(subscriptions), len(guards), len(timers), len(clients), len(services)) # Construct a wait set - wait_set = None + wait_set = _rclpy.rclpy_get_zero_initialized_wait_set() with ExitStack() as context_stack: - sub_handles = [] + sub_capsules = [] for sub in subscriptions: try: - context_stack.enter_context(sub.handle) - sub_handles.append(sub.handle) + sub_capsules.append(context_stack.enter_context(sub.handle)) except InvalidHandle: entity_count.num_subscriptions -= 1 @@ -541,11 +540,10 @@ def _wait_for_ready_callbacks( except InvalidHandle: entity_count.num_timers -= 1 - guard_handles = [] + guard_capsules = [] for gc in guards: try: - context_stack.enter_context(gc.handle) - guard_handles.append(gc.handle) + guard_capsules.append(context_stack.enter_context(gc.handle)) except InvalidHandle: entity_count.num_guard_conditions -= 1 @@ -556,44 +554,44 @@ def _wait_for_ready_callbacks( except InvalidHandle: pass - context_stack.enter_context(self._context.handle) - - wait_set = _rclpy.WaitSet( + context_capsule = context_stack.enter_context(self._context.handle) + _rclpy.rclpy_wait_set_init( + wait_set, 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, - self._context.handle) + context_capsule) - wait_set.clear_entities() - for sub_handle in sub_handles: - wait_set.add_subscription(sub_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) for cli_handle in client_handles: - wait_set.add_client(cli_handle) + _rclpy.rclpy_wait_set_add_client(wait_set, cli_handle) for srv_capsule in service_handles: - wait_set.add_service(srv_capsule) + _rclpy.rclpy_wait_set_add_service(wait_set, srv_capsule) for tmr_handle in timer_handles: - wait_set.add_timer(tmr_handle) - for gc_handle in guard_handles: - wait_set.add_guard_condition(gc_handle) + _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) for waitable in waitables: waitable.add_to_wait_set(wait_set) # Wait for something to become ready - wait_set.wait(timeout_nsec) + _rclpy.rclpy_wait(wait_set, timeout_nsec) if self._is_shutdown: raise ShutdownException() if not self._context.ok(): raise ExternalShutdownException() # get ready entities - 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') + 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) # 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 8b83df621..319b55fb8 100644 --- a/rclpy/rclpy/guard_condition.py +++ b/rclpy/rclpy/guard_condition.py @@ -12,6 +12,7 @@ # 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 @@ -20,8 +21,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: - self.__gc = _rclpy.GuardCondition(self._context.handle) + with self._context.handle as capsule: + self.__handle = Handle(_rclpy.rclpy_create_guard_condition(capsule)) self.callback = callback self.callback_group = callback_group # True when the callback is ready to fire but has not been "taken" by an executor @@ -30,12 +31,12 @@ def __init__(self, callback, callback_group, context=None): self._executor_triggered = False def trigger(self): - with self.__gc: - self.__gc.trigger_guard_condition() + with self.handle as capsule: + _rclpy.rclpy_trigger_guard_condition(capsule) @property def handle(self): - return self.__gc + return self.__handle def destroy(self): - self.handle.destroy_when_not_in_use() + self.handle.destroy() diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 4a090704b..a4ad1560b 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -56,6 +56,7 @@ 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 @@ -169,16 +170,16 @@ def __init__( namespace = namespace or '' if not self._context.ok(): raise NotInitializedException('cannot create node') - with self._context.handle: + with self._context.handle as capsule: try: - self.__node = _rclpy.Node( + self.__handle = Handle(_rclpy.rclpy_create_node( node_name, namespace, - self._context.handle, + capsule, 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) @@ -190,16 +191,16 @@ def __init__( validate_namespace(namespace) # Should not get to this point raise RuntimeError('rclpy_create_node failed for unknown reason') - with self.handle: - self._logger = get_logger(self.__node.logger_name()) + with self.handle as capsule: + self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(capsule)) self.__executor_weakref = None self._parameter_event_publisher = self.create_publisher( ParameterEvent, '/parameter_events', qos_profile_parameter_events) - with self.handle: - self._parameter_overrides = self.__node.get_parameters(Parameter) + with self.handle as capsule: + self._parameter_overrides = _rclpy.rclpy_get_node_parameters(Parameter, capsule) # 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: @@ -304,7 +305,7 @@ def handle(self): :raises: AttributeError if modified after creation. """ - return self.__node + return self.__handle @handle.setter def handle(self, value): @@ -312,13 +313,13 @@ def handle(self, value): def get_name(self) -> str: """Get the name of the node.""" - with self.handle: - return self.handle.get_node_name() + with self.handle as capsule: + return _rclpy.rclpy_get_node_name(capsule) def get_namespace(self) -> str: """Get the namespace of the node.""" - with self.handle: - return self.handle.get_namespace() + with self.handle as capsule: + return _rclpy.rclpy_get_node_namespace(capsule) def get_clock(self) -> Clock: """Get the clock used by the node.""" @@ -1189,8 +1190,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: - return _rclpy.rclpy_resolve_name(self.handle, topic, only_expand, False) + with self.handle as capsule: + return _rclpy.rclpy_resolve_name(capsule, topic, only_expand, False) def resolve_service_name( self, service: str, *, only_expand: bool = False @@ -1203,8 +1204,8 @@ def resolve_service_name( :return: a fully qualified service name, result of applying expansion and remapping to the given `service`. """ - with self.handle: - return _rclpy.rclpy_resolve_name(self.handle, service, only_expand, True) + with self.handle as capsule: + return _rclpy.rclpy_resolve_name(capsule, service, only_expand, True) def create_publisher( self, @@ -1255,21 +1256,22 @@ def create_publisher( failed = False check_is_valid_msg_type(msg_type) try: - with self.handle: - publisher_object = _rclpy.Publisher( - self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) + with self.handle as node_capsule: + publisher_capsule = _rclpy.rclpy_create_publisher( + node_capsule, 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_object, msg_type, topic, qos_profile, + publisher_handle, msg_type, topic, qos_profile, event_callbacks=event_callbacks or PublisherEventCallbacks(), callback_group=callback_group) except Exception: - publisher_object.destroy_when_not_in_use() + publisher_handle.destroy() raise self.__publishers.append(publisher) self._wake_executor() @@ -1332,21 +1334,22 @@ def create_subscription( failed = False check_is_valid_msg_type(msg_type) try: - with self.handle: - subscription_object = _rclpy.Subscription( - self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) + with self.handle as capsule: + subscription_capsule = _rclpy.rclpy_create_subscription( + capsule, 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_object, msg_type, + subscription_handle, msg_type, topic, callback, callback_group, qos_profile, raw, event_callbacks=event_callbacks or SubscriptionEventCallbacks()) except Exception: - subscription_object.destroy_when_not_in_use() + subscription_handle.destroy() raise self.__subscriptions.append(subscription) callback_group.add_entity(subscription) @@ -1379,9 +1382,9 @@ def create_client( check_is_valid_srv_type(srv_type) failed = False try: - with self.handle: + with self.handle as node_capsule: client_impl = _rclpy.Client( - self.handle, + node_capsule, srv_type, srv_name, qos_profile.get_c_qos_profile()) @@ -1424,9 +1427,9 @@ def create_service( check_is_valid_srv_type(srv_type) failed = False try: - with self.handle: + with self.handle as node_capsule: service_impl = _rclpy.Service( - self.handle, + node_capsule, srv_type, srv_name, qos_profile.get_c_qos_profile()) @@ -1652,7 +1655,7 @@ def destroy_node(self) -> bool: self.destroy_timer(self.__timers[0]) while self.__guards: self.destroy_guard_condition(self.__guards[0]) - self.__node.destroy_when_not_in_use() + self.handle.destroy() self._wake_executor() def get_publisher_names_and_types_by_node( @@ -1673,9 +1676,9 @@ def get_publisher_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle: + with self.handle as capsule: return _rclpy.rclpy_get_publisher_names_and_types_by_node( - self.handle, no_demangle, node_name, node_namespace) + capsule, no_demangle, node_name, node_namespace) def get_subscriber_names_and_types_by_node( self, @@ -1695,9 +1698,9 @@ def get_subscriber_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle: + with self.handle as capsule: return _rclpy.rclpy_get_subscriber_names_and_types_by_node( - self.handle, no_demangle, node_name, node_namespace) + capsule, no_demangle, node_name, node_namespace) def get_service_names_and_types_by_node( self, @@ -1715,9 +1718,9 @@ def get_service_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle: + with self.handle as capsule: return _rclpy.rclpy_get_service_names_and_types_by_node( - self.handle, node_name, node_namespace) + capsule, node_name, node_namespace) def get_client_names_and_types_by_node( self, @@ -1735,9 +1738,9 @@ def get_client_names_and_types_by_node( :raise NodeNameNonExistentError: If the node wasn't found. :raise RuntimeError: Unexpected failure. """ - with self.handle: + with self.handle as capsule: return _rclpy.rclpy_get_client_names_and_types_by_node( - self.handle, node_name, node_namespace) + capsule, node_name, node_namespace) def get_topic_names_and_types(self, no_demangle: bool = False) -> List[Tuple[str, List[str]]]: """ @@ -1748,8 +1751,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: - return _rclpy.rclpy_get_topic_names_and_types(self.handle, no_demangle) + with self.handle as capsule: + return _rclpy.rclpy_get_topic_names_and_types(capsule, no_demangle) def get_service_names_and_types(self) -> List[Tuple[str, List[str]]]: """ @@ -1759,8 +1762,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: - return _rclpy.rclpy_get_service_names_and_types(self.handle) + with self.handle as capsule: + return _rclpy.rclpy_get_service_names_and_types(capsule) def get_node_names(self) -> List[str]: """ @@ -1768,8 +1771,8 @@ def get_node_names(self) -> List[str]: :return: List of node names. """ - with self.handle: - names_ns = self.handle.get_node_names_and_namespaces() + with self.handle as capsule: + names_ns = _rclpy.rclpy_get_node_names_and_namespaces(capsule) return [n[0] for n in names_ns] def get_node_names_and_namespaces(self) -> List[Tuple[str, str]]: @@ -1778,8 +1781,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: - return self.handle.get_node_names_and_namespaces() + with self.handle as capsule: + return _rclpy.rclpy_get_node_names_and_namespaces(capsule) def get_node_names_and_namespaces_with_enclaves(self) -> List[Tuple[str, str, str]]: """ @@ -1788,8 +1791,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: - return self.handle.get_node_names_and_namespaces_with_enclaves() + with self.handle as capsule: + return _rclpy.rclpy_get_node_names_and_namespaces_with_enclaves(capsule) def get_fully_qualified_name(self) -> str: """ @@ -1797,14 +1800,14 @@ def get_fully_qualified_name(self) -> str: :return: Fully qualified node name. """ - with self.handle: - return self.handle.get_fully_qualified_name() + with self.handle as capsule: + return _rclpy.rclpy_node_get_fully_qualified_name(capsule) 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: - return func(fq_topic_name) + with self.handle as node_capsule: + return func(node_capsule, fq_topic_name) def count_publishers(self, topic_name: str) -> int: """ @@ -1817,9 +1820,7 @@ 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. """ - with self.handle: - return self._count_publishers_or_subscribers( - topic_name, self.handle.get_count_publishers) + return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_publishers) def count_subscribers(self, topic_name: str) -> int: """ @@ -1832,9 +1833,7 @@ 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. """ - with self.handle: - return self._count_publishers_or_subscribers( - topic_name, self.handle.get_count_subscribers) + return self._count_publishers_or_subscribers(topic_name, _rclpy.rclpy_count_subscribers) def _get_info_by_topic( self, @@ -1842,16 +1841,16 @@ def _get_info_by_topic( no_mangle: bool, func: Callable[[object, str, bool], List[Dict]] ) -> List[TopicEndpointInfo]: - with self.handle: + with self.handle as node_capsule: 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(self.handle, fq_topic_name) + fq_topic_name = _rclpy.rclpy_remap_topic_name(node_capsule, fq_topic_name) - info_dicts = func(self.handle, fq_topic_name, no_mangle) + info_dicts = func(node_capsule, 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 ccd41e58f..eb9c31d7c 100644 --- a/rclpy/rclpy/publisher.py +++ b/rclpy/rclpy/publisher.py @@ -15,6 +15,7 @@ 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 @@ -27,7 +28,7 @@ class Publisher: def __init__( self, - publisher_impl: _rclpy.Publisher, + publisher_handle: Handle, msg_type: MsgType, topic: str, qos_profile: QoSProfile, @@ -43,18 +44,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_impl: Publisher wrapping the underlying ``rcl_publisher_t`` object. + :param publisher_handle: Capsule pointing to 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.__publisher = publisher_impl + self.__handle = publisher_handle 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_impl, topic) + callback_group, publisher_handle, topic) def publish(self, msg: Union[MsgType, bytes]) -> None: """ @@ -64,32 +65,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: + with self.handle as capsule: if isinstance(msg, self.msg_type): - self.__publisher.publish(msg) + _rclpy.rclpy_publish(capsule, msg) elif isinstance(msg, bytes): - self.__publisher.publish_raw(msg) + _rclpy.rclpy_publish_raw(capsule, 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: - return self.__publisher.get_subscription_count() + with self.handle as capsule: + return _rclpy.rclpy_publisher_get_subscription_count(capsule) @property def topic_name(self) -> str: - with self.handle: - return self.__publisher.get_topic_name() + with self.handle as capsule: + return _rclpy.rclpy_publisher_get_topic_name(capsule) @property def handle(self): - return self.__publisher + return self.__handle def destroy(self): for handler in self.event_handlers: handler.destroy() - self.__publisher.destroy_when_not_in_use() + self.handle.destroy() def assert_liveliness(self) -> None: """ @@ -98,5 +99,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: - _rclpy.rclpy_assert_liveliness(self.handle) + with self.handle as capsule: + _rclpy.rclpy_assert_liveliness(capsule) diff --git a/rclpy/rclpy/qos_event.py b/rclpy/rclpy/qos_event.py index 4969816da..1e0edf33e 100644 --- a/rclpy/rclpy/qos_event.py +++ b/rclpy/rclpy/qos_event.py @@ -19,6 +19,7 @@ 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 @@ -69,16 +70,15 @@ def __init__( callback_group: CallbackGroup, callback: Callable, event_type: IntEnum, - parent_impl, + parent_handle: Handle, ): # Waitable init adds self to callback_group super().__init__(callback_group) self.event_type = event_type self.callback = callback - with parent_impl: - self.__event = _rclpy.QoSEvent(parent_impl, event_type) - + with parent_handle as parent_capsule: + self.__event = _rclpy.QoSEvent(parent_capsule, 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 wait_set.is_ready('event', self._event_index): + if _rclpy.rclpy_wait_set_is_ready('event', wait_set, 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 = wait_set.add_event(self.__event) + self._event_index = _rclpy.rclpy_wait_set_add_event(wait_set, 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: _rclpy.Subscription, topic_name: str, + self, callback_group: CallbackGroup, subscription_handle: Handle, topic_name: str, ) -> List[QoSEventHandler]: - with subscription: - logger = get_logger(subscription.get_logger_name()) + with subscription_handle as subscription_capsule: + logger = get_logger(_rclpy.rclpy_get_subscription_logger_name(subscription_capsule)) 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_impl=subscription)) + parent_handle=subscription_handle)) 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_impl=subscription)) + parent_handle=subscription_handle)) 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_impl=subscription)) + parent_handle=subscription_handle)) 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_impl=subscription)) + parent_handle=subscription_handle)) if self.message_lost: event_handlers.append(QoSEventHandler( callback_group=callback_group, callback=self.message_lost, event_type=QoSSubscriptionEventType.RCL_SUBSCRIPTION_MESSAGE_LOST, - parent_impl=subscription)) + parent_handle=subscription_handle)) 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: _rclpy.Publisher, topic_name: str, + self, callback_group: CallbackGroup, publisher_handle: Handle, topic_name: str, ) -> List[QoSEventHandler]: - with publisher: - logger = get_logger(publisher.get_logger_name()) + with publisher_handle as publisher_capsule: + logger = get_logger(_rclpy.rclpy_get_publisher_logger_name(publisher_capsule)) 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_impl=publisher)) + parent_handle=publisher_handle)) if self.liveliness: event_handlers.append(QoSEventHandler( callback_group=callback_group, callback=self.liveliness, event_type=QoSPublisherEventType.RCL_PUBLISHER_LIVELINESS_LOST, - parent_impl=publisher)) + parent_handle=publisher_handle)) 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_impl=publisher)) + parent_handle=publisher_handle)) 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_impl=publisher)) + parent_handle=publisher_handle)) except UnsupportedEventTypeError: pass diff --git a/rclpy/rclpy/signals.py b/rclpy/rclpy/signals.py index df47ffc18..0dc425af7 100644 --- a/rclpy/rclpy/signals.py +++ b/rclpy/rclpy/signals.py @@ -21,9 +21,8 @@ class SignalHandlerGuardCondition(GuardCondition): def __init__(self, context=None): super().__init__(callback=None, callback_group=None, context=context) - with self.handle: - # TODO(ahcorde): Remove the pycapsule method when #728 is in - _signals.rclpy_register_sigint_guard_condition(self.handle.pycapsule()) + with self.handle as capsule: + _signals.rclpy_register_sigint_guard_condition(capsule) def __del__(self): try: @@ -36,7 +35,6 @@ def __del__(self): pass def destroy(self): - with self.handle: - # TODO(ahcorde): Remove the pycapsule method when #728 is in - _signals.rclpy_unregister_sigint_guard_condition(self.handle.pycapsule()) + with self.handle as capsule: + _signals.rclpy_unregister_sigint_guard_condition(capsule) super().destroy() diff --git a/rclpy/rclpy/subscription.py b/rclpy/rclpy/subscription.py index ed16d5ecb..3ad22766c 100644 --- a/rclpy/rclpy/subscription.py +++ b/rclpy/rclpy/subscription.py @@ -16,6 +16,7 @@ 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 @@ -30,7 +31,7 @@ class Subscription: def __init__( self, - subscription_impl: _rclpy.Subscription, + subscription_handle: Handle, msg_type: MsgType, topic: str, callback: Callable, @@ -45,8 +46,8 @@ def __init__( .. warning:: Users should not create a subscription with this constructor, instead they should call :meth:`.Node.create_subscription`. - :param subscription_impl: :class:`Subscription` wrapping the underlying - ``rcl_subscription_t`` object. + :param subscription_handle: :class:`Handle` 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 @@ -57,7 +58,7 @@ def __init__( :param raw: If ``True``, then received messages will be stored in raw binary representation. """ - self.__subscription = subscription_impl + self.__handle = subscription_handle self.msg_type = msg_type self.topic = topic self.callback = callback @@ -68,18 +69,18 @@ def __init__( self.raw = raw self.event_handlers: QoSEventHandler = event_callbacks.create_event_handlers( - callback_group, subscription_impl, topic) + callback_group, subscription_handle, topic) @property def handle(self): - return self.__subscription + return self.__handle def destroy(self): for handler in self.event_handlers: handler.destroy() - self.handle.destroy_when_not_in_use() + self.handle.destroy() @property def topic_name(self): - with self.handle: - return self.__subscription.get_topic_name() + with self.handle as h: + return _rclpy.rclpy_get_subscription_topic_name(h) diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index c570524d9..83d4fc4f8 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: + with self._clock.handle, self._context.handle as context_capsule: self.__timer = _rclpy.Timer( - self._clock.handle, self._context.handle, timer_period_ns) + self._clock.handle, context_capsule, 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 new file mode 100644 index 000000000..d105ad6f0 --- /dev/null +++ b/rclpy/src/rclpy/_rclpy_action.cpp @@ -0,0 +1,1320 @@ +// 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 e191984b3..2fbacb1ae 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -15,11 +15,8 @@ #include #include -#include -#include "action_client.hpp" -#include "action_goal_handle.hpp" -#include "action_server.hpp" +#include "action_api.hpp" #include "client.hpp" #include "clock.hpp" #include "context.hpp" @@ -28,6 +25,7 @@ #include "graph.hpp" #include "guard_condition.hpp" #include "handle_api.hpp" +#include "init.hpp" #include "logging.hpp" #include "logging_api.hpp" #include "names.hpp" @@ -59,13 +57,6 @@ 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") @@ -109,13 +100,45 @@ 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); - rclpy::define_context(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_duration(m); - rclpy::define_publisher(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_service(m); @@ -125,18 +148,64 @@ 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_action_get_rmw_qos_profile", &rclpy::rclpy_action_get_rmw_qos_profile, - "Get an action RMW QoS profile."); - rclpy::define_guard_condition(m); + "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::define_timer(m); - rclpy::define_subscription(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_time_point(m); rclpy::define_clock(m); - rclpy::define_waitset(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."); m.def( "rclpy_expand_topic_name", &rclpy::expand_topic_name, @@ -201,9 +270,43 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { "rclpy_deserialize", &rclpy::deserialize, "Deserialize a ROS message."); - rclpy::define_node(m); + 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_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, @@ -229,4 +332,5 @@ 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 new file mode 100644 index 000000000..6da5c7d3c --- /dev/null +++ b/rclpy/src/rclpy/action_api.hpp @@ -0,0 +1,31 @@ +// 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 deleted file mode 100644 index 6fac5bf81..000000000 --- a/rclpy/src/rclpy/action_client.cpp +++ /dev/null @@ -1,321 +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 "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 deleted file mode 100644 index e8a24229c..000000000 --- a/rclpy/src/rclpy/action_client.hpp +++ /dev/null @@ -1,239 +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_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 deleted file mode 100644 index 771cde2a1..000000000 --- a/rclpy/src/rclpy/action_goal_handle.cpp +++ /dev/null @@ -1,121 +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 - -#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 deleted file mode 100644 index bb9da8819..000000000 --- a/rclpy/src/rclpy/action_goal_handle.hpp +++ /dev/null @@ -1,90 +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_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 deleted file mode 100644 index a0c1ee302..000000000 --- a/rclpy/src/rclpy/action_server.cpp +++ /dev/null @@ -1,403 +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 "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 deleted file mode 100644 index 6482bbd06..000000000 --- a/rclpy/src/rclpy/action_server.hpp +++ /dev/null @@ -1,258 +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_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 4cba6cd88..8587f527d 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -34,13 +34,15 @@ void Client::destroy() { rcl_client_.reset(); - node_.destroy(); + node_handle_.reset(); } Client::Client( - Node & node, py::object pysrv_type, const char * service_name, py::object pyqos_profile) -: node_(node) + py::capsule pynode, py::object pysrv_type, const char * service_name, py::object pyqos_profile) +: node_handle_(std::make_shared(pynode)) { + auto node = node_handle_->cast("rcl_node_t"); + auto srv_type = static_cast( rclpy_common_get_type_support(pysrv_type.ptr())); if (!srv_type) { @@ -57,10 +59,11 @@ Client::Client( // Create a client rcl_client_ = std::shared_ptr( PythonAllocator().allocate(1), - [node](rcl_client_t * client) + [this](rcl_client_t * client) { - // Intentionally capture node by value so shared_ptr can be transferred to copies - rcl_ret_t ret = rcl_client_fini(client, node.rcl_ptr()); + auto node = node_handle_->cast_or_warn("rcl_node_t"); + + rcl_ret_t ret = rcl_client_fini(client, node); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -75,7 +78,7 @@ Client::Client( *rcl_client_ = rcl_get_zero_initialized_client(); rcl_ret_t ret = rcl_client_init( - rcl_client_.get(), node_.rcl_ptr(), srv_type, service_name, &client_ops); + rcl_client_.get(), node, 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 '"}; @@ -111,8 +114,9 @@ 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_ptr(), rcl_client_.get(), &is_ready); + rcl_ret_t ret = rcl_service_server_is_available(node, rcl_client_.get(), &is_ready); if (RCL_RET_OK != ret) { throw RCLError("failed to check service availability"); } @@ -151,8 +155,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 1280bd099..f9f5a9f34 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -22,13 +22,13 @@ #include #include "destroyable.hpp" -#include "node.hpp" +#include "handle.hpp" namespace py = pybind11; namespace rclpy { -class Client : public Destroyable, public std::enable_shared_from_this +class Client : public Destroyable { public: /// Create a client @@ -40,12 +40,12 @@ class Client : public Destroyable, public std::enable_shared_from_this * Raises ValueError if the capsules are not the correct types * Raises RuntimeError if the client could not be created * - * \param[in] node Node to add the client to + * \param[in] pynode Capsule pointing to the 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(Node & node, py::object pysrv_type, const char * service_name, py::object pyqos); + Client(py::capsule pynode, py::object pysrv_type, const char * service_name, py::object pyqos); ~Client() = default; @@ -91,7 +91,8 @@ class Client : public Destroyable, public std::enable_shared_from_this destroy() override; private: - Node node_; + // TODO(sloretz) replace with std::shared_ptr when rclpy::Node exists + std::shared_ptr node_handle_; std::shared_ptr rcl_client_; }; diff --git a/rclpy/src/rclpy/clock.cpp b/rclpy/src/rclpy/clock.cpp index f74947997..bf91c84c7 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 37b8dbb75..06bd5076b 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, public std::enable_shared_from_this +class Clock : public Destroyable { public: /// Create a clock @@ -105,7 +105,13 @@ class Clock : public Destroyable, public std::enable_shared_from_this void remove_clock_callback(py::object pyjump_handle); - /// Get rcl_clock_t pointer + /// Get rcl_client_t pointer + std::shared_ptr get_shared_ptr() + { + return rcl_clock_; + } + + /// Get rcl_client_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 e1c155c45..794c29a78 100644 --- a/rclpy/src/rclpy/context.cpp +++ b/rclpy/src/rclpy/context.cpp @@ -20,137 +20,103 @@ #include #include -#include -#include -#include +#include #include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" #include "context.hpp" -#include "utils.hpp" namespace rclpy { -Context::Context(py::list pyargs, size_t domain_id) +size_t +context_get_domain_id(py::capsule pycontext) { - 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(); - } - } - - 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"); + auto context = static_cast( + rclpy_handle_get_pointer_from_capsule(pycontext.ptr(), "rcl_context_t")); + if (!context) { + throw py::error_already_set(); } - 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()); + size_t domain_id; + rcl_ret_t ret = rcl_context_get_domain_id(context, &domain_id); if (RCL_RET_OK != ret) { - throw RCLError("failed to initialize rcl"); + throw RCLError("Failed to get domain id"); } - throw_if_unparsed_ros_args(pyargs, rcl_context_.get()->global_arguments); + return domain_id; } void -Context::destroy() +_rclpy_context_handle_destructor(void * p) { - rcl_context_.reset(); + 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); } -size_t -Context::get_domain_id() +py::capsule +create_context() { - 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"); + auto context = static_cast(PyMem_Malloc(sizeof(rcl_context_t))); + if (!context) { + throw std::bad_alloc(); } - - return domain_id; + *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(); + } + return py::reinterpret_steal(capsule); } +/// Status of the the client library +/** + * \return True if rcl is running properly, False otherwise + */ bool -Context::ok() +context_is_valid(py::capsule pycontext) { - 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"); + auto context = static_cast(rclpy_handle_get_pointer_from_capsule( + pycontext.ptr(), "rcl_context_t")); + if (!context) { + throw py::error_already_set(); } -} -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"); + return rcl_context_is_valid(context); } - } // namespace rclpy diff --git a/rclpy/src/rclpy/context.hpp b/rclpy/src/rclpy/context.hpp index ed00992ef..f6f4181a1 100644 --- a/rclpy/src/rclpy/context.hpp +++ b/rclpy/src/rclpy/context.hpp @@ -17,91 +17,36 @@ #include -#include - -#include -#include - -#include "destroyable.hpp" -#include "rclpy_common/exceptions.hpp" - namespace py = pybind11; 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) { - 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); +/// 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); } // namespace rclpy #endif // RCLPY__CONTEXT_HPP_ diff --git a/rclpy/src/rclpy/destroyable.cpp b/rclpy/src/rclpy/destroyable.cpp index 2865983e8..fa6ced3a1 100644 --- a/rclpy/src/rclpy/destroyable.cpp +++ b/rclpy/src/rclpy/destroyable.cpp @@ -19,17 +19,8 @@ #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() { @@ -62,10 +53,6 @@ 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(); @@ -75,7 +62,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 92d6b0ad3..333db0916 100644 --- a/rclpy/src/rclpy/destroyable.hpp +++ b/rclpy/src/rclpy/destroyable.hpp @@ -25,12 +25,6 @@ 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 b9af8fa28..b63c8f366 100644 --- a/rclpy/src/rclpy/graph.cpp +++ b/rclpy/src/rclpy/graph.cpp @@ -36,13 +36,19 @@ namespace rclpy py::list graph_get_publisher_names_and_types_by_node( - Node & node, bool no_demangle, + py::capsule pynode, 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.rcl_ptr(), &allocator, no_demangle, node_name.c_str(), + node, &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) { @@ -69,13 +75,19 @@ graph_get_publisher_names_and_types_by_node( py::list graph_get_subscriber_names_and_types_by_node( - Node & node, bool no_demangle, + py::capsule pynode, 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.rcl_ptr(), &allocator, no_demangle, node_name.c_str(), + node, &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) { @@ -102,13 +114,18 @@ graph_get_subscriber_names_and_types_by_node( py::list graph_get_service_names_and_types_by_node( - Node & node, std::string node_name, std::string node_namespace) + py::capsule pynode, 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.rcl_ptr(), &allocator, node_name.c_str(), node_namespace.c_str(), - &service_names_and_types); + node, &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( @@ -134,12 +151,18 @@ graph_get_service_names_and_types_by_node( py::list graph_get_client_names_and_types_by_node( - Node & node, std::string node_name, std::string node_namespace) + py::capsule pynode, 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.rcl_ptr(), &allocator, node_name.c_str(), node_namespace.c_str(), &client_names_and_types); + node, &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( @@ -164,12 +187,18 @@ graph_get_client_names_and_types_by_node( } py::list -graph_get_topic_names_and_types(Node & node, bool no_demangle) +graph_get_topic_names_and_types(py::capsule pynode, 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.rcl_ptr(), &allocator, no_demangle, &topic_names_and_types); + rcl_get_topic_names_and_types(node, &allocator, no_demangle, &topic_names_and_types); if (RCL_RET_OK != ret) { throw RCLError("failed to get topic names and types"); } @@ -191,12 +220,17 @@ graph_get_topic_names_and_types(Node & node, bool no_demangle) } py::list -graph_get_service_names_and_types(Node & node) +graph_get_service_names_and_types(py::capsule pynode) { + 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.rcl_ptr(), &allocator, &service_names_and_types); + rcl_ret_t ret = rcl_get_service_names_and_types(node, &allocator, &service_names_and_types); if (RCL_RET_OK != ret) { throw RCLError("failed to get service names and types"); } @@ -225,12 +259,18 @@ typedef rcl_ret_t (* rcl_get_info_by_topic_func_t)( py::list _get_info_by_topic( - Node & node, + py::capsule pynode, 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(); @@ -247,8 +287,7 @@ _get_info_by_topic( } }); - rcl_ret_t ret = rcl_get_info_by_topic( - node.rcl_ptr(), &allocator, topic_name, no_mangle, &info_array); + rcl_ret_t ret = rcl_get_info_by_topic(node, &allocator, topic_name, no_mangle, &info_array); if (RCL_RET_OK != ret) { if (RCL_RET_UNSUPPORTED == ret) { throw NotImplementedError( @@ -265,19 +304,19 @@ _get_info_by_topic( py::list graph_get_publishers_info_by_topic( - Node & node, const char * topic_name, bool no_mangle) + py::capsule pynode, const char * topic_name, bool no_mangle) { return _get_info_by_topic( - node, topic_name, no_mangle, "publishers", + pynode, topic_name, no_mangle, "publishers", rcl_get_publishers_info_by_topic); } py::list graph_get_subscriptions_info_by_topic( - Node & node, const char * topic_name, bool no_mangle) + py::capsule pynode, const char * topic_name, bool no_mangle) { return _get_info_by_topic( - node, topic_name, no_mangle, "subscriptions", + pynode, 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 22967626f..05ca47604 100644 --- a/rclpy/src/rclpy/graph.hpp +++ b/rclpy/src/rclpy/graph.hpp @@ -22,8 +22,6 @@ #include #include -#include "node.hpp" - namespace py = pybind11; namespace rclpy @@ -31,10 +29,11 @@ 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] node Node to get publisher topic names and types. + * \param[in] pynode Capsule pointing to the node. * \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. @@ -46,15 +45,16 @@ namespace rclpy */ py::list graph_get_publisher_names_and_types_by_node( - Node & node, bool no_demangle, + py::capsule pynode, 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] node node to get subscriber topic names and types + * \param[in] pynode Capsule pointing to the node. * \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,15 +66,16 @@ graph_get_publisher_names_and_types_by_node( */ py::list graph_get_subscriber_names_and_types_by_node( - Node & node, bool no_demangle, + py::capsule pynode, 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] node Node to get service topic names and types + * \param[in] pynode Capsule pointing to the node. * \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 @@ -84,14 +85,15 @@ graph_get_subscriber_names_and_types_by_node( */ py::list graph_get_service_names_and_types_by_node( - Node & node, std::string node_name, std::string node_namespace); + py::capsule pynode, 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] node node to get client topic names and types + * \param[in] pynode Capsule pointing to the node. * \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 @@ -101,13 +103,14 @@ graph_get_service_names_and_types_by_node( */ py::list graph_get_client_names_and_types_by_node( - Node & node, std::string node_name, std::string node_namespace); + py::capsule pynode, 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] node node to get topic names and types + * \param[in] pynode Capsule pointing to the node to query the ROS graph. * \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 @@ -116,20 +119,21 @@ graph_get_client_names_and_types_by_node( * \see rcl_get_topic_names_and_types */ py::list -graph_get_topic_names_and_types(Node & node, bool no_demangle); +graph_get_topic_names_and_types(py::capsule pynode, 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] node Node to get all topic service names and types + * \param[in] pynode Capsule pointing to the node to query the ROS graph. * \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(Node & node); +graph_get_service_names_and_types(py::capsule pynode); /// Return a list of publishers on a given topic. /** @@ -139,7 +143,7 @@ graph_get_service_names_and_types(Node & node); * Raises NotImplementedError if the call is not supported by RMW * Raises RCLError if there is an rcl error * - * \param[in] node Node to get topic publisher info + * \param[in] pynode Capsule pointing to the node to get the namespace from. * \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. @@ -147,7 +151,7 @@ graph_get_service_names_and_types(Node & node); */ py::list graph_get_publishers_info_by_topic( - Node & node, const char * topic_name, bool no_mangle); + py::capsule pynode, const char * topic_name, bool no_mangle); /// Return a list of subscriptions on a given topic. /** @@ -157,7 +161,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] node node to get topic subscriber info + * \param[in] pynode Capsule pointing to the node to get the namespace from. * \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. @@ -165,7 +169,7 @@ graph_get_publishers_info_by_topic( */ py::list graph_get_subscriptions_info_by_topic( - Node & node, const char * topic_name, bool no_mangle); + py::capsule pynode, 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 4c41cd4c8..d5f6e43f6 100644 --- a/rclpy/src/rclpy/guard_condition.cpp +++ b/rclpy/src/rclpy/guard_condition.cpp @@ -31,66 +31,89 @@ namespace rclpy { -GuardCondition::GuardCondition(Context & context) -: context_(context) +/// Handle destructor for guard condition +static void +_rclpy_destroy_guard_condition(void * p) { - 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(); + 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_ret_t ret = rcl_guard_condition_init( - rcl_guard_condition_.get(), context.rcl_ptr(), gc_options); + rcl_ret_t ret = rcl_guard_condition_fini(gc); if (RCL_RET_OK != ret) { - throw RCLError("failed to create guard_condition"); + // 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); } + PyMem_Free(gc); } -void -GuardCondition::destroy() +py::capsule +guard_condition_create(py::capsule pycontext) { - rcl_guard_condition_.reset(); - context_.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; } void -GuardCondition::trigger_guard_condition() +guard_condition_trigger(py::capsule pygc) { - rcl_ret_t ret = rcl_trigger_guard_condition(rcl_guard_condition_.get()); + auto gc = static_cast( + rclpy_handle_get_pointer_from_capsule(pygc.ptr(), "rcl_guard_condition_t")); + if (!gc) { + throw py::error_already_set(); + } - if (RCL_RET_OK != ret) { + rcl_ret_t ret = rcl_trigger_guard_condition(gc); + + if (ret != RCL_RET_OK) { 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 d77f9f0a8..4a61c9be9 100644 --- a/rclpy/src/rclpy/guard_condition.hpp +++ b/rclpy/src/rclpy/guard_condition.hpp @@ -17,75 +17,31 @@ #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 -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); +/** + * 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); } // namespace rclpy #endif // RCLPY__GUARD_CONDITION_HPP_ diff --git a/rclpy/src/rclpy/init.cpp b/rclpy/src/rclpy/init.cpp new file mode 100644 index 000000000..73019815c --- /dev/null +++ b/rclpy/src/rclpy/init.cpp @@ -0,0 +1,154 @@ +// 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 new file mode 100644 index 000000000..e5c4f829d --- /dev/null +++ b/rclpy/src/rclpy/init.hpp @@ -0,0 +1,50 @@ +// 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 f6d642149..f95fb8503 100644 --- a/rclpy/src/rclpy/logging.cpp +++ b/rclpy/src/rclpy/logging.cpp @@ -62,13 +62,18 @@ rclpy_thread_safe_logging_output_handler( } void -logging_configure(Context & context) +logging_configure(py::object pycontext) { + 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.rcl_ptr()->global_arguments, + &context->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 f97c9cb34..c5152df8f 100644 --- a/rclpy/src/rclpy/logging.hpp +++ b/rclpy/src/rclpy/logging.hpp @@ -19,8 +19,6 @@ #include -#include "context.hpp" - namespace py = pybind11; namespace rclpy @@ -42,7 +40,7 @@ class LoggingGuard * \param[in] pycontext a context instance to use to retrieve global CLI arguments */ void -logging_configure(Context & _context); +logging_configure(py::object pycontext); /// Finalize rcl logging void diff --git a/rclpy/src/rclpy/names.cpp b/rclpy/src/rclpy/names.cpp index e265af6ce..5f4fdf23a 100644 --- a/rclpy/src/rclpy/names.cpp +++ b/rclpy/src/rclpy/names.cpp @@ -209,17 +209,23 @@ expand_topic_name(const char * topic, const char * node_name, const char * node_ } std::string -remap_topic_name(Node & node, const char * topic_name) +remap_topic_name(py::capsule pynode, 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.rcl_ptr()); + const rcl_node_options_t * node_options = rcl_node_get_options(node); 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.rcl_ptr()->context->global_arguments); + global_args = &(node->context->global_arguments); } char * output_cstr = nullptr; @@ -227,8 +233,8 @@ remap_topic_name(Node & node, const char * topic_name) &(node_options->arguments), global_args, topic_name, - rcl_node_get_name(node.rcl_ptr()), - rcl_node_get_namespace(node.rcl_ptr()), + rcl_node_get_name(node), + rcl_node_get_namespace(node), node_options->allocator, &output_cstr); if (RCL_RET_OK != ret) { @@ -250,16 +256,22 @@ remap_topic_name(Node & node, const char * topic_name) } std::string -resolve_name(Node & node, const char * topic_name, bool only_expand, bool is_service) +resolve_name(py::capsule pynode, const char * topic_name, bool only_expand, bool is_service) { - const rcl_node_options_t * node_options = rcl_node_get_options(node.rcl_ptr()); + 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); if (nullptr == node_options) { throw RCLError("failed to get node options"); } char * output_cstr = nullptr; rcl_ret_t ret = rcl_node_resolve_name( - node.rcl_ptr(), + node, topic_name, node_options->allocator, is_service, diff --git a/rclpy/src/rclpy/names.hpp b/rclpy/src/rclpy/names.hpp index 6ae90e822..e79e33440 100644 --- a/rclpy/src/rclpy/names.hpp +++ b/rclpy/src/rclpy/names.hpp @@ -19,8 +19,6 @@ #include -#include "node.hpp" - namespace py = pybind11; namespace rclpy @@ -96,26 +94,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] Node node to remap the topic name + * \param[in] pynode Capsule pointing to the node * \param[in] topic_name topic string to be remapped * \return remapped topic name */ std::string -remap_topic_name(Node & node, const char * topic_name); +remap_topic_name(py::capsule pynode, 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] node node to expand and remap a topic name + * \param[in] pynode Capsule pointing to the node * \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(Node & node, const char * topic_name, bool only_expand, bool is_service); +resolve_name(py::capsule pynode, 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 c41138c92..43d233012 100644 --- a/rclpy/src/rclpy/node.cpp +++ b/rclpy/src/rclpy/node.cpp @@ -15,7 +15,6 @@ // Include pybind11 before rclpy_common/handle.h includes Python.h #include -#include #include #include #include @@ -35,16 +34,24 @@ #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 * -Node::get_fully_qualified_name() +get_node_fully_qualified_name(py::capsule pynode) { - const char * fully_qualified_node_name = rcl_node_get_fully_qualified_name(rcl_node_.get()); + 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); if (!fully_qualified_node_name) { throw RCLError("Fully qualified name not set"); } @@ -53,9 +60,15 @@ Node::get_fully_qualified_name() } const char * -Node::logger_name() +get_node_logger_name(py::capsule pynode) { - const char * node_logger_name = rcl_node_get_logger_name(rcl_node_.get()); + 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); if (!node_logger_name) { throw RCLError("Logger name not set"); } @@ -64,9 +77,15 @@ Node::logger_name() } const char * -Node::get_node_name() +get_node_name(py::capsule pynode) { - const char * node_name = rcl_node_get_name(rcl_node_.get()); + 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); if (!node_name) { throw RCLError("Node name not set"); } @@ -75,9 +94,15 @@ Node::get_node_name() } const char * -Node::get_namespace() +get_node_namespace(py::capsule pynode) { - const char * node_namespace = rcl_node_get_namespace(rcl_node_.get()); + 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); if (!node_namespace) { throw RCLError("Node namespace not set"); } @@ -86,10 +111,16 @@ Node::get_namespace() } size_t -Node::get_count_publishers(const char * topic_name) +get_count_publishers(py::capsule pynode, 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(rcl_node_.get(), topic_name, &count); + rcl_ret_t ret = rcl_count_publishers(node, topic_name, &count); if (RCL_RET_OK != ret) { throw RCLError("Error in rcl_count_publishers"); } @@ -98,10 +129,16 @@ Node::get_count_publishers(const char * topic_name) } size_t -Node::get_count_subscribers(const char * topic_name) +get_count_subscribers(py::capsule pynode, 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(rcl_node_.get(), topic_name, &count); + rcl_ret_t ret = rcl_count_subscribers(node, topic_name, &count); if (RCL_RET_OK != ret) { throw RCLError("Error in rcl_count_subscribers"); } @@ -110,8 +147,14 @@ Node::get_count_subscribers(const char * topic_name) } py::list -Node::get_names_impl(bool get_enclaves) +get_node_names_impl(py::capsule pynode, 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(); @@ -120,10 +163,10 @@ Node::get_names_impl(bool get_enclaves) rcl_ret_t ret = RCL_RET_OK; if (get_enclaves) { ret = rcl_get_node_names_with_enclaves( - rcl_node_.get(), allocator, &node_names, &node_namespaces, &enclaves); + node, allocator, &node_names, &node_namespaces, &enclaves); } else { ret = rcl_get_node_names( - rcl_node_.get(), allocator, &node_names, &node_namespaces); + node, allocator, &node_names, &node_namespaces); } if (RCL_RET_OK != ret) { throw RCLError("Failed to get node names"); @@ -133,32 +176,29 @@ Node::get_names_impl(bool get_enclaves) { rcutils_ret_t fini_ret = rcutils_string_array_fini(&node_names); if (RCUTILS_RET_OK != fini_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 node names during error handling: %s", - RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + 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"); rcl_reset_error(); } fini_ret = rcutils_string_array_fini(&node_namespaces); if (RCUTILS_RET_OK != fini_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 node namespaces during error handling: %s", - RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + 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"); rcl_reset_error(); } fini_ret = rcutils_string_array_fini(&enclaves); if (RCUTILS_RET_OK != fini_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 enclaves string array during error handling: %s", - RCUTILS_STRINGIFY(__FILE__), RCUTILS_STRINGIFY(__LINE__), rcl_get_error_string().str); + 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"); rcl_reset_error(); } }); @@ -181,15 +221,15 @@ Node::get_names_impl(bool get_enclaves) } py::list -Node::get_node_names_and_namespaces() +get_node_names_and_namespaces(py::capsule pynode) { - return get_names_impl(false); + return get_node_names_impl(pynode, false); } py::list -Node::get_node_names_and_namespaces_with_enclaves() +get_node_names_and_namespaces_with_enclaves(py::capsule pynode) { - return get_names_impl(true); + return get_node_names_impl(pynode, true); } /// Create an rclpy.parameter.Parameter from an rcl_variant_t @@ -322,16 +362,22 @@ _parse_param_overrides( } py::dict -Node::get_parameters(py::object pyparameter_cls) +get_node_parameters(py::object pyparameter_cls, py::capsule pynode) { + 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(rcl_node_.get()); + const rcl_node_options_t * node_options = rcl_node_get_options(node); if (node_options->use_global_arguments) { _parse_param_overrides( - &(rcl_node_.get()->context->global_arguments), pyparameter_cls, + &(node->context->global_arguments), pyparameter_cls, parameter_type_cls, params_by_node_name); } @@ -339,7 +385,7 @@ Node::get_parameters(py::object pyparameter_cls) &(node_options->arguments), pyparameter_cls, parameter_type_cls, params_by_node_name); - const char * node_fqn = rcl_node_get_fully_qualified_name(rcl_node_.get()); + const char * node_fqn = rcl_node_get_fully_qualified_name(node); if (!node_fqn) { throw RCLError("failed to get node fully qualified name"); } @@ -363,22 +409,49 @@ Node::get_parameters(py::object pyparameter_cls) return node_params; } -void Node::destroy() +/// Handle destructor for node +static +void +_rclpy_destroy_node(void * p) { - rcl_node_.reset(); - context_.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); } -Node::Node( +py::capsule +create_node( const char * node_name, const char * namespace_, - Context & context, + py::capsule pycontext, 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 @@ -430,23 +503,14 @@ Node::Node( throw_if_unparsed_ros_args(pyargs, arguments); - 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(); + 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_options_t options = rcl_node_get_default_options(); options.use_global_arguments = use_global_arguments; options.arguments = arguments; @@ -454,8 +518,7 @@ Node::Node( { rclpy::LoggingGuard scoped_logging_guard; - ret = rcl_node_init( - rcl_node_.get(), node_name, namespace_, context.rcl_ptr(), &options); + ret = rcl_node_init(node.get(), node_name, namespace_, context, &options); } if (RCL_RET_BAD_ALLOC == ret) { @@ -471,107 +534,22 @@ Node::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); -} -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"); + PyObject * pynode_c = rclpy_create_handle_capsule(node.get(), "rcl_node_t", _rclpy_destroy_node); + if (!pynode_c) { + 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"); + 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); -} - -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"); + return pynode; } } // namespace rclpy diff --git a/rclpy/src/rclpy/node.hpp b/rclpy/src/rclpy/node.hpp index fb6eeb289..e09a463b3 100644 --- a/rclpy/src/rclpy/node.hpp +++ b/rclpy/src/rclpy/node.hpp @@ -17,189 +17,157 @@ #include -#include // rcl_names_and_types_t - -#include - -#include "context.hpp" -#include "destroyable.hpp" - namespace py = pybind11; 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); +/// 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 + #endif // RCLPY__NODE_HPP_ diff --git a/rclpy/src/rclpy/publisher.cpp b/rclpy/src/rclpy/publisher.cpp index 467d8f56f..5a73882fd 100644 --- a/rclpy/src/rclpy/publisher.cpp +++ b/rclpy/src/rclpy/publisher.cpp @@ -21,6 +21,7 @@ #include #include "rclpy_common/common.h" +#include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" @@ -28,11 +29,40 @@ namespace rclpy { -Publisher::Publisher( - Node & node, py::object pymsg_type, std::string topic, +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, 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) { @@ -45,30 +75,22 @@ Publisher::Publisher( publisher_ops.qos = pyqos_profile.cast(); } - 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(); + // 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_ret_t ret = rcl_publisher_init( - rcl_publisher_.get(), node_.rcl_ptr(), msg_type, + &(pub->publisher), node, msg_type, topic.c_str(), &publisher_ops); - if (RCL_RET_OK != ret) { - if (RCL_RET_TOPIC_NAME_INVALID == ret) { + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TOPIC_NAME_INVALID) { std::string error_text{"Failed to create publisher due to invalid topic name '"}; error_text += topic; error_text += "'"; @@ -76,18 +98,37 @@ Publisher::Publisher( } throw RCLError("Failed to create publisher"); } -} -void Publisher::destroy() -{ - rcl_publisher_.reset(); - node_.destroy(); + 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; } const char * -Publisher::get_logger_name() +publisher_get_logger_name(py::capsule pypublisher) { - const char * node_logger_name = rcl_node_get_logger_name(node_.rcl_ptr()); + 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); if (!node_logger_name) { throw RCLError("Node logger name not set"); } @@ -96,11 +137,17 @@ Publisher::get_logger_name() } size_t -Publisher::get_subscription_count() +publisher_get_subscription_count(py::capsule pypublisher) { + 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(rcl_publisher_.get(), &count); - if (RCL_RET_OK != ret) { + rcl_ret_t ret = rcl_publisher_get_subscription_count(&pub->publisher, &count); + if (ret != RCL_RET_OK) { throw RCLError("failed to get subscription count"); } @@ -108,9 +155,15 @@ Publisher::get_subscription_count() } std::string -Publisher::get_topic_name() +publisher_get_topic_name(py::capsule pypublisher) { - const char * topic_name = rcl_publisher_get_topic_name(rcl_publisher_.get()); + 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); if (!topic_name) { throw RCLError("failed to get topic name"); } @@ -119,59 +172,44 @@ Publisher::get_topic_name() } void -Publisher::publish(py::object pymsg) +publisher_publish_message(py::capsule pypublisher, 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(rcl_publisher_.get(), raw_ros_message, NULL); + rcl_ret_t ret = rcl_publish(&(pub->publisher), raw_ros_message, NULL); destroy_ros_message(raw_ros_message); - if (RCL_RET_OK != ret) { + if (ret != RCL_RET_OK) { throw RCLError("Failed to publish"); } } void -Publisher::publish_raw(std::string msg) +publisher_publish_raw(py::capsule pypublisher, 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(rcl_publisher_.get(), &serialized_msg, NULL); - if (RCL_RET_OK != ret) { + rcl_ret_t ret = rcl_publish_serialized_message(&(pub->publisher), &serialized_msg, NULL); + if (ret != RCL_RET_OK) { 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 b40b9a2d6..a42bfb377 100644 --- a/rclpy/src/rclpy/publisher.hpp +++ b/rclpy/src/rclpy/publisher.hpp @@ -17,104 +17,87 @@ #include -#include -#include - -#include #include -#include "destroyable.hpp" -#include "node.hpp" - namespace py = pybind11; namespace rclpy { +/// Create a publisher /** - * This class will create a publisher and attach it to the provided topic name + * This function 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. + * 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 */ -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 RCLError if logger name not set - * - * \return the logger name - */ - const char * - get_logger_name(); +py::capsule +publisher_create( + py::capsule pynode, py::object pymsg_type, std::string topic, + py::object pyqos_profile); - /// 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 - /** - * Raises RCLError if the name cannot be determined - * - * \return name of the publisher's topic - */ - std::string - get_topic_name(); - - /// Publish a message - /** - * Raises RCLError if the message cannot be published - * - * \param[in] pymsg Message to send - */ - void - publish(py::object pymsg); +/// 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); - /// 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); +/// 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); - /// Get rcl_publisher_t pointer - rcl_publisher_t * - rcl_ptr() const - { - return rcl_publisher_.get(); - } +/// 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); - /// Force an early destruction of this object - void - destroy() override; +/// 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); -private: - Node node_; - std::shared_ptr rcl_publisher_; -}; -/// Define a pybind11 wrapper for an rclpy::Service -void define_publisher(py::object module); +/// 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); } // namespace rclpy #endif // RCLPY__PUBLISHER_HPP_ diff --git a/rclpy/src/rclpy/qos_event.cpp b/rclpy/src/rclpy/qos_event.cpp index 10abd191f..ffd4c798c 100644 --- a/rclpy/src/rclpy/qos_event.cpp +++ b/rclpy/src/rclpy/qos_event.cpp @@ -21,7 +21,6 @@ #include #include -#include #include // NOLINT #include "rclpy_common/common.h" @@ -59,19 +58,20 @@ void QoSEvent::destroy() { rcl_event_.reset(); - std::visit([](auto & t) {t.destroy();}, grandparent_); + parent_handle_.reset(); } QoSEvent::QoSEvent( - rclpy::Subscription & subscription, rcl_subscription_event_type_t event_type) -: event_type_(event_type), grandparent_(subscription) + py::capsule pysubscription, rcl_subscription_event_type_t event_type) +: parent_handle_(std::make_shared(pysubscription)), event_type_(event_type) { + 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(), - std::get(grandparent_).rcl_ptr(), event_type); + rcl_event_.get(), &(wrapper->subscription), event_type); if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); throw std::bad_alloc(); @@ -85,15 +85,16 @@ QoSEvent::QoSEvent( } QoSEvent::QoSEvent( - rclpy::Publisher & publisher, rcl_publisher_event_type_t event_type) -: event_type_(event_type), grandparent_(publisher) + py::capsule pypublisher, rcl_publisher_event_type_t event_type) +: parent_handle_(std::make_shared(pypublisher)), event_type_(event_type) { + 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(), - std::get(grandparent_).rcl_ptr(), event_type); + rcl_event_.get(), &(wrapper->publisher), event_type); if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); throw std::bad_alloc(); @@ -167,9 +168,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 7d7bbcd96..7c886758c 100644 --- a/rclpy/src/rclpy/qos_event.hpp +++ b/rclpy/src/rclpy/qos_event.hpp @@ -24,30 +24,27 @@ #include "destroyable.hpp" #include "handle.hpp" -#include "publisher.hpp" -#include "subscription.hpp" namespace py = pybind11; namespace rclpy { -/* - * This class will create an event handle for the given subscription. - */ -class QoSEvent : public Destroyable, public std::enable_shared_from_this +class QoSEvent : public Destroyable { 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] ssubscription Subscription wrapping the underlying ``rcl_subscription_t`` object. + * \param[in] pysubscription Capsule containing the subscription * \param[in] event_type Type of event to create */ - QoSEvent(rclpy::Subscription & subscriber, rcl_subscription_event_type_t event_type); + QoSEvent(py::capsule pysubscription, rcl_subscription_event_type_t event_type); /// Create a publisher event /** @@ -58,10 +55,10 @@ class QoSEvent : public Destroyable, public std::enable_shared_from_this event_type_; - std::variant grandparent_; + std::shared_ptr parent_handle_; std::shared_ptr rcl_event_; + std::variant event_type_; }; /// Define a pybind11 wrapper for an rclpy::QoSEvent diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index 2f4acf093..1002fbf73 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -35,14 +35,16 @@ void Service::destroy() { rcl_service_.reset(); - node_.destroy(); + node_handle_.reset(); } Service::Service( - Node & node, py::object pysrv_type, std::string service_name, + py::capsule pynode, py::object pysrv_type, std::string service_name, py::object pyqos_profile) -: node_(node) +: node_handle_(std::make_shared(pynode)) { + auto node = node_handle_->cast("rcl_node_t"); + auto srv_type = static_cast( rclpy_common_get_type_support(pysrv_type.ptr())); if (!srv_type) { @@ -58,10 +60,11 @@ Service::Service( // Create a client rcl_service_ = std::shared_ptr( new rcl_service_t, - [node](rcl_service_t * service) + [this](rcl_service_t * service) { - // Intentionally capture node by copy so shared_ptr can be transfered to copies - rcl_ret_t ret = rcl_service_fini(service, node.rcl_ptr()); + auto node = node_handle_->cast_or_warn("rcl_node_t"); + + rcl_ret_t ret = rcl_service_fini(service, node); if (RCL_RET_OK != ret) { // Warning should use line number of the current stack frame int stack_level = 1; @@ -76,7 +79,7 @@ Service::Service( *rcl_service_ = rcl_get_zero_initialized_service(); rcl_ret_t ret = rcl_service_init( - rcl_service_.get(), node_.rcl_ptr(), srv_type, + rcl_service_.get(), node, srv_type, service_name.c_str(), &service_ops); if (RCL_RET_OK != ret) { if (ret == RCL_RET_SERVICE_NAME_INVALID) { @@ -117,7 +120,8 @@ 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(); @@ -135,8 +139,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 6d8b0f298..ec87880c7 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -25,7 +25,6 @@ #include "destroyable.hpp" #include "handle.hpp" -#include "node.hpp" #include "rclpy_common/exceptions.hpp" #include "utils.hpp" @@ -34,7 +33,7 @@ namespace py = pybind11; namespace rclpy { -class Service : public Destroyable, public std::enable_shared_from_this +class Service : public Destroyable { public: /// Create a service server @@ -46,14 +45,14 @@ class Service : public Destroyable, public std::enable_shared_from_this * Raises ValueError if the capsules are not the correct types * Raises RCLError if the service could not be created * - * \param[in] node node to add the service to + * \param[in] pynode Capsule pointing to the 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( - Node & node, py::object pysrv_type, std::string service_name, + py::capsule pynode, py::object pysrv_type, std::string service_name, py::object pyqos_profile); ~Service() = default; @@ -82,7 +81,7 @@ class Service : public Destroyable, public std::enable_shared_from_this py::tuple service_take_request(py::object pyrequest_type); - /// Get rcl_service_t pointer + /// Get rcl_client_t pointer rcl_service_t * rcl_ptr() const { @@ -94,7 +93,8 @@ class Service : public Destroyable, public std::enable_shared_from_this destroy() override; private: - Node node_; + // TODO(ahcorde) replace with std::shared_ptr when rclpy::Node exists + std::shared_ptr node_handle_; std::shared_ptr rcl_service_; }; diff --git a/rclpy/src/rclpy/subscription.cpp b/rclpy/src/rclpy/subscription.cpp index 2b2f11cff..6a20d8f26 100644 --- a/rclpy/src/rclpy/subscription.cpp +++ b/rclpy/src/rclpy/subscription.cpp @@ -23,6 +23,7 @@ #include #include "rclpy_common/common.h" +#include "rclpy_common/handle.h" #include "rclpy_common/exceptions.hpp" @@ -34,11 +35,40 @@ using pybind11::literals::operator""_a; namespace rclpy { -Subscription::Subscription( - Node & node, py::object pymsg_type, std::string topic, +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, 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) { @@ -51,27 +81,19 @@ Subscription::Subscription( subscription_ops.qos = pyqos_profile.cast(); } - 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(); + // 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_ret_t ret = rcl_subscription_init( - rcl_subscription_.get(), node_.rcl_ptr(), msg_type, + &(sub->subscription), node, msg_type, topic.c_str(), &subscription_ops); if (ret != RCL_RET_OK) { if (ret == RCL_RET_TOPIC_NAME_INVALID) { @@ -82,23 +104,41 @@ Subscription::Subscription( } throw RCLError("Failed to create subscription"); } -} -void Subscription::destroy() -{ - rcl_subscription_.reset(); - node_.destroy(); + 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; } py::object -Subscription::take_message(py::object pymsg_type, bool raw) +subscription_take_message(py::capsule pysubscription, 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( - rcl_subscription_.get(), &taken.rcl_msg, &message_info, NULL); + &(wrapper->subscription), &taken.rcl_msg, &message_info, NULL); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); @@ -116,7 +156,7 @@ Subscription::take_message(py::object pymsg_type, bool raw) auto taken_msg = create_from_py(pymsg_type); rcl_ret_t ret = rcl_take( - rcl_subscription_.get(), taken_msg.get(), &message_info, NULL); + &(wrapper->subscription), taken_msg.get(), &message_info, NULL); if (RCL_RET_OK != ret) { if (RCL_RET_BAD_ALLOC == ret) { rcl_reset_error(); @@ -137,45 +177,37 @@ Subscription::take_message(py::object pymsg_type, bool raw) "received_timestamp"_a = message_info.received_timestamp)); } -const char * -Subscription::get_logger_name() +py::object +subscription_get_logger_name(py::capsule pysubscription) { - const char * node_logger_name = rcl_node_get_logger_name(node_.rcl_ptr()); - if (!node_logger_name) { - throw RCLError("Node logger name not set"); + 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(); } - return node_logger_name; + return py::str(node_logger_name); } std::string -Subscription::get_topic_name() +subscription_get_topic_name(py::capsule pysubscription) { - const char * subscription_name = rcl_subscription_get_topic_name(rcl_subscription_.get()); + 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)); 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 21640a16e..5c31aa757 100644 --- a/rclpy/src/rclpy/subscription.hpp +++ b/rclpy/src/rclpy/subscription.hpp @@ -17,92 +17,74 @@ #include -#include -#include - -#include #include -#include "destroyable.hpp" -#include "node.hpp" - namespace py = pybind11; namespace rclpy { /// Create a subscription /** - * This class will create a subscription for the given topic name. + * This function 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. + * 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 */ -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 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. - /** - * - * \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 RCLError if the topic name could not be determined - * - * \return a string with the topic name - */ - std::string - get_topic_name(); +py::capsule +subscription_create( + py::capsule pynode, py::object pymsg_type, std::string topic, + py::object pyqos_profile); - /// Get rcl_subscription_t pointer - rcl_subscription_t * - rcl_ptr() const - { - return rcl_subscription_.get(); - } +/// 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); - /// Force an early destruction of this object - void - destroy() override; +/// 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); -private: - Node node_; - std::shared_ptr rcl_subscription_; -}; -/// Define a pybind11 wrapper for an rclpy::Service -void define_subscription(py::object module); +/// 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); } // namespace rclpy #endif // RCLPY__SUBSCRIPTION_HPP_ diff --git a/rclpy/src/rclpy/timer.cpp b/rclpy/src/rclpy/timer.cpp index a424dd013..b4e192e57 100644 --- a/rclpy/src/rclpy/timer.cpp +++ b/rclpy/src/rclpy/timer.cpp @@ -37,14 +37,20 @@ void Timer::destroy() { rcl_timer_.reset(); - clock_.destroy(); - context_.destroy(); + clock_handle_.reset(); } Timer::Timer( - Clock & clock, Context & context, int64_t period_nsec) -: context_(context), clock_(clock) + Clock & rclcy_clock, py::capsule pycontext, int64_t period_nsec) { + 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, @@ -66,7 +72,7 @@ Timer::Timer( rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_ret_t ret = rcl_timer_init( - rcl_timer_.get(), clock_.rcl_ptr(), context.rcl_ptr(), + rcl_timer_.get(), clock_handle_.get(), context, period_nsec, NULL, allocator); if (RCL_RET_OK != ret) { @@ -162,8 +168,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 55c37b292..57e5dde7e 100644 --- a/rclpy/src/rclpy/timer.hpp +++ b/rclpy/src/rclpy/timer.hpp @@ -22,7 +22,6 @@ #include #include "clock.hpp" -#include "context.hpp" #include "destroyable.hpp" #include "handle.hpp" @@ -31,7 +30,7 @@ namespace py = pybind11; namespace rclpy { -class Timer : public Destroyable, public std::enable_shared_from_this +class Timer : public Destroyable { public: /// Create a timer @@ -47,7 +46,7 @@ class Timer : public Destroyable, public std::enable_shared_from_this * \param[in] period_nsec the period of the timer in nanoseconds * \return a timer capsule */ - Timer(Clock & clock, Context & context, int64_t period_nsec); + Timer(Clock & clock, py::capsule pycontext, int64_t period_nsec); ~Timer() = default; @@ -123,7 +122,7 @@ class Timer : public Destroyable, public std::enable_shared_from_this */ bool is_timer_canceled(); - /// Get rcl_timer_t pointer + /// Get rcl_client_t pointer rcl_timer_t * rcl_ptr() const { @@ -134,8 +133,8 @@ class Timer : public Destroyable, public std::enable_shared_from_this void destroy() override; private: - Context context_; - Clock clock_; + // TODO(ahcorde) replace with std::shared_ptr when rclpy::Clock exists + std::shared_ptr clock_handle_; std::shared_ptr rcl_timer_; }; diff --git a/rclpy/src/rclpy/utils.cpp b/rclpy/src/rclpy/utils.cpp index 6f2cf2c0d..8eb013c87 100644 --- a/rclpy/src/rclpy/utils.cpp +++ b/rclpy/src/rclpy/utils.cpp @@ -17,18 +17,17 @@ #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 @@ -82,31 +81,6 @@ 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) { @@ -130,10 +104,20 @@ get_rmw_implementation_identifier() } void -assert_liveliness(rclpy::Publisher * publisher) +assert_liveliness(py::capsule pyentity) { - if (RCL_RET_OK != rcl_publisher_assert_liveliness(publisher->rcl_ptr())) { - throw RCLError("Failed to assert liveliness on the 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."); } } @@ -226,54 +210,4 @@ 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 0f4ae104e..58fa0b24a 100644 --- a/rclpy/src/rclpy/utils.hpp +++ b/rclpy/src/rclpy/utils.hpp @@ -21,9 +21,6 @@ #include -#include "rclpy_common/common.h" -#include "publisher.hpp" - namespace py = pybind11; namespace rclpy @@ -50,16 +47,6 @@ 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. @@ -91,7 +78,7 @@ get_rmw_implementation_identifier(); * \return None */ void -assert_liveliness(rclpy::Publisher * publisher); +assert_liveliness(py::capsule pyentity); /// Remove ROS specific args from a list of args. /** @@ -106,21 +93,6 @@ assert_liveliness(rclpy::Publisher * publisher); 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 2e837be15..bc15e432c 100644 --- a/rclpy/src/rclpy/wait_set.cpp +++ b/rclpy/src/rclpy/wait_set.cpp @@ -32,93 +32,157 @@ namespace rclpy { -WaitSet::WaitSet( +/// 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, 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) -: context_(context) + py::capsule pycontext) { - // 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(); + 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(); + } rcl_ret_t ret = rcl_wait_set_init( - rcl_wait_set_.get(), + wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers, number_of_clients, number_of_services, number_of_events, - context.rcl_ptr(), + context, rcl_get_default_allocator()); - if (RCL_RET_OK != ret) { + if (ret != RCL_RET_OK) { throw RCLError("failed to initialize wait set"); } } void -WaitSet::destroy() +wait_set_clear_entities(py::capsule pywait_set) { - rcl_wait_set_.reset(); - context_.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); -void -WaitSet::clear_entities() -{ - rcl_ret_t ret = rcl_wait_set_clear(rcl_wait_set_.get()); + rcl_ret_t ret = rcl_wait_set_clear(wait_set); if (ret != RCL_RET_OK) { throw RCLError("failed to clear wait set"); } } size_t -WaitSet::add_service(const Service & service) +wait_set_add_entity(const std::string & entity_type, py::capsule pywait_set, py::capsule pyentity) { + 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; - 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"); + + 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); } return index; } size_t -WaitSet::add_subscription(const Subscription & subscription) +wait_set_add_service(const py::capsule pywait_set, 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); + size_t index; - rcl_ret_t ret = rcl_wait_set_add_subscription( - rcl_wait_set_.get(), subscription.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_service(wait_set, service.rcl_ptr(), &index); if (RCL_RET_OK != ret) { - throw RCLError("failed to add subscription to wait set"); + throw RCLError("failed to add service to wait set"); } return index; } size_t -WaitSet::add_timer(const Timer & timer) +wait_set_add_timer(const py::capsule pywait_set, 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(rcl_wait_set_.get(), timer.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_timer(wait_set, timer.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add client to wait set"); } @@ -126,21 +190,15 @@ WaitSet::add_timer(const Timer & timer) } size_t -WaitSet::add_guard_condition(const GuardCondition & gc) +wait_set_add_client(const py::capsule pywait_set, const Client & client) { - 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"); + if (0 != std::strcmp("rcl_wait_set_t", pywait_set.name())) { + throw py::value_error("capsule is not an rcl_wait_set_t"); } - return index; -} + auto wait_set = static_cast(pywait_set); -size_t -WaitSet::add_client(const Client & client) -{ size_t index; - rcl_ret_t ret = rcl_wait_set_add_client(rcl_wait_set_.get(), client.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_client(wait_set, client.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add client to wait set"); } @@ -148,10 +206,15 @@ WaitSet::add_client(const Client & client) } size_t -WaitSet::add_event(const QoSEvent & event) +wait_set_add_event(const py::capsule pywait_set, 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(rcl_wait_set_.get(), event.rcl_ptr(), &index); + rcl_ret_t ret = rcl_wait_set_add_event(wait_set, event.rcl_ptr(), &index); if (RCL_RET_OK != ret) { throw RCLError("failed to add event to wait set"); } @@ -159,28 +222,33 @@ WaitSet::add_event(const QoSEvent & event) } bool -WaitSet::is_ready(const std::string & entity_type, size_t index) +wait_set_is_ready(const std::string & entity_type, py::capsule pywait_set, 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(rcl_wait_set_->subscriptions); - num_entities = rcl_wait_set_->size_of_subscriptions; + entities = reinterpret_cast(wait_set->subscriptions); + num_entities = wait_set->size_of_subscriptions; } else if ("client" == entity_type) { - entities = reinterpret_cast(rcl_wait_set_->clients); - num_entities = rcl_wait_set_->size_of_clients; + entities = reinterpret_cast(wait_set->clients); + num_entities = wait_set->size_of_clients; } else if ("service" == entity_type) { - entities = reinterpret_cast(rcl_wait_set_->services); - num_entities = rcl_wait_set_->size_of_services; + entities = reinterpret_cast(wait_set->services); + num_entities = wait_set->size_of_services; } else if ("timer" == entity_type) { - entities = reinterpret_cast(rcl_wait_set_->timers); - num_entities = rcl_wait_set_->size_of_timers; + entities = reinterpret_cast(wait_set->timers); + num_entities = wait_set->size_of_timers; } else if ("guard_condition" == entity_type) { - entities = reinterpret_cast(rcl_wait_set_->guard_conditions); - num_entities = rcl_wait_set_->size_of_guard_conditions; + entities = reinterpret_cast(wait_set->guard_conditions); + num_entities = wait_set->size_of_guard_conditions; } else if ("event" == entity_type) { - entities = reinterpret_cast(rcl_wait_set_->events); - num_entities = rcl_wait_set_->size_of_events; + entities = reinterpret_cast(wait_set->events); + num_entities = wait_set->size_of_events; } else { std::string error_text{"'"}; error_text += entity_type; @@ -215,23 +283,23 @@ _get_ready_entities(const EntityArray ** entities, const size_t num_entities) } py::list -WaitSet::get_ready_entities(const std::string & entity_type) +get_ready_entities(const std::string & entity_type, py::capsule pywait_set) { + 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( - rcl_wait_set_->subscriptions, rcl_wait_set_->size_of_subscriptions); + return _get_ready_entities(wait_set->subscriptions, wait_set->size_of_subscriptions); } else if ("client" == entity_type) { - return _get_ready_entities( - rcl_wait_set_->clients, rcl_wait_set_->size_of_clients); + return _get_ready_entities(wait_set->clients, wait_set->size_of_clients); } else if ("service" == entity_type) { - return _get_ready_entities( - rcl_wait_set_->services, rcl_wait_set_->size_of_services); + return _get_ready_entities(wait_set->services, wait_set->size_of_services); } else if ("timer" == entity_type) { - return _get_ready_entities( - rcl_wait_set_->timers, rcl_wait_set_->size_of_timers); + return _get_ready_entities(wait_set->timers, wait_set->size_of_timers); } else if ("guard_condition" == entity_type) { - return _get_ready_entities( - rcl_wait_set_->guard_conditions, rcl_wait_set_->size_of_guard_conditions); + return _get_ready_entities(wait_set->guard_conditions, wait_set->size_of_guard_conditions); } std::string error_text{"'"}; @@ -241,59 +309,23 @@ WaitSet::get_ready_entities(const std::string & entity_type) } void -WaitSet::wait(int64_t timeout) +wait(py::capsule pywait_set, 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(rcl_wait_set_.get(), timeout); + ret = rcl_wait(wait_set, timeout); } - if (RCL_RET_OK != ret && RCL_RET_TIMEOUT != ret) { + if (ret != RCL_RET_OK && ret != RCL_RET_TIMEOUT) { 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 7d74cb1fc..f73052560 100644 --- a/rclpy/src/rclpy/wait_set.hpp +++ b/rclpy/src/rclpy/wait_set.hpp @@ -21,162 +21,146 @@ #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 { -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); +/// 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); } // 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 db18e9586..a5ba6f069 100644 --- a/rclpy/src/rclpy_common/include/rclpy_common/common.h +++ b/rclpy/src/rclpy_common/include/rclpy_common/common.h @@ -32,6 +32,49 @@ 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. @@ -41,6 +84,17 @@ 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 e96319360..e0788fdd8 100644 --- a/rclpy/src/rclpy_common/src/common.c +++ b/rclpy/src/rclpy_common/src/common.c @@ -55,6 +55,23 @@ 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) { @@ -74,6 +91,55 @@ 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 e16d11fe7..97ad26bc9 100644 --- a/rclpy/test/test_destruction.py +++ b/rclpy/test/test_destruction.py @@ -38,7 +38,8 @@ def test_destroy_node_twice(): try: node = rclpy.create_node('test_node2', context=context) node.destroy_node() - node.destroy_node() + with pytest.raises(InvalidHandle): + node.destroy_node() finally: rclpy.shutdown(context=context) @@ -143,7 +144,11 @@ def test_destroy_subscription_asap(): with sub.handle: pass - node.destroy_subscription(sub) + with sub.handle: + node.destroy_subscription(sub) + # handle valid because it's still being used + with sub.handle: + pass with pytest.raises(InvalidHandle): # handle invalid because it was destroyed when no one was using it @@ -161,8 +166,11 @@ def test_destroy_node_asap(): try: node = rclpy.create_node('test_destroy_subscription_asap', context=context) - - node.destroy_node() + with node.handle: + node.destroy_node() + # handle valid because it's still being used + with node.handle: + pass with pytest.raises(InvalidHandle): # handle invalid because it was destroyed when no one was using it @@ -185,7 +193,11 @@ def test_destroy_publisher_asap(): with pub.handle: pass - node.destroy_publisher(pub) + with pub.handle: + node.destroy_publisher(pub) + # handle valid because it's still being used + with pub.handle: + pass 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 99b016280..6c9cab2e8 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.destroy_node() + node.handle.destroy() with pytest.raises(InvalidHandle): with node.handle: pass @@ -38,7 +38,9 @@ def test_handle_destroyed_when_not_used(): try: node = rclpy.create_node('test_handle_destroyed_when_not_used', context=context) with node.handle: - node.destroy_node() + node.handle.destroy() + with node.handle: + pass with pytest.raises(InvalidHandle): with node.handle: diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index 5ba2abfe1..ada7f196d 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -41,6 +41,7 @@ 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 @@ -159,8 +160,8 @@ def test_take(self): basic_types_pub.publish(basic_types_msg) cycle_count = 0 while cycle_count < 5: - with sub.handle: - result = sub.handle.take_message(sub.msg_type, False) + with sub.handle as capsule: + result = _rclpy.rclpy_take(capsule, 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 a94b95865..447b9ba62 100644 --- a/rclpy/test/test_qos_event.py +++ b/rclpy/test/test_qos_event.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import gc import unittest from unittest.mock import Mock @@ -51,11 +50,6 @@ 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) @@ -202,8 +196,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: - event = _rclpy.QoSEvent(parent_entity.handle, event_type) + with parent_entity.handle as parent_capsule: + event = _rclpy.QoSEvent(parent_capsule, event_type) self.assertIsNotNone(event) return event @@ -243,40 +237,41 @@ 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) - with self.context.handle: - wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, self.context.handle) + 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) deadline_event_handle = self._create_event_handle( publisher, QoSPublisherEventType.RCL_PUBLISHER_OFFERED_DEADLINE_MISSED) with deadline_event_handle: - deadline_event_index = wait_set.add_event(deadline_event_handle) + deadline_event_index = _rclpy.rclpy_wait_set_add_event(wait_set, 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 = wait_set.add_event( - liveliness_event_handle) + liveliness_event_index = _rclpy.rclpy_wait_set_add_event( + wait_set, 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 = wait_set.add_event( - incompatible_qos_event_handle) + incompatible_qos_event_index = _rclpy.rclpy_wait_set_add_event( + wait_set, 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. - wait_set.wait(0) - self.assertFalse(wait_set.is_ready('event', deadline_event_index)) - self.assertFalse(wait_set.is_ready('event', liveliness_event_index)) + _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)) if not self.is_fastrtps: - self.assertFalse(wait_set.is_ready( - 'event', incompatible_qos_event_index)) + self.assertFalse(_rclpy.rclpy_wait_set_is_ready( + 'event', wait_set, 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 @@ -315,39 +310,42 @@ 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) - with self.context.handle: - wait_set = _rclpy.WaitSet(0, 0, 0, 0, 0, 3, self.context.handle) + 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) deadline_event_handle = self._create_event_handle( subscription, QoSSubscriptionEventType.RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED) with deadline_event_handle: - deadline_event_index = wait_set.add_event(deadline_event_handle) + deadline_event_index = _rclpy.rclpy_wait_set_add_event( + wait_set, 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 = wait_set.add_event(liveliness_event_handle) + liveliness_event_index = _rclpy.rclpy_wait_set_add_event( + wait_set, 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 = wait_set.add_event( - incompatible_qos_event_handle) + incompatible_qos_event_index = _rclpy.rclpy_wait_set_add_event( + wait_set, 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. - wait_set.wait(0) - self.assertFalse(wait_set.is_ready('event', deadline_event_index)) - self.assertFalse(wait_set.is_ready('event', liveliness_event_index)) + _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)) if not self.is_fastrtps: - self.assertFalse(wait_set.is_ready( - 'event', incompatible_qos_event_index)) + self.assertFalse(_rclpy.rclpy_wait_set_is_ready( + 'event', wait_set, 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 b3f994b71..e12b4e7a8 100644 --- a/rclpy/test/test_subscription.py +++ b/rclpy/test/test_subscription.py @@ -16,7 +16,6 @@ import rclpy from rclpy.node import Node - from test_msgs.msg import Empty @@ -49,13 +48,7 @@ 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): - 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, - ) + sub = make_mock_subscription(namespace, topic_name) assert sub.topic_name == expected @@ -68,11 +61,5 @@ 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): - 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, - ) + sub = make_mock_subscription(namespace, topic_name, cli_args) assert sub.topic_name == expected diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index f79841c51..7e15fa2d6 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: + with node.handle as node_capsule: self.client = _rclpy.Client( - node.handle, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) + node_capsule, 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 wait_set.is_ready('client', self.client_index): + if _rclpy.rclpy_wait_set_is_ready('client', wait_set, 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 = wait_set.add_client(self.client) + self.client_index = _rclpy.rclpy_wait_set_add_client(wait_set, self.client) class ServerWaitable(Waitable): @@ -84,9 +84,9 @@ class ServerWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle: + with node.handle as node_capsule: self.server = _rclpy.Service( - node.handle, EmptySrv, 'test_server', QoSProfile(depth=10).get_c_qos_profile()) + node_capsule, 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 wait_set.is_ready('service', self.server_index): + if _rclpy.rclpy_wait_set_is_ready('service', wait_set, 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 = wait_set.add_service(self.server) + self.server_index = _rclpy.rclpy_wait_set_add_service(wait_set, 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: + with self._clock.handle, node.context.handle as context_capsule: self.timer = _rclpy.Timer( - self._clock.handle, node.context.handle, period_nanoseconds) + self._clock.handle, context_capsule, 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 wait_set.is_ready('timer', self.timer_index): + if _rclpy.rclpy_wait_set_is_ready('timer', wait_set, 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 = wait_set.add_timer(self.timer) + self.timer_index = _rclpy.rclpy_wait_set_add_timer(wait_set, self.timer) class SubscriptionWaitable(Waitable): @@ -173,9 +173,9 @@ class SubscriptionWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.handle: - self.subscription = _rclpy.Subscription( - node.handle, EmptyMsg, 'test_topic', QoSProfile(depth=10).get_c_qos_profile()) + with node.handle as node_capsule: + self.subscription = _rclpy.rclpy_create_subscription( + node_capsule, 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 wait_set.is_ready('subscription', self.subscription_index): + if _rclpy.rclpy_wait_set_is_ready('subscription', wait_set, 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 = self.subscription.take_message(EmptyMsg, False) + msg_info = _rclpy.rclpy_take(self.subscription, 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 = wait_set.add_subscription( - self.subscription) + self.subscription_index = _rclpy.rclpy_wait_set_add_entity( + 'subscription', wait_set, self.subscription) class GuardConditionWaitable(Waitable): @@ -219,8 +219,8 @@ class GuardConditionWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) - with node.context.handle: - self.guard_condition = _rclpy.GuardCondition(node.context.handle) + with node.context.handle as context_capsule: + self.guard_condition = _rclpy.rclpy_create_guard_condition(context_capsule) 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 wait_set.is_ready('guard_condition', self.guard_condition_index): + if _rclpy.rclpy_wait_set_is_ready('guard_condition', wait_set, self.guard_condition_index): self.guard_is_ready = True return self.guard_is_ready @@ -253,7 +253,8 @@ def get_num_entities(self): def add_to_wait_set(self, wait_set): """Add entities to wait set.""" - self.guard_condition_index = wait_set.add_guard_condition(self.guard_condition) + self.guard_condition_index = _rclpy.rclpy_wait_set_add_entity( + 'guard_condition', wait_set, self.guard_condition) class MutuallyExclusiveWaitable(Waitable): @@ -368,7 +369,7 @@ def test_waitable_with_guard_condition(self): self.node.add_waitable(self.waitable) thr = self.start_spin_thread(self.waitable) - self.waitable.guard_condition.trigger_guard_condition() + _rclpy.rclpy_trigger_guard_condition(self.waitable.guard_condition) thr.join() assert self.waitable.future.done()