From aa404312dca422314ec1ff8ab33b734f10ff024c Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 30 Mar 2021 17:08:06 -0300 Subject: [PATCH 1/2] Use py::class_ for rmw_qos_profile_t Signed-off-by: Michel Hidalgo --- rclpy/rclpy/qos.py | 29 ++--- rclpy/src/rclpy/_rclpy_action.cpp | 77 +++++------- rclpy/src/rclpy/_rclpy_pybind11.cpp | 10 +- rclpy/src/rclpy/client.cpp | 8 +- rclpy/src/rclpy/client.hpp | 4 +- rclpy/src/rclpy/publisher.cpp | 8 +- rclpy/src/rclpy/publisher.hpp | 4 +- rclpy/src/rclpy/qos.cpp | 158 ++++++++++++++----------- rclpy/src/rclpy/qos.hpp | 59 +-------- rclpy/src/rclpy/service.cpp | 8 +- rclpy/src/rclpy/service.hpp | 4 +- rclpy/src/rclpy/subscription.cpp | 8 +- rclpy/src/rclpy/subscription.hpp | 4 +- rclpy/test/test_qos.py | 5 +- rclpy/test/test_topic_endpoint_info.py | 3 +- 15 files changed, 160 insertions(+), 229 deletions(-) diff --git a/rclpy/rclpy/qos.py b/rclpy/rclpy/qos.py index 2fd34a33f..c4dc790ce 100644 --- a/rclpy/rclpy/qos.py +++ b/rclpy/rclpy/qos.py @@ -58,7 +58,8 @@ class QoSProfile: """Define Quality of Service policies.""" # default QoS profile not exposed to the user to encourage them to think about QoS settings - __qos_profile_default_dict = _rclpy.rclpy_get_rmw_qos_profile('qos_profile_default') + __qos_profile_default_dict = \ + _rclpy.rmw_qos_profile_t.predefined('qos_profile_default').to_dict() __slots__ = [ '_history', @@ -241,7 +242,7 @@ def avoid_ros_namespace_conventions(self, value): self._avoid_ros_namespace_conventions = value def get_c_qos_profile(self): - return _rclpy.rclpy_convert_from_py_qos_policy( + return _rclpy.rmw_qos_profile_t( self.history, self.depth, self.reliability, @@ -419,26 +420,26 @@ class LivelinessPolicy(QoSPolicyEnum): # 2. ros2/rmw : rmw/include/rmw/qos_profiles.h #: Used for initialization. Should not be used as the actual QoS profile. -qos_profile_unknown = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_unknown')) +qos_profile_unknown = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( + 'qos_profile_unknown').to_dict()) #: Uses the default QoS settings defined in the DDS vendor tool -qos_profile_system_default = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_system_default')) +qos_profile_system_default = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( + 'qos_profile_system_default').to_dict()) #: For sensor data, using best effort reliability and small #: queue depth -qos_profile_sensor_data = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_sensor_data')) +qos_profile_sensor_data = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( + 'qos_profile_sensor_data').to_dict()) #: For services, using reliable reliability and volatile durability -qos_profile_services_default = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_services_default')) +qos_profile_services_default = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( + 'qos_profile_services_default').to_dict()) #: For parameter communication. Similar to service QoS profile but with larger #: queue depth so that requests do not get lost. -qos_profile_parameters = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_parameters')) +qos_profile_parameters = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( + 'qos_profile_parameters').to_dict()) #: For parameter change events. Currently same as the QoS profile for #: parameters. -qos_profile_parameter_events = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile( - 'qos_profile_parameter_events')) +qos_profile_parameter_events = QoSProfile(**_rclpy.rmw_qos_profile_t.predefined( + 'qos_profile_parameter_events').to_dict()) # Separate rcl_action profile defined at # ros2/rcl : rcl/rcl_action/include/rcl_action/default_qos.h diff --git a/rclpy/src/rclpy/_rclpy_action.cpp b/rclpy/src/rclpy/_rclpy_action.cpp index 76f8cd141..8c57de5a5 100644 --- a/rclpy/src/rclpy/_rclpy_action.cpp +++ b/rclpy/src/rclpy/_rclpy_action.cpp @@ -306,13 +306,6 @@ rclpy_action_wait_set_is_ready(py::capsule pyentity, py::capsule pywait_set) throw std::runtime_error(error_text); } -void -copy_qos_profile(rmw_qos_profile_t & profile, py::capsule pyprofile) -{ - auto qos_profile = get_pointer(pyprofile, "rmw_qos_profile_t"); - profile = *qos_profile; -} - /// Create an action client. /** * This function will create an action client for the given action name. @@ -329,16 +322,11 @@ copy_qos_profile(rmw_qos_profile_t & profile, py::capsule pyprofile) * \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] pygoal_service_qos Capsule pointing to a rmw_qos_profile_t object - * for the goal service. - * \param[in] pyresult_service_qos Capsule pointing to a rmw_qos_profile_t object - * for the result service. - * \param[in] pycancel_service_qos Capsule pointing to a rmw_qos_profile_t object - * for the cancel service. - * \param[in] pyfeedback_qos Capsule pointing to a rmw_qos_profile_t object - * for the feedback subscriber. - * \param[in] pystatus_qos Capsule pointing to a rmw_qos_profile_t object for the - * status subscriber. + * \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. */ @@ -347,11 +335,11 @@ rclpy_action_create_client( py::capsule pynode, py::object pyaction_type, const char * action_name, - py::capsule pygoal_service_qos, - py::capsule pyresult_service_qos, - py::capsule pycancel_service_qos, - py::capsule pyfeedback_topic_qos, - py::capsule pystatus_topic_qos) + 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")); @@ -368,11 +356,11 @@ rclpy_action_create_client( rcl_action_client_options_t action_client_ops = rcl_action_client_get_default_options(); - copy_qos_profile(action_client_ops.goal_service_qos, pygoal_service_qos); - copy_qos_profile(action_client_ops.result_service_qos, pyresult_service_qos); - copy_qos_profile(action_client_ops.cancel_service_qos, pycancel_service_qos); - copy_qos_profile(action_client_ops.feedback_topic_qos, pyfeedback_topic_qos); - copy_qos_profile(action_client_ops.status_topic_qos, pystatus_topic_qos); + 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( @@ -423,16 +411,11 @@ rclpy_action_create_client( * \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] pygoal_service_qos Capsule pointing to a rmw_qos_profile_t object - * for the goal service. - * \param[in] pyresult_service_qos Capsule pointing to a rmw_qos_profile_t object - * for the result service. - * \param[in] pycancel_service_qos Capsule pointing to a rmw_qos_profile_t object - * for the cancel service. - * \param[in] pyfeedback_qos Capsule pointing to a rmw_qos_profile_t object - * for the feedback subscriber. - * \param[in] pystatus_qos Capsule pointing to a rmw_qos_profile_t object for the - * status subscriber. + * \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. */ @@ -442,11 +425,11 @@ rclpy_action_create_server( py::capsule pyclock, py::object pyaction_type, const char * action_name, - py::capsule pygoal_service_qos, - py::capsule pyresult_service_qos, - py::capsule pycancel_service_qos, - py::capsule pyfeedback_topic_qos, - py::capsule pystatus_topic_qos, + 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( @@ -469,11 +452,11 @@ rclpy_action_create_server( rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options(); - copy_qos_profile(action_server_ops.goal_service_qos, pygoal_service_qos); - copy_qos_profile(action_server_ops.result_service_qos, pyresult_service_qos); - copy_qos_profile(action_server_ops.cancel_service_qos, pycancel_service_qos); - copy_qos_profile(action_server_ops.feedback_topic_qos, pyfeedback_topic_qos); - copy_qos_profile(action_server_ops.status_topic_qos, pystatus_topic_qos); + 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);}; diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index 4e9a1d147..ffb8727f5 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -393,15 +393,7 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { "rclpy_remove_ros_args", &rclpy::remove_ros_args, "Remove ROS-specific arguments from argument vector."); - m.def( - "rclpy_convert_from_py_qos_policy", &rclpy::convert_from_py_qos_policy, - "Convert rclpy.qos.QoSProfile arguments into a rmw_qos_profile_t."); - m.def( - "rclpy_convert_to_py_qos_policy", &rclpy::convert_to_py_qos_policy, - "Convert a rmw_qos_profile_t into rclpy.qos.QoSProfile keyword arguments."); - m.def( - "rclpy_get_rmw_qos_profile", &rclpy::get_rmw_qos_profile, - "Fetch a predefined rclpy.qos.QoSProfile keyword arguments."); + rclpy::define_rmw_qos_profile(m); m.def( "rclpy_logging_fini", rclpy::logging_fini, diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index a171d568e..1d476c9b3 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -37,7 +37,7 @@ Client::destroy() } Client::Client( - py::capsule pynode, py::object pysrv_type, const char * service_name, py::capsule pyqos_profile) + 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"); @@ -51,11 +51,7 @@ Client::Client( rcl_client_options_t client_ops = rcl_client_get_default_options(); if (!pyqos_profile.is_none()) { - if (0 != strcmp("rmw_qos_profile_t", pyqos_profile.name())) { - throw py::value_error("capsule is not an rmw_qos_profile_t"); - } - auto qos_profile = static_cast(pyqos_profile); - client_ops.qos = *qos_profile; + client_ops.qos = pyqos_profile.cast(); } // Create a client diff --git a/rclpy/src/rclpy/client.hpp b/rclpy/src/rclpy/client.hpp index 0600fca6a..f9f5a9f34 100644 --- a/rclpy/src/rclpy/client.hpp +++ b/rclpy/src/rclpy/client.hpp @@ -43,9 +43,9 @@ class Client : public Destroyable * \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_profile QoSProfile Python object for this client + * \param[in] pyqos rmw_qos_profile_t object for this client */ - Client(py::capsule pynode, py::object pysrv_type, const char * service_name, py::capsule pyqos); + Client(py::capsule pynode, py::object pysrv_type, const char * service_name, py::object pyqos); ~Client() = default; diff --git a/rclpy/src/rclpy/publisher.cpp b/rclpy/src/rclpy/publisher.cpp index 555a459c2..5a73882fd 100644 --- a/rclpy/src/rclpy/publisher.cpp +++ b/rclpy/src/rclpy/publisher.cpp @@ -55,7 +55,7 @@ _rclpy_destroy_publisher(void * p) py::capsule publisher_create( py::capsule pynode, py::object pymsg_type, std::string topic, - py::capsule pyqos_profile) + py::object pyqos_profile) { auto node = static_cast( rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); @@ -72,11 +72,7 @@ publisher_create( rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options(); if (!pyqos_profile.is_none()) { - if (0 != strcmp("rmw_qos_profile_t", pyqos_profile.name())) { - throw py::value_error("capsule is not an rmw_qos_profile_t"); - } - auto qos_profile = static_cast(pyqos_profile); - publisher_ops.qos = *qos_profile; + publisher_ops.qos = pyqos_profile.cast(); } // Use smart pointer to make sure memory is free'd on error diff --git a/rclpy/src/rclpy/publisher.hpp b/rclpy/src/rclpy/publisher.hpp index f8d6b2046..a42bfb377 100644 --- a/rclpy/src/rclpy/publisher.hpp +++ b/rclpy/src/rclpy/publisher.hpp @@ -36,13 +36,13 @@ namespace rclpy * \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 QoSProfile object with the profile of this publisher + * \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 */ py::capsule publisher_create( py::capsule pynode, py::object pymsg_type, std::string topic, - py::capsule pyqos_profile); + py::object pyqos_profile); /// Get the name of the logger associated with the node of the publisher. /** diff --git a/rclpy/src/rclpy/qos.cpp b/rclpy/src/rclpy/qos.cpp index e5fac7371..f153a8501 100644 --- a/rclpy/src/rclpy/qos.cpp +++ b/rclpy/src/rclpy/qos.cpp @@ -38,24 +38,13 @@ namespace rclpy QoSCheckCompatibleResult qos_check_compatible( - const py::capsule & publisher_qos_profile, - const py::capsule & subscription_qos_profile -) + const rmw_qos_profile_t & publisher_qos_profile, + const rmw_qos_profile_t & subscription_qos_profile) { - if (0 != strcmp("rmw_qos_profile_t", publisher_qos_profile.name())) { - throw py::value_error("capsule is not an rmw_qos_profile_t"); - } - auto publisher_qos_profile_ = static_cast(publisher_qos_profile); - - if (0 != strcmp("rmw_qos_profile_t", subscription_qos_profile.name())) { - throw py::value_error("capsule is not an rmw_qos_profile_t"); - } - auto subscription_qos_profile_ = static_cast(subscription_qos_profile); - QoSCheckCompatibleResult result; rmw_ret_t ret = rmw_qos_profile_check_compatible( - *publisher_qos_profile_, - *subscription_qos_profile_, + publisher_qos_profile, + subscription_qos_profile, &result.compatibility, result.reason, sizeof(QoSCheckCompatibleResult::reason)); @@ -78,10 +67,28 @@ _convert_py_duration_to_rmw_time(const rcl_duration_t & duration, rmw_time_t * o out_time->nsec = duration.nanoseconds % (1000LL * 1000LL * 1000LL); } -} // namespace - -py::capsule -convert_from_py_qos_policy( +// Create an rmw_qos_profile_t instance. +/** + * Raises ValueError if a any capsule is not of the expected type. + * Raises MemoryError if rmw_qos_profile_t allocation fails. + * + * \param[in] qos_history an rclpy.qos.QoSHistoryPolicy value. + * \param[in] qos_depth depth of the message queue. + * \param[in] qos_reliability an rclpy.qos.QoSReliabilityPolicy value. + * \param[in] qos_durability an rclpy.qos.QoSDurabilityPolicy value. + * \param[in] pyqos_lifespan lifespan QoS policy parameter of + * rcl_duration_t type. + * \param[in] pyqos_deadline deadline QoS policy parameter of + * rcl_duration_t type. + * \param[in] qos_liveliness an rclpy.qos.QoSLivelinessPolicy value. + * \param[in] pyqos_liveliness_lease_duration livelines QoS policy + * lease duration of rcl_duration_t type. + * \param[in] avoid_ros_namespace_conventions Whether to use ROS + * namespace conventions or not. + * \return an rmw_qos_profile_t instance. + */ +rmw_qos_profile_t +create_qos_profile( int qos_history, int qos_depth, int qos_reliability, @@ -92,81 +99,92 @@ convert_from_py_qos_policy( const rcl_duration_t & pyqos_liveliness_lease_duration, bool avoid_ros_namespace_conventions) { - auto qos_profile = - std::unique_ptr( - static_cast( - PyMem_Malloc(sizeof(rmw_qos_profile_t))), PyMem_Free); - if (!qos_profile) { - throw std::bad_alloc(); - } // Set to default so that we don't use uninitialized data if new fields are added - *qos_profile = rmw_qos_profile_default; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; // Overwrite defaults with passed values - qos_profile->history = static_cast(qos_history); - qos_profile->depth = qos_depth; - qos_profile->reliability = static_cast(qos_reliability); - qos_profile->durability = static_cast(qos_durability); + qos_profile.history = static_cast(qos_history); + qos_profile.depth = qos_depth; + qos_profile.reliability = static_cast(qos_reliability); + qos_profile.durability = static_cast(qos_durability); - _convert_py_duration_to_rmw_time(pyqos_lifespan, &(qos_profile->lifespan)); - _convert_py_duration_to_rmw_time(pyqos_deadline, &(qos_profile->deadline)); + _convert_py_duration_to_rmw_time(pyqos_lifespan, &(qos_profile.lifespan)); + _convert_py_duration_to_rmw_time(pyqos_deadline, &(qos_profile.deadline)); - qos_profile->liveliness = static_cast(qos_liveliness); + qos_profile.liveliness = static_cast(qos_liveliness); _convert_py_duration_to_rmw_time( pyqos_liveliness_lease_duration, - &(qos_profile->liveliness_lease_duration)); + &(qos_profile.liveliness_lease_duration)); - qos_profile->avoid_ros_namespace_conventions = avoid_ros_namespace_conventions; + qos_profile.avoid_ros_namespace_conventions = avoid_ros_namespace_conventions; - return py::capsule( - qos_profile.release(), "rmw_qos_profile_t", - [](PyObject * pycapsule_c) { - PyMem_Free(PyCapsule_GetPointer(pycapsule_c, "rmw_qos_profile_t")); - }); + return qos_profile; } +// Convert a a rmw_qos_profile_t instance to a dictionary. +/** + * This function is exposed to facilitate testing profile type conversion. + * + * \param[in] qos_profile an rmw_qos_profile_t instance + * \return a dictionary suitable for rclpy.qos.QoSProfile instantiation. + */ py::dict -convert_to_py_qos_policy(py::capsule pyqos_profile) +convert_qos_profile_to_dict(const rmw_qos_profile_t & qos_profile) { - if (0 != strcmp("rmw_qos_profile_t", pyqos_profile.name())) { - throw py::value_error("capsule is not an rmw_qos_profile_t"); - } - auto qos_profile = static_cast(pyqos_profile); - PyObject * pydict = rclpy_common_convert_to_qos_dict(qos_profile); + PyObject * pydict = rclpy_common_convert_to_qos_dict(&qos_profile); if (!pydict) { throw py::error_already_set(); } return py::reinterpret_steal(pydict); } -py::dict -get_rmw_qos_profile(const char * qos_profile_name) +// Fetch a predefined rmw_qos_profile_t instance. +/** + * Raises InvalidArgument if the given \p qos_profile_name is unknown. + * + * \param[in] qos_profile_name Name of the profile to fetch. + * \return an rmw_qos_profile_t instance + */ +rmw_qos_profile_t +predefined_qos_profile_from_name(const char * qos_profile_name) { - PyObject * pyqos_profile = nullptr; if (0 == strcmp(qos_profile_name, "qos_profile_sensor_data")) { - pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_sensor_data); - } else if (0 == strcmp(qos_profile_name, "qos_profile_default")) { - pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_default); - } else if (0 == strcmp(qos_profile_name, "qos_profile_system_default")) { - pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_system_default); - } else if (0 == strcmp(qos_profile_name, "qos_profile_services_default")) { - pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_services_default); - } else if (0 == strcmp(qos_profile_name, "qos_profile_unknown")) { - pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_unknown); - } else if (0 == strcmp(qos_profile_name, "qos_profile_parameters")) { - pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_parameters); - } else if (0 == strcmp(qos_profile_name, "qos_profile_parameter_events")) { - pyqos_profile = rclpy_common_convert_to_qos_dict(&rmw_qos_profile_parameter_events); - } else { - std::string error_text = "Requested unknown rmw_qos_profile: "; - error_text += qos_profile_name; - throw std::invalid_argument(error_text); + return rmw_qos_profile_sensor_data; } - if (!pyqos_profile) { - throw py::error_already_set(); + if (0 == strcmp(qos_profile_name, "qos_profile_default")) { + return rmw_qos_profile_default; + } + if (0 == strcmp(qos_profile_name, "qos_profile_system_default")) { + return rmw_qos_profile_system_default; } - return py::reinterpret_steal(pyqos_profile); + if (0 == strcmp(qos_profile_name, "qos_profile_services_default")) { + return rmw_qos_profile_services_default; + } + if (0 == strcmp(qos_profile_name, "qos_profile_unknown")) { + return rmw_qos_profile_unknown; + } + if (0 == strcmp(qos_profile_name, "qos_profile_parameters")) { + return rmw_qos_profile_parameters; + } + if (0 == strcmp(qos_profile_name, "qos_profile_parameter_events")) { + return rmw_qos_profile_parameter_events; + } + + std::string error_text = "Requested unknown rmw_qos_profile: "; + error_text += qos_profile_name; + throw std::invalid_argument(error_text); +} + +} // namespace + +void +define_rmw_qos_profile(py::object module) +{ + py::class_(module, "rmw_qos_profile_t") + .def(py::init<>(&create_qos_profile)) + .def("to_dict", &convert_qos_profile_to_dict) + .def_static("predefined", &predefined_qos_profile_from_name); } } // namespace rclpy diff --git a/rclpy/src/rclpy/qos.hpp b/rclpy/src/rclpy/qos.hpp index cfb6421d5..818c35153 100644 --- a/rclpy/src/rclpy/qos.hpp +++ b/rclpy/src/rclpy/qos.hpp @@ -64,63 +64,16 @@ struct QoSCheckCompatibleResult */ QoSCheckCompatibleResult qos_check_compatible( - const py::capsule & publisher_qos_profile, - const py::capsule & subscription_qos_profile + const rmw_qos_profile_t & publisher_qos_profile, + const rmw_qos_profile_t & subscription_qos_profile ); -/// Convert rclpy.qos.QoSProfile arguments to a C rmw_qos_profile_t capsule. +/// Define a pybind11 wrapper for an rmw_qos_profile_t /** - * Raises ValueError if a any capsule is not of the expected type. - * Raises MemoryError if rmw_qos_profile_t allocation fails. - * - * \param[in] qos_history an rclpy.qos.QoSHistoryPolicy value. - * \param[in] qos_depth depth of the message queue. - * \param[in] qos_reliability an rclpy.qos.QoSReliabilityPolicy value. - * \param[in] qos_durability an rclpy.qos.QoSDurabilityPolicy value. - * \param[in] pyqos_lifespan lifespan QoS policy parameter - * as an rcl_duration_t capsule. - * \param[in] pyqos_deadline deadline QoS policy parameter - * as an rcl_duration_t capsule. - * \param[in] qos_liveliness an rclpy.qos.QoSLivelinessPolicy value. - * \param[in] pyqos_liveliness_lease_duration livelines QoS policy - * lease duration as an rcl_duration_t capsule. - * \param[in] avoid_ros_namespace_conventions Whether to use ROS - * namespace conventions or not. - * \return Capsule pointing to a rmw_qos_profile_t C struct. - */ -py::capsule -convert_from_py_qos_policy( - int qos_history, - int qos_depth, - int qos_reliability, - int qos_durability, - const rcl_duration_t & pyqos_lifespan, - const rcl_duration_t & pyqos_deadline, - int qos_liveliness, - const rcl_duration_t & pyqos_liveliness_lease_duration, - bool avoid_ros_namespace_conventions); - -/// Convert a C rmw_qos_profile_t capsule to a dictionary. -/** - * This function is exposed to facilitate testing profile type conversion. - * - * Raises ValueError if a \p pyqos_profile is not an rmw_qos_profile_t capsule. - * - * \param[in] pyqos_profile Capsule pointing to rmw_qos_profile_t - * \return a dictionary suitable for rclpy.qos.QoSProfile instantiation. - */ -py::dict -convert_to_py_qos_policy(py::capsule pyqos_profile); - -/// Fetch a predefined rclpy.qos.QoSProfile keyword arguments. -/** - * Raises InvalidArgument if the given \p rmw_profile is unknown. - * - * \param[in] qos_profile_name Name of the profile to fetch. - * \return a dictionary suitable for rclpy.qos.QoSProfile instantiation. + * \param[in] module a pybind11 module to add the definition to */ -py::dict -get_rmw_qos_profile(const char * qos_profile_name); +void +define_rmw_qos_profile(py::object module); } // namespace rclpy diff --git a/rclpy/src/rclpy/service.cpp b/rclpy/src/rclpy/service.cpp index 269fb6091..d1cb3836a 100644 --- a/rclpy/src/rclpy/service.cpp +++ b/rclpy/src/rclpy/service.cpp @@ -57,7 +57,7 @@ _rclpy_destroy_service(void * p) py::capsule service_create( py::capsule pynode, py::object pysrv_type, std::string service_name, - py::capsule pyqos_profile) + py::object pyqos_profile) { auto node = static_cast( rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); @@ -74,11 +74,7 @@ service_create( rcl_service_options_t service_ops = rcl_service_get_default_options(); if (!pyqos_profile.is_none()) { - if (0 != strcmp("rmw_qos_profile_t", pyqos_profile.name())) { - throw py::value_error("capsule is not an rmw_qos_profile_t"); - } - auto qos_profile = static_cast(pyqos_profile); - service_ops.qos = *qos_profile; + service_ops.qos = pyqos_profile.cast(); } // Use smart pointer to make sure memory is free'd on error diff --git a/rclpy/src/rclpy/service.hpp b/rclpy/src/rclpy/service.hpp index d7de1bea1..c704078f0 100644 --- a/rclpy/src/rclpy/service.hpp +++ b/rclpy/src/rclpy/service.hpp @@ -38,13 +38,13 @@ namespace rclpy * \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 + * \param[in] pyqos_profile rmw_qos_profile_t object for this service * \return capsule containing the rcl_service_t */ py::capsule service_create( py::capsule pynode, py::object pysrv_type, std::string service_name, - py::capsule pyqos_profile); + py::object pyqos_profile); /// Publish a response message /** diff --git a/rclpy/src/rclpy/subscription.cpp b/rclpy/src/rclpy/subscription.cpp index 98b81b5d7..6a20d8f26 100644 --- a/rclpy/src/rclpy/subscription.cpp +++ b/rclpy/src/rclpy/subscription.cpp @@ -61,7 +61,7 @@ _rclpy_destroy_subscription(void * p) py::capsule subscription_create( py::capsule pynode, py::object pymsg_type, std::string topic, - py::capsule pyqos_profile) + py::object pyqos_profile) { auto node = static_cast( rclpy_handle_get_pointer_from_capsule(pynode.ptr(), "rcl_node_t")); @@ -78,11 +78,7 @@ subscription_create( rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options(); if (!pyqos_profile.is_none()) { - if (0 != strcmp("rmw_qos_profile_t", pyqos_profile.name())) { - throw py::value_error("capsule is not an rmw_qos_profile_t"); - } - auto qos_profile = static_cast(pyqos_profile); - subscription_ops.qos = *qos_profile; + subscription_ops.qos = pyqos_profile.cast(); } // Use smart pointer to make sure memory is free'd on error diff --git a/rclpy/src/rclpy/subscription.hpp b/rclpy/src/rclpy/subscription.hpp index 48a8b7dba..5c31aa757 100644 --- a/rclpy/src/rclpy/subscription.hpp +++ b/rclpy/src/rclpy/subscription.hpp @@ -38,13 +38,13 @@ namespace rclpy * \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 QoSProfile Python object for this subscription + * \param[in] pyqos_profile rmw_qos_profile_t object for this subscription * \return capsule containing the rclpy_subscription_t */ py::capsule subscription_create( py::capsule pynode, py::object pymsg_type, std::string topic, - py::capsule pyqos_profile); + py::object pyqos_profile); /// Take a message and its metadata from a subscription /** diff --git a/rclpy/test/test_qos.py b/rclpy/test/test_qos.py index ef694c8d2..9938a6342 100644 --- a/rclpy/test/test_qos.py +++ b/rclpy/test/test_qos.py @@ -15,7 +15,6 @@ import unittest from rclpy.duration import Duration -from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import InvalidQoSProfileException from rclpy.qos import qos_check_compatible from rclpy.qos import qos_profile_system_default @@ -31,8 +30,8 @@ class TestQosProfile(unittest.TestCase): def convert_and_assert_equality(self, qos_profile): - c_profile = qos_profile.get_c_qos_profile() - converted_profile = QoSProfile(**_rclpy.rclpy_convert_to_py_qos_policy(c_profile)) + c_qos_profile = qos_profile.get_c_qos_profile() + converted_profile = QoSProfile(**c_qos_profile.to_dict()) self.assertEqual(qos_profile, converted_profile) def test_depth_only_constructor(self): diff --git a/rclpy/test/test_topic_endpoint_info.py b/rclpy/test/test_topic_endpoint_info.py index 4f173545a..643ec0ec5 100644 --- a/rclpy/test/test_topic_endpoint_info.py +++ b/rclpy/test/test_topic_endpoint_info.py @@ -77,7 +77,8 @@ def test_endpoint_gid_only_constructor(self): self.assertEqual(test_endpoint_gid, info_from_ctor.endpoint_gid) def test_qos_profile_only_constructor(self): - test_qos_profile = QoSProfile(**_rclpy.rclpy_get_rmw_qos_profile('qos_profile_default')) + c_qos_profile = _rclpy.rmw_qos_profile_t.predefined('qos_profile_default') + test_qos_profile = QoSProfile(**c_qos_profile.to_dict()) info_for_ref = TopicEndpointInfo() info_for_ref.qos_profile = test_qos_profile From 0d1b355c4488003da6344cf1116bc4ca0ea6dace Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Wed, 31 Mar 2021 10:07:46 -0300 Subject: [PATCH 2/2] Update Client constructor binding signature Signed-off-by: Michel Hidalgo --- rclpy/src/rclpy/client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclpy/src/rclpy/client.cpp b/rclpy/src/rclpy/client.cpp index 1d476c9b3..e6289434f 100644 --- a/rclpy/src/rclpy/client.cpp +++ b/rclpy/src/rclpy/client.cpp @@ -167,7 +167,7 @@ void define_client(py::object module) { py::class_(module, "Client") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Client & client) { return reinterpret_cast(client.rcl_ptr());