diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index e5d7c5e130..c6b790fc7a 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -137,6 +137,9 @@ class RCLCPP_PUBLIC Duration static Duration from_nanoseconds(rcl_duration_value_t nanoseconds); + static Duration + from_rmw_time(rmw_time_t duration); + /// Convert Duration into a std::chrono::Duration. template DurationT diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index c901b17cd8..e5617e7a05 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -30,6 +30,38 @@ namespace rclcpp RCLCPP_PUBLIC std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind); +enum class HistoryPolicy +{ + KeepLast = RMW_QOS_POLICY_HISTORY_KEEP_LAST, + KeepAll = RMW_QOS_POLICY_HISTORY_KEEP_ALL, + SystemDefault = RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, + Unknown = RMW_QOS_POLICY_HISTORY_UNKNOWN, +}; + +enum class ReliabilityPolicy +{ + BestEffort = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + Reliable = RMW_QOS_POLICY_RELIABILITY_RELIABLE, + SystemDefault = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT, + Unknown = RMW_QOS_POLICY_RELIABILITY_UNKNOWN, +}; + +enum class DurabilityPolicy +{ + Volatile = RMW_QOS_POLICY_DURABILITY_VOLATILE, + TransientLocal = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + SystemDefault = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, + Unknown = RMW_QOS_POLICY_DURABILITY_UNKNOWN, +}; + +enum class LivelinessPolicy +{ + Automatic = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, + ManualByTopic = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, + SystemDefault = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + Unknown = RMW_QOS_POLICY_LIVELINESS_UNKNOWN, +}; + /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead. struct RCLCPP_PUBLIC QoSInitialization { @@ -82,6 +114,10 @@ class RCLCPP_PUBLIC QoS const rmw_qos_profile_t & get_rmw_qos_profile() const; + /// Set the history policy. + QoS & + history(HistoryPolicy history); + /// Set the history policy. QoS & history(rmw_qos_history_policy_t history); @@ -98,6 +134,10 @@ class RCLCPP_PUBLIC QoS QoS & reliability(rmw_qos_reliability_policy_t reliability); + /// Set the reliability setting. + QoS & + reliability(ReliabilityPolicy reliability); + /// Set the reliability setting to reliable. QoS & reliable(); @@ -110,6 +150,10 @@ class RCLCPP_PUBLIC QoS QoS & durability(rmw_qos_durability_policy_t durability); + /// Set the durability setting. + QoS & + durability(DurabilityPolicy durability); + /// Set the durability setting to volatile. /** * Note that this cannot be named `volatile` because it is a C++ keyword. @@ -141,6 +185,10 @@ class RCLCPP_PUBLIC QoS QoS & liveliness(rmw_qos_liveliness_policy_t liveliness); + /// Set the liveliness setting. + QoS & + liveliness(LivelinessPolicy liveliness); + /// Set the liveliness_lease_duration setting. QoS & liveliness_lease_duration(rmw_time_t liveliness_lease_duration); @@ -153,6 +201,42 @@ class RCLCPP_PUBLIC QoS QoS & avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions); + /// Get the history qos policy. + HistoryPolicy + history() const; + + /// Get the history depth. + size_t + depth() const; + + /// Get the reliability policy. + ReliabilityPolicy + reliability() const; + + /// Get the durability policy. + DurabilityPolicy + durability() const; + + /// Get the deadline duration setting. + rclcpp::Duration + deadline() const; + + /// Get the lifespan duration setting. + rclcpp::Duration + lifespan() const; + + /// Get the liveliness policy. + LivelinessPolicy + liveliness() const; + + /// Get the liveliness lease duration setting. + rclcpp::Duration + liveliness_lease_duration() const; + + /// Get the `avoid ros namespace convention` setting. + bool + avoid_ros_namespace_conventions() const; + private: rmw_qos_profile_t rmw_qos_profile_; }; diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index 40ce9daaa9..317d6b0e90 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -246,6 +246,27 @@ Duration::to_rmw_time() const return result; } +Duration +Duration::from_rmw_time(rmw_time_t duration) +{ + Duration ret; + constexpr rcl_duration_value_t limit_ns = std::numeric_limits::max(); + constexpr rcl_duration_value_t limit_sec = RCL_NS_TO_S(limit_ns); + if (duration.sec > limit_sec || duration.nsec > limit_ns) { + // saturate if will overflow + ret.rcl_duration_.nanoseconds = limit_ns; + return ret; + } + uint64_t total_ns = RCL_S_TO_NS(duration.sec) + duration.nsec; + if (total_ns > limit_ns) { + // saturate if will overflow + ret.rcl_duration_.nanoseconds = limit_ns; + return ret; + } + ret.rcl_duration_.nanoseconds = static_cast(total_ns); + return ret; +} + Duration Duration::from_seconds(double seconds) { diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp index 8cbb72c648..d0f47cd01e 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -99,6 +99,13 @@ QoS::history(rmw_qos_history_policy_t history) return *this; } +QoS & +QoS::history(HistoryPolicy history) +{ + rmw_qos_profile_.history = static_cast(history); + return *this; +} + QoS & QoS::keep_last(size_t depth) { @@ -122,6 +129,13 @@ QoS::reliability(rmw_qos_reliability_policy_t reliability) return *this; } +QoS & +QoS::reliability(ReliabilityPolicy reliability) +{ + rmw_qos_profile_.reliability = static_cast(reliability); + return *this; +} + QoS & QoS::reliable() { @@ -141,6 +155,13 @@ QoS::durability(rmw_qos_durability_policy_t durability) return *this; } +QoS & +QoS::durability(DurabilityPolicy durability) +{ + rmw_qos_profile_.durability = static_cast(durability); + return *this; +} + QoS & QoS::durability_volatile() { @@ -186,6 +207,14 @@ QoS::liveliness(rmw_qos_liveliness_policy_t liveliness) return *this; } +QoS & +QoS::liveliness(LivelinessPolicy liveliness) +{ + rmw_qos_profile_.liveliness = static_cast(liveliness); + return *this; +} + + QoS & QoS::liveliness_lease_duration(rmw_time_t liveliness_lease_duration) { @@ -206,6 +235,51 @@ QoS::avoid_ros_namespace_conventions(bool avoid_ros_namespace_conventions) return *this; } +HistoryPolicy +QoS::history() const +{ + return static_cast(rmw_qos_profile_.history); +} + +size_t +QoS::depth() const {return rmw_qos_profile_.depth;} + +ReliabilityPolicy +QoS::reliability() const +{ + return static_cast(rmw_qos_profile_.reliability); +} + +DurabilityPolicy +QoS::durability() const +{ + return static_cast(rmw_qos_profile_.durability); +} + +Duration +QoS::deadline() const {return Duration::from_rmw_time(rmw_qos_profile_.deadline);} + +Duration +QoS::lifespan() const {return Duration::from_rmw_time(rmw_qos_profile_.lifespan);} + +LivelinessPolicy +QoS::liveliness() const +{ + return static_cast(rmw_qos_profile_.liveliness); +} + +Duration +QoS::liveliness_lease_duration() const +{ + return Duration::from_rmw_time(rmw_qos_profile_.liveliness_lease_duration); +} + +bool +QoS::avoid_ros_namespace_conventions() const +{ + return rmw_qos_profile_.avoid_ros_namespace_conventions; +} + namespace { /// Check if two rmw_time_t have the same values. diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp index f3cbdffa3c..0d349d0d12 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -148,6 +148,34 @@ TEST_F(TestDuration, from_seconds) { rclcpp::Duration::from_seconds(-1.5)); } +TEST_F(TestDuration, from_rmw_time) { + constexpr auto max_rcl_duration = std::numeric_limits::max(); + { + rmw_time_t rmw_duration{}; + rmw_duration.sec = RCL_NS_TO_S(max_rcl_duration) + 1uLL; + EXPECT_EQ(rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), max_rcl_duration); + } + { + rmw_time_t rmw_duration{}; + rmw_duration.nsec = max_rcl_duration + 1uLL; + EXPECT_EQ(rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), max_rcl_duration); + } + { + rmw_time_t rmw_duration{}; + rmw_duration.nsec = max_rcl_duration; + rmw_duration.sec = RCL_NS_TO_S(max_rcl_duration); + EXPECT_EQ(rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), max_rcl_duration); + } + { + rmw_time_t rmw_duration{}; + rmw_duration.sec = 1u; + rmw_duration.nsec = 1000u; + EXPECT_EQ( + rclcpp::Duration::from_rmw_time(rmw_duration).nanoseconds(), + static_cast(RCL_S_TO_NS(rmw_duration.sec) + rmw_duration.nsec)); + } +} + TEST_F(TestDuration, std_chrono_constructors) { EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration(0.0s)); EXPECT_EQ(rclcpp::Duration(0ns), rclcpp::Duration(0s)); diff --git a/rclcpp/test/rclcpp/test_qos.cpp b/rclcpp/test/rclcpp/test_qos.cpp index 41114f22f9..6786750ed4 100644 --- a/rclcpp/test/rclcpp/test_qos.cpp +++ b/rclcpp/test/rclcpp/test_qos.cpp @@ -78,33 +78,41 @@ TEST(TestQoS, equality_namespace) { EXPECT_NE(a, b); } -TEST(TestQoS, setters) { +TEST(TestQoS, setters_and_getters) { rclcpp::QoS qos(10); qos.keep_all(); - EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_ALL, qos.get_rmw_qos_profile().history); + EXPECT_EQ(rclcpp::HistoryPolicy::KeepAll, qos.history()); qos.keep_last(20); - EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_LAST, qos.get_rmw_qos_profile().history); - EXPECT_EQ(20u, qos.get_rmw_qos_profile().depth); + EXPECT_EQ(rclcpp::HistoryPolicy::KeepLast, qos.history()); + EXPECT_EQ(20u, qos.depth()); qos.reliable(); - EXPECT_EQ(RMW_QOS_POLICY_RELIABILITY_RELIABLE, qos.get_rmw_qos_profile().reliability); + EXPECT_EQ(rclcpp::ReliabilityPolicy::Reliable, qos.reliability()); + + qos.reliability(rclcpp::ReliabilityPolicy::BestEffort); + EXPECT_EQ(rclcpp::ReliabilityPolicy::BestEffort, qos.reliability()); qos.durability_volatile(); - EXPECT_EQ(RMW_QOS_POLICY_DURABILITY_VOLATILE, qos.get_rmw_qos_profile().durability); + EXPECT_EQ(rclcpp::DurabilityPolicy::Volatile, qos.durability()); qos.transient_local(); - EXPECT_EQ(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, qos.get_rmw_qos_profile().durability); + EXPECT_EQ(rclcpp::DurabilityPolicy::TransientLocal, qos.durability()); + + qos.durability(rclcpp::DurabilityPolicy::Volatile); + EXPECT_EQ(rclcpp::DurabilityPolicy::Volatile, qos.durability()); qos.history(RMW_QOS_POLICY_HISTORY_KEEP_ALL); - EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_ALL, qos.get_rmw_qos_profile().history); + EXPECT_EQ(rclcpp::HistoryPolicy::KeepAll, qos.history()); - constexpr size_t duration_ns = 12345; + qos.history(rclcpp::HistoryPolicy::KeepLast); + EXPECT_EQ(rclcpp::HistoryPolicy::KeepLast, qos.history()); + + constexpr rcl_duration_value_t duration_ns = 12345; constexpr std::chrono::nanoseconds duration(duration_ns); qos.deadline(duration); - EXPECT_EQ(0u, qos.get_rmw_qos_profile().deadline.sec); - EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().deadline.nsec); + EXPECT_EQ(duration_ns, qos.deadline().nanoseconds()); const rmw_time_t rmw_time {0, 54321}; qos.deadline(rmw_time); @@ -112,28 +120,29 @@ TEST(TestQoS, setters) { EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().deadline.nsec); qos.lifespan(duration); - EXPECT_EQ(0u, qos.get_rmw_qos_profile().lifespan.sec); - EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().lifespan.nsec); + EXPECT_EQ(duration_ns, qos.lifespan().nanoseconds()); qos.lifespan(rmw_time); EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().lifespan.sec); EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().lifespan.nsec); qos.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); - EXPECT_EQ(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, qos.get_rmw_qos_profile().liveliness); + EXPECT_EQ(rclcpp::LivelinessPolicy::ManualByTopic, qos.liveliness()); + + qos.liveliness(rclcpp::LivelinessPolicy::Automatic); + EXPECT_EQ(rclcpp::LivelinessPolicy::Automatic, qos.liveliness()); qos.liveliness_lease_duration(duration); - EXPECT_EQ(0u, qos.get_rmw_qos_profile().liveliness_lease_duration.sec); - EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().liveliness_lease_duration.nsec); + EXPECT_EQ(duration_ns, qos.liveliness_lease_duration().nanoseconds()); qos.liveliness_lease_duration(rmw_time); EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().liveliness_lease_duration.sec); EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().liveliness_lease_duration.nsec); qos.avoid_ros_namespace_conventions(true); - EXPECT_TRUE(qos.get_rmw_qos_profile().avoid_ros_namespace_conventions); + EXPECT_TRUE(qos.avoid_ros_namespace_conventions()); qos.avoid_ros_namespace_conventions(false); - EXPECT_FALSE(qos.get_rmw_qos_profile().avoid_ros_namespace_conventions); + EXPECT_FALSE(qos.avoid_ros_namespace_conventions()); } bool operator==(const rmw_qos_profile_t & lhs, const rmw_qos_profile_t & rhs)