From 58fc68ce29ea20865f3e1fbf570ced7f8c548184 Mon Sep 17 00:00:00 2001 From: Andrea Sorbini Date: Fri, 23 Apr 2021 11:24:41 -0700 Subject: [PATCH 1/3] Improved conversion of time values between ROS and DDS formats Signed-off-by: Andrea Sorbini --- rmw_connextdds_common/src/common/rmw_impl.cpp | 90 ++++++++++--------- 1 file changed, 47 insertions(+), 43 deletions(-) diff --git a/rmw_connextdds_common/src/common/rmw_impl.cpp b/rmw_connextdds_common/src/common/rmw_impl.cpp index 90ac36f4..05733937 100644 --- a/rmw_connextdds_common/src/common/rmw_impl.cpp +++ b/rmw_connextdds_common/src/common/rmw_impl.cpp @@ -19,6 +19,8 @@ #include #include +#include "rmw_dds_common/time_utils.hpp" + #include "rmw_connextdds/graph_cache.hpp" #define ROS_SERVICE_REQUESTER_PREFIX_STR "rq" @@ -244,6 +246,34 @@ dds_qos_policy_to_rmw_qos_policy(const DDS_QosPolicyId_t last_policy_id) return RMW_QOS_POLICY_INVALID; } +static +DDS_Duration_t +rmw_time_to_dds_duration(const rmw_time_t & time) +{ + if (rmw_time_equal(time, RMW_DURATION_INFINITE)) { + return DDS_DURATION_INFINITE; + } + // Use rmw_dds_common::clamp_rmw_time_to_dds_time() to make sure value doesn't + // exceed the valid domain for DDS_Duration_t. + const rmw_time_t ctime = rmw_dds_common::clamp_rmw_time_to_dds_time(time); + DDS_Duration_t duration; + duration.sec = static_cast(ctime.sec); + duration.nanosec = static_cast(ctime.nsec); + return duration; +} + +static +rmw_time_t +dds_duration_to_rmw_time(const DDS_Duration_t & duration) +{ + if (DDS_Duration_is_infinite(&duration)) { + return RMW_DURATION_INFINITE; + } + assert(duration.sec > 0); + rmw_time_t result = {static_cast(duration.sec), duration.nanosec}; + return result; +} + rmw_ret_t rmw_connextdds_get_readerwriter_qos( const bool writer_qos, @@ -360,21 +390,13 @@ rmw_connextdds_get_readerwriter_qos( } } - if (qos_policies->deadline.sec != 0 || qos_policies->deadline.nsec != 0) { - deadline->period.sec = - static_cast(qos_policies->deadline.sec); - deadline->period.nanosec = - static_cast(qos_policies->deadline.nsec); + if (!rmw_time_equal(qos_policies->deadline, RMW_DURATION_UNSPECIFIED)) { + deadline->period = rmw_time_to_dds_duration(qos_policies->deadline); } - if (qos_policies->liveliness_lease_duration.sec != 0 || - qos_policies->liveliness_lease_duration.nsec != 0) - { - liveliness->lease_duration.sec = - static_cast(qos_policies->liveliness_lease_duration.sec); - liveliness->lease_duration.nanosec = - static_cast( - qos_policies->liveliness_lease_duration.nsec); + if (!rmw_time_equal(qos_policies->liveliness_lease_duration, RMW_DURATION_UNSPECIFIED)) { + liveliness->lease_duration = + rmw_time_to_dds_duration(qos_policies->liveliness_lease_duration); } switch (qos_policies->liveliness) { @@ -401,33 +423,17 @@ rmw_connextdds_get_readerwriter_qos( } } - if (nullptr != lifespan && - (qos_policies->lifespan.sec != 0 || qos_policies->lifespan.nsec != 0)) - { + if (!rmw_time_equal(qos_policies->lifespan, RMW_DURATION_UNSPECIFIED)) { #if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO - lifespan->duration.sec = static_cast(qos_policies->lifespan.sec); - lifespan->duration.nanosec = - static_cast(qos_policies->lifespan.nsec); + assert(nullptr != lifespan); + lifespan->duration = rmw_time_to_dds_duration(qos_policies->lifespan); #else + // Print a warning when lifespan is not supported but was customized by user. + assert(nullptr == lifespan); RMW_CONNEXT_LOG_WARNING("lifespan qos policy not supported") #endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */ } - // Make sure that resource limits are consistent with history qos - // TODO(asorbini): do not overwrite if using non-default QoS - // if (history->kind == DDS_KEEP_LAST_HISTORY_QOS && - // history->depth > 1 && - // resource_limits->max_samples_per_instance == DDS_LENGTH_UNLIMITED) - // { - // resource_limits->max_samples_per_instance = history->depth; - // if (resource_limits->max_samples != DDS_LENGTH_UNLIMITED && - // resource_limits->max_samples < resource_limits->max_samples_per_instance) - // { - // resource_limits->max_samples = - // resource_limits->max_samples_per_instance; - // } - // } - return RMW_RET_OK; } @@ -501,13 +507,10 @@ rmw_connextdds_readerwriter_qos_to_ros( } } - qos_policies->deadline.sec = deadline->period.sec; - qos_policies->deadline.nsec = deadline->period.nanosec; + qos_policies->deadline = dds_duration_to_rmw_time(deadline->period); - qos_policies->liveliness_lease_duration.sec = - liveliness->lease_duration.sec; - qos_policies->liveliness_lease_duration.nsec = - liveliness->lease_duration.nanosec; + qos_policies->liveliness_lease_duration = + dds_duration_to_rmw_time(liveliness->lease_duration); switch (liveliness->kind) { case DDS_AUTOMATIC_LIVELINESS_QOS: @@ -530,10 +533,11 @@ rmw_connextdds_readerwriter_qos_to_ros( if (nullptr != lifespan) { #if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO - qos_policies->lifespan.sec = lifespan->duration.sec; - qos_policies->lifespan.nsec = lifespan->duration.nanosec; + qos_policies->lifespan = dds_duration_to_rmw_time(lifespan->duration); #else - RMW_CONNEXT_LOG_WARNING("lifespan qos policy not supported") + // Only emit a debug message in this case, since we don't want to pollute the + // output too much. We print a warning when going from ROS to DDS. + RMW_CONNEXT_LOG_DEBUG("lifespan qos policy not supported") #endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */ } From 47224b249a48eb3af093cb3f6ad4684bf1db2d86 Mon Sep 17 00:00:00 2001 From: Andrea Sorbini Date: Fri, 23 Apr 2021 16:30:24 -0700 Subject: [PATCH 2/3] Account for no lifespan when converting reader qos Signed-off-by: Andrea Sorbini --- rmw_connextdds_common/src/common/rmw_impl.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/rmw_connextdds_common/src/common/rmw_impl.cpp b/rmw_connextdds_common/src/common/rmw_impl.cpp index 05733937..0a2c6cd8 100644 --- a/rmw_connextdds_common/src/common/rmw_impl.cpp +++ b/rmw_connextdds_common/src/common/rmw_impl.cpp @@ -423,14 +423,18 @@ rmw_connextdds_get_readerwriter_qos( } } - if (!rmw_time_equal(qos_policies->lifespan, RMW_DURATION_UNSPECIFIED)) { + // LifespanQosPolicy is a writer-only policy, so `lifespan` might be NULL. + // Micro does not support this policy, so the value will always be NULL. +#if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_MICRO + assert(nullptr == lifespan); +#endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_MICRO */ + if (lifespan != nullptr && + !rmw_time_equal(qos_policies->lifespan, RMW_DURATION_UNSPECIFIED)) + { + // Guard access to type since it's not defined by Micro (only forward declared + // by rmw_connextdds/dds_api_rtime.hpp) #if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO - assert(nullptr != lifespan); lifespan->duration = rmw_time_to_dds_duration(qos_policies->lifespan); -#else - // Print a warning when lifespan is not supported but was customized by user. - assert(nullptr == lifespan); - RMW_CONNEXT_LOG_WARNING("lifespan qos policy not supported") #endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */ } From 1323a0195d78de24f52663ce133bb482fa9c93e0 Mon Sep 17 00:00:00 2001 From: Andrea Sorbini Date: Fri, 23 Apr 2021 16:40:16 -0700 Subject: [PATCH 3/3] Adjust guard for LifespanQosPolicy Signed-off-by: Andrea Sorbini --- rmw_connextdds_common/src/common/rmw_impl.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rmw_connextdds_common/src/common/rmw_impl.cpp b/rmw_connextdds_common/src/common/rmw_impl.cpp index 0a2c6cd8..2de25d16 100644 --- a/rmw_connextdds_common/src/common/rmw_impl.cpp +++ b/rmw_connextdds_common/src/common/rmw_impl.cpp @@ -427,16 +427,15 @@ rmw_connextdds_get_readerwriter_qos( // Micro does not support this policy, so the value will always be NULL. #if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_MICRO assert(nullptr == lifespan); -#endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_MICRO */ +#else /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */ if (lifespan != nullptr && !rmw_time_equal(qos_policies->lifespan, RMW_DURATION_UNSPECIFIED)) { // Guard access to type since it's not defined by Micro (only forward declared // by rmw_connextdds/dds_api_rtime.hpp) -#if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO lifespan->duration = rmw_time_to_dds_duration(qos_policies->lifespan); -#endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */ } +#endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */ return RMW_RET_OK; }