Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add API for checking QoS profile compatibility #708

Merged
merged 20 commits into from
Mar 25, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions rclpy/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -176,6 +176,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED
src/rclpy/names.cpp
src/rclpy/node.cpp
src/rclpy/publisher.cpp
src/rclpy/qos.cpp
src/rclpy/qos_events.cpp
src/rclpy/serialization.cpp
src/rclpy/service.cpp
Expand Down
26 changes: 26 additions & 0 deletions rclpy/rclpy/qos.py
Original file line number Diff line number Diff line change
Expand Up @@ -471,3 +471,29 @@ def short_keys(cls):
def get_from_short_key(cls, name):
"""Retrieve a policy type from a short name, case-insensitive."""
return cls[name.upper()].value


QoSCompatibility = _rclpy.QoSCompatibility


def qos_check_compatible(publisher_qos: QoSProfile, subscription_qos: QoSProfile):
audrow marked this conversation as resolved.
Show resolved Hide resolved
"""
Check if two QoS profiles are compatible.

Two QoS profiles are compatible if a publisher and subscription
using the QoS policies can communicate with each other.

If any policies have value "system default" or "unknown" then it is possible that
compatibility cannot be determined.
In this case, the value QoSCompatility.WARNING is set as part of
the returned structure.
"""
result = _rclpy.rclpy_qos_check_compatible(
publisher_qos.get_c_qos_profile(),
subscription_qos.get_c_qos_profile()
)
compatibility = QoSCompatibility(
result.compatibility
)
reason = result.reason
return compatibility, reason
17 changes: 17 additions & 0 deletions rclpy/src/rclpy/_rclpy_pybind11.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include "names.hpp"
#include "node.hpp"
#include "publisher.hpp"
#include "qos.hpp"
#include "qos_events.hpp"
#include "rclpy_common/exceptions.hpp"
#include "serialization.hpp"
Expand Down Expand Up @@ -74,6 +75,18 @@ PYBIND11_MODULE(_rclpy_pybind11, m) {
.value("RCL_PUBLISHER_LIVELINESS_LOST", RCL_PUBLISHER_LIVELINESS_LOST)
.value("RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS", RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS);

py::enum_<rmw_qos_compatibility_type_t>(m, "QoSCompatibility")
.value("OK", RMW_QOS_COMPATIBILITY_OK)
.value("WARNING", RMW_QOS_COMPATIBILITY_WARNING)
.value("ERROR", RMW_QOS_COMPATIBILITY_ERROR);

py::class_<rclpy::QoSCheckCompatibleResult>(
m, "QoSCheckCompatibleResult",
"Result type for checking QoS compatibility with result")
.def(py::init<>())
.def_readonly("compatibility", &rclpy::QoSCheckCompatibleResult::compatibility)
.def_readonly("reason", &rclpy::QoSCheckCompatibleResult::reason);

py::register_exception<rclpy::RCUtilsError>(m, "RCUtilsError", PyExc_RuntimeError);
py::register_exception<rclpy::RMWError>(m, "RMWError", PyExc_RuntimeError);
auto rclerror = py::register_exception<rclpy::RCLError>(m, "RCLError", PyExc_RuntimeError);
Expand Down Expand Up @@ -158,6 +171,10 @@ PYBIND11_MODULE(_rclpy_pybind11, m) {
"rclpy_service_info_get_received_timestamp", &rclpy::service_info_get_received_timestamp,
"Retrieve received timestamp from service_info");

m.def(
"rclpy_qos_check_compatible", &rclpy::qos_check_compatible,
"Check if two QoS profiles are compatible.");

m.def(
"rclpy_create_guard_condition", &rclpy::guard_condition_create,
"Create a general purpose guard condition");
Expand Down
61 changes: 61 additions & 0 deletions rclpy/src/rclpy/qos.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,61 @@
// 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 <rmw/error_handling.h>
#include <rmw/incompatible_qos_events_statuses.h>
#include <rmw/qos_profiles.h>
#include <rmw/types.h>

#include <pybind11/pybind11.h>

#include "qos.hpp"
#include "rclpy_common/exceptions.hpp"

namespace rclpy
{

QoSCheckCompatibleResult
qos_check_compatible(
const py::capsule & publisher_qos_profile,
const py::capsule & 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<rmw_qos_profile_t *>(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<rmw_qos_profile_t *>(subscription_qos_profile);

QoSCheckCompatibleResult result;
rmw_ret_t ret = rmw_qos_profile_check_compatible(
*publisher_qos_profile_,
*subscription_qos_profile_,
&result.compatibility,
result.reason,
sizeof(QoSCheckCompatibleResult::reason));

if (RMW_RET_OK != ret) {
throw RMWError("failed to check if qos profiles are compatible");
}

return result;
}

} // namespace rclpy
73 changes: 73 additions & 0 deletions rclpy/src/rclpy/qos.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,73 @@
// 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__QOS_HPP_
#define RCLPY__QOS_HPP_

#include <rmw/qos_profiles.h>

#include <pybind11/pybind11.h>


namespace py = pybind11;

namespace rclpy
{

/// Result type for checking QoS compatibility
/**
* \see rclpy::qos_check_compatible()
*/
struct QoSCheckCompatibleResult
{
/// Compatibility result.
rmw_qos_compatibility_type_t compatibility;

/// Reason for a (possible) incompatibility.
/**
* Set if compatiblity is RMW_QOS_COMPATIBILITY_WARNING or RMW_QOS_COMPATIBILITY_ERROR.
* Not set if the QoS profiles are compatible.
*/
char reason[2048];
};

/// Check if two QoS profiles are compatible.
/**
* Two QoS profiles are compatible if a publisher and subcription
* using the QoS policies can communicate with each other.
*
* If any policies have value "system default" or "unknown" then it is possible that
* compatiblity cannot be determined.
* In this case, the value RMW_QOS_COMPATIBILITY_WARNING is set as part of
* the returned structure.
*
* \param[in] publisher_qos_profile: The QoS profile for a publisher.
* \param[in] subscription_qos_profile: The QoS profile for a subscription.
* \return Struct with compatiblity set to RMW_QOS_COMPATIBILITY_OK if the QoS profiles are
* compatible, or
* \return Struct with compatibility set to RMW_QOS_COMPATIBILITY_WARNING if there is a chance
* the QoS profiles are not compatible, or
* \return Struct with compatibility set to RMW_QOS_COMPATIBILITY_ERROR if the QoS profiles are
* not compatible.
* \throws RMWError if an unexpected error occurs.
*/
QoSCheckCompatibleResult
qos_check_compatible(
const py::capsule & publisher_qos_profile,
const py::capsule & subscription_qos_profile
);

} // namespace rclpy

#endif // RCLPY__QOS_HPP_
64 changes: 64 additions & 0 deletions rclpy/test/test_qos.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,9 @@
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
from rclpy.qos import QoSCompatibility
from rclpy.qos import QoSDurabilityPolicy
from rclpy.qos import QoSHistoryPolicy
from rclpy.qos import QoSLivelinessPolicy
Expand Down Expand Up @@ -124,3 +126,65 @@ def test_preset_profiles(self):
assert (
QoSPresetProfiles.SYSTEM_DEFAULT.value ==
QoSPresetProfiles.get_from_short_key('system_default'))


class TestCheckQosCompatibility(unittest.TestCase):

def test_compatible(self):
qos = QoSProfile(
depth=1,
reliability=QoSReliabilityPolicy.RELIABLE,
durability=QoSDurabilityPolicy.VOLATILE,
audrow marked this conversation as resolved.
Show resolved Hide resolved
lifespan=Duration(seconds=1),
deadline=Duration(seconds=1),
liveliness=QoSLivelinessPolicy.AUTOMATIC,
liveliness_lease_duration=Duration(seconds=1),
)
compatibility, reason = qos_check_compatible(
qos, qos
)

assert compatibility == QoSCompatibility.OK
assert reason == ''

def test_incompatible(self):
audrow marked this conversation as resolved.
Show resolved Hide resolved
"""
This test is assuming a DDS implementation.

It's possible that a "best effort" publisher and "reliable"
subscription is a valid match in a non-DDS implementation.
"""
pub_qos = QoSProfile(
depth=1,
reliability=QoSReliabilityPolicy.BEST_EFFORT,
)
sub_qos = QoSProfile(
depth=1,
reliability=QoSReliabilityPolicy.RELIABLE,
)

compatibility, reason = qos_check_compatible(
pub_qos, sub_qos
)

assert compatibility == QoSCompatibility.ERROR
assert reason != ''

def test_warn_of_possible_incompatibility(self):
audrow marked this conversation as resolved.
Show resolved Hide resolved
"""
This test is assuming a DDS implementation.

It's possible that a "best effort" publisher and "reliable"
subscription is a valid match in a non-DDS implementation.
"""
pub_qos = QoSPresetProfiles.SYSTEM_DEFAULT.value
sub_qos = QoSProfile(
depth=1,
reliability=QoSReliabilityPolicy.RELIABLE,
)
compatibility, reason = qos_check_compatible(
pub_qos, sub_qos
)

assert compatibility == QoSCompatibility.WARNING
assert reason != ''