Skip to content

Commit

Permalink
Revert "Improved conversion of time values between ROS and DDS formats (
Browse files Browse the repository at this point in the history
ros2#43)"

This reverts commit 46892f5.

Signed-off-by: Emerson Knapp <[email protected]>
  • Loading branch information
Emerson Knapp committed Jul 20, 2021
1 parent 938d7d1 commit 4d6d5e2
Showing 1 changed file with 46 additions and 51 deletions.
97 changes: 46 additions & 51 deletions rmw_connextdds_common/src/common/rmw_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -262,34 +262,6 @@ 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 @@ -406,13 +378,21 @@ rmw_connextdds_get_readerwriter_qos(
}
}

if (!rmw_time_equal(qos_policies->deadline, RMW_DURATION_UNSPECIFIED)) {
deadline->period = rmw_time_to_dds_duration(qos_policies->deadline);
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->liveliness_lease_duration, RMW_DURATION_UNSPECIFIED)) {
liveliness->lease_duration =
rmw_time_to_dds_duration(qos_policies->liveliness_lease_duration);
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);
}

switch (qos_policies->liveliness) {
Expand All @@ -439,19 +419,32 @@ rmw_connextdds_get_readerwriter_qos(
}
}

// 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 (nullptr != lifespan &&
(qos_policies->lifespan.sec != 0 || qos_policies->lifespan.nsec != 0))
{
// 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);
}
#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 */
}

// 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;
}
Expand Down Expand Up @@ -526,10 +519,13 @@ rmw_connextdds_readerwriter_qos_to_ros(
}
}

qos_policies->deadline = dds_duration_to_rmw_time(deadline->period);
qos_policies->deadline.sec = deadline->period.sec;
qos_policies->deadline.nsec = deadline->period.nanosec;

qos_policies->liveliness_lease_duration =
dds_duration_to_rmw_time(liveliness->lease_duration);
qos_policies->liveliness_lease_duration.sec =
liveliness->lease_duration.sec;
qos_policies->liveliness_lease_duration.nsec =
liveliness->lease_duration.nanosec;

switch (liveliness->kind) {
case DDS_AUTOMATIC_LIVELINESS_QOS:
Expand All @@ -552,11 +548,10 @@ rmw_connextdds_readerwriter_qos_to_ros(

if (nullptr != lifespan) {
#if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO
qos_policies->lifespan = dds_duration_to_rmw_time(lifespan->duration);
qos_policies->lifespan.sec = lifespan->duration.sec;
qos_policies->lifespan.nsec = lifespan->duration.nanosec;
#else
// 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")
RMW_CONNEXT_LOG_WARNING("lifespan qos policy not supported")
#endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */
}

Expand Down

0 comments on commit 4d6d5e2

Please sign in to comment.