def create_publisher( self, msg_type, topic: str, *, qos_profile: QoSProfile = qos_profile_default) -> Publisher: """ Create a new publisher. :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. :return: The new publisher. """ # this line imports the typesupport for the message module if not already done check_for_type_support(msg_type) failed = False try: 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) publisher_handle.requires(self.handle) publisher = Publisher(publisher_handle, msg_type, topic, qos_profile) self.__publishers.append(publisher) return publisher
def create_client(self, srv_type, srv_name: str, *, qos_profile: QoSProfile = qos_profile_services_default, callback_group: CallbackGroup = None) -> Client: """ Create a new service client. :param srv_type: The service type. :param srv_name: The name of the service. :param qos_profile: The quality of service profile to apply the service client. :param callback_group: The callback group for the service client. If ``None``, then the nodes default callback group is used. """ if callback_group is None: callback_group = self.default_callback_group check_for_type_support(srv_type) failed = False try: [client_handle, client_pointer ] = _rclpy.rclpy_create_client(self.handle, srv_type, srv_name, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(srv_name, is_service=True) client = Client(self.handle, self.context, client_handle, client_pointer, srv_type, srv_name, qos_profile, callback_group) self.clients.append(client) callback_group.add_entity(client) return client
def deserialize_message(serialized_message: bytes, message_type): """ Deserialize a ROS message. :param serialized_message: The ROS message to deserialized. :param message_type: The type of the serialized ROS message. :return: The deserialized ROS message. """ # this line imports the typesupport for the message module if not already done check_for_type_support(message_type) return _rclpy.rclpy_deserialize(serialized_message, message_type)
def serialize_message(message) -> bytes: """ Serialize a ROS message. :param message: The ROS message to serialize. :return: The serialized bytes. """ message_type = type(message) # this line imports the typesupport for the message module if not already done check_for_type_support(message_type) return _rclpy.rclpy_serialize(message, message_type)
def create_publisher(self, msg_type, topic, *, qos_profile=qos_profile_default): # this line imports the typesupport for the message module if not already done check_for_type_support(msg_type) failed = False try: publisher_handle = _rclpy.rclpy_create_publisher( self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(topic) publisher = Publisher(publisher_handle, msg_type, topic, qos_profile, self.handle) self.publishers.append(publisher) return publisher
def test_check_for_type_support(): type_support.check_for_type_support(Strings) type_support.check_for_type_support(Empty) with pytest.raises(AttributeError): type_support.check_for_type_support(object()) with pytest.raises(NoTypeSupportImportedException): type_support.check_for_type_support(MockType)
def create_subscription(self, msg_type, topic: str, callback: Callable[[MsgType], None], *, qos_profile: QoSProfile = qos_profile_default, callback_group: CallbackGroup = None, raw: bool = False) -> Subscription: """ Create a new subscription. :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 received by the subscription. :param qos_profile: The quality of service profile to apply to the subscription. :param callback_group: The callback group for the subscription. If ``None``, then the nodes default callback group is used. :param raw: If ``True``, then received messages will be stored in raw binary representation. """ if callback_group is None: callback_group = self.default_callback_group # this line imports the typesupport for the message module if not already done check_for_type_support(msg_type) failed = False try: 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) subscription_handle.requires(self.handle) subscription = Subscription(subscription_handle, msg_type, topic, callback, callback_group, qos_profile, raw) self.__subscriptions.append(subscription) callback_group.add_entity(subscription) return subscription
def create_service(self, srv_type, srv_name: str, callback: Callable[[SrvTypeRequest, SrvTypeResponse], SrvTypeResponse], *, qos_profile: QoSProfile = qos_profile_services_default, callback_group: CallbackGroup = None) -> Service: """ Create a new service server. :param srv_type: The service type. :param srv_name: The name of the service. :param callback: A user-defined callback function that is called when a service request received by the server. :param qos_profile: The quality of service profile to apply the service server. :param callback_group: The callback group for the service server. If ``None``, then the nodes default callback group is used. """ if callback_group is None: callback_group = self.default_callback_group check_for_type_support(srv_type) failed = False try: with self.handle as node_capsule: service_capsule = _rclpy.rclpy_create_service( node_capsule, srv_type, srv_name, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(srv_name, is_service=True) service_handle = Handle(service_capsule) service_handle.requires(self.handle) service = Service(service_handle, srv_type, srv_name, callback, callback_group, qos_profile) self.__services.append(service) callback_group.add_entity(service) return service
def create_subscription( self, msg_type, topic, callback, *, qos_profile=qos_profile_default, callback_group=None, raw=False): if callback_group is None: callback_group = self.default_callback_group # this line imports the typesupport for the message module if not already done check_for_type_support(msg_type) failed = False try: [subscription_handle, subscription_pointer] = _rclpy.rclpy_create_subscription( self.handle, msg_type, topic, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(topic) subscription = Subscription( subscription_handle, subscription_pointer, msg_type, topic, callback, callback_group, qos_profile, self.handle, raw) self.subscriptions.append(subscription) callback_group.add_entity(subscription) return subscription
def create_service( self, srv_type, srv_name, callback, *, qos_profile=qos_profile_services_default, callback_group=None): if callback_group is None: callback_group = self.default_callback_group check_for_type_support(srv_type) failed = False try: [service_handle, service_pointer] = _rclpy.rclpy_create_service( self.handle, srv_type, srv_name, qos_profile.get_c_qos_profile()) except ValueError: failed = True if failed: self._validate_topic_or_service_name(srv_name, is_service=True) service = Service( self.handle, service_handle, service_pointer, srv_type, srv_name, callback, callback_group, qos_profile) self.services.append(service) callback_group.add_entity(service) return service
def __init__( self, node, action_type, action_name, *, callback_group=None, goal_service_qos_profile=qos_profile_services_default, result_service_qos_profile=qos_profile_services_default, cancel_service_qos_profile=qos_profile_services_default, feedback_sub_qos_profile=qos_profile_default, status_sub_qos_profile=qos_profile_action_status_default ): """ Constructor. :param node: The ROS node to add the action client to. :param action_type: Type of the action. :param action_name: Name of the action. Used as part of the underlying topic and service names. :param callback_group: Callback group to add the action client to. If None, then the node's default callback group is used. :param goal_service_qos_profile: QoS profile for the goal service. :param result_service_qos_profile: QoS profile for the result service. :param cancel_service_qos_profile: QoS profile for the cancel service. :param feedback_sub_qos_profile: QoS profile for the feedback subscriber. :param status_sub_qos_profile: QoS profile for the status subscriber. """ if callback_group is None: callback_group = node.default_callback_group super().__init__(callback_group) # Import the typesupport for the action module if not already done check_for_type_support(action_type) self._node = node self._action_type = action_type self._client_handle = _rclpy_action.rclpy_action_create_client( node.handle, action_type, action_name, goal_service_qos_profile.get_c_qos_profile(), result_service_qos_profile.get_c_qos_profile(), cancel_service_qos_profile.get_c_qos_profile(), feedback_sub_qos_profile.get_c_qos_profile(), status_sub_qos_profile.get_c_qos_profile() ) self._is_ready = False # key: UUID in bytes, value: weak reference to ClientGoalHandle self._goal_handles = {} # key: goal request sequence_number, value: Future for goal response self._pending_goal_requests = {} # key: goal request sequence_number, value: UUID self._sequence_number_to_goal_id = {} # key: cancel request sequence number, value: Future for cancel response self._pending_cancel_requests = {} # key: result request sequence number, value: Future for result response self._pending_result_requests = {} # key: UUID in bytes, value: callback function self._feedback_callbacks = {} callback_group.add_entity(self) self._node.add_waitable(self)
def __init__(self, node, action_type, action_name, execute_callback, *, callback_group=None, goal_callback=default_goal_callback, handle_accepted_callback=default_handle_accepted_callback, cancel_callback=default_cancel_callback, goal_service_qos_profile=qos_profile_services_default, result_service_qos_profile=qos_profile_services_default, cancel_service_qos_profile=qos_profile_services_default, feedback_pub_qos_profile=QoSProfile(depth=10), status_pub_qos_profile=qos_profile_action_status_default, result_timeout=900): """ Constructor. :param node: The ROS node to add the action server to. :param action_type: Type of the action. :param action_name: Name of the action. Used as part of the underlying topic and service names. :param execute_callback: Callback function for processing accepted goals. This is called if when :class:`ServerGoalHandle.execute()` is called for a goal handle that is being tracked by this action server. :param callback_group: Callback group to add the action server to. If None, then the node's default callback group is used. :param goal_callback: Callback function for handling new goal requests. :param handle_accepted_callback: Callback function for handling newly accepted goals. Passes an instance of `ServerGoalHandle` as an argument. :param cancel_callback: Callback function for handling cancel requests. :param goal_service_qos_profile: QoS profile for the goal service. :param result_service_qos_profile: QoS profile for the result service. :param cancel_service_qos_profile: QoS profile for the cancel service. :param feedback_pub_qos_profile: QoS profile for the feedback publisher. :param status_pub_qos_profile: QoS profile for the status publisher. :param result_timeout: How long in seconds a result is kept by the server after a goal reaches a terminal state. """ if callback_group is None: callback_group = node.default_callback_group super().__init__(callback_group) self._lock = threading.Lock() self.register_handle_accepted_callback(handle_accepted_callback) self.register_goal_callback(goal_callback) self.register_cancel_callback(cancel_callback) self.register_execute_callback(execute_callback) # Import the typesupport for the action module if not already done check_for_type_support(action_type) self._node = node self._action_type = action_type with node.handle as node_capsule, node.get_clock( ).handle as clock_capsule: self._handle = _rclpy_action.rclpy_action_create_server( node_capsule, clock_capsule, action_type, action_name, goal_service_qos_profile.get_c_qos_profile(), result_service_qos_profile.get_c_qos_profile(), cancel_service_qos_profile.get_c_qos_profile(), feedback_pub_qos_profile.get_c_qos_profile(), status_pub_qos_profile.get_c_qos_profile(), result_timeout, ) # key: UUID in bytes, value: GoalHandle self._goal_handles = {} callback_group.add_entity(self) self._node.add_waitable(self)
import rclpy from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from rclpy.clock import Clock from rclpy.clock import ClockType from rclpy.executors import SingleThreadedExecutor from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile from rclpy.task import Future from rclpy.type_support import check_for_type_support from rclpy.waitable import NumberOfEntities from rclpy.waitable import Waitable from test_msgs.msg import Empty as EmptyMsg from test_msgs.srv import Empty as EmptySrv check_for_type_support(EmptyMsg) check_for_type_support(EmptySrv) class ClientWaitable(Waitable): def __init__(self, node): super().__init__(ReentrantCallbackGroup()) with node.handle: self.client = _rclpy.Client( node.handle, EmptySrv, 'test_client', QoSProfile(depth=10).get_c_qos_profile()) self.client_index = None self.client_is_ready = False self.node = node