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.

Sign up for GitHub

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

Jump to bottom

Improved conversion of time values between ROS and DDS formats #43

Merged
merged 3 commits into from
Apr 28, 2021
Merged

Improved conversion of time values between ROS and DDS formats #43

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
58fc68c
Improved conversion of time values between ROS and DDS formats
asorbini Apr 23, 2021
47224b2
Account for no lifespan when converting reader qos
asorbini Apr 23, 2021
1323a01
Adjust guard for LifespanQosPolicy
asorbini Apr 23, 2021
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
99 changes: 53 additions & 46 deletions rmw_connextdds_common/src/common/rmw_impl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@
#include <vector>
#include <stdexcept>

#include "rmw_dds_common/time_utils.hpp"

#include "rmw_connextdds/graph_cache.hpp"

#define ROS_SERVICE_REQUESTER_PREFIX_STR "rq"
Expand Down Expand Up @@ -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<DDS_Long>(ctime.sec);
duration.nanosec = static_cast<DDS_UnsignedLong>(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<uint64_t>(duration.sec), duration.nanosec};
return result;
}

rmw_ret_t
rmw_connextdds_get_readerwriter_qos(
const bool writer_qos,
Expand Down Expand Up @@ -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<DDS_Long>(qos_policies->deadline.sec);
deadline->period.nanosec =
static_cast<DDS_UnsignedLong>(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<DDS_Long>(qos_policies->liveliness_lease_duration.sec);
liveliness->lease_duration.nanosec =
static_cast<DDS_UnsignedLong>(
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) {
Expand All @@ -401,32 +423,19 @@ rmw_connextdds_get_readerwriter_qos(
}
}

if (nullptr != lifespan &&
(qos_policies->lifespan.sec != 0 || qos_policies->lifespan.nsec != 0))
// 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);
#else /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */
if (lifespan != nullptr &&
!rmw_time_equal(qos_policies->lifespan, RMW_DURATION_UNSPECIFIED))
{
#if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO
lifespan->duration.sec = static_cast<DDS_Long>(qos_policies->lifespan.sec);
lifespan->duration.nanosec =
static_cast<DDS_UnsignedLong>(qos_policies->lifespan.nsec);
#else
RMW_CONNEXT_LOG_WARNING("lifespan qos policy not supported")
#endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */
// Guard access to type since it's not defined by Micro (only forward declared
// by rmw_connextdds/dds_api_rtime.hpp)
lifespan->duration = rmw_time_to_dds_duration(qos_policies->lifespan);
}

// 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;
// }
// }
#endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */

return RMW_RET_OK;
}
Expand Down Expand Up @@ -501,13 +510,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:
Expand All @@ -530,10 +536,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 */
}

Expand Down