From 31ae9c6a600f77d1566e58fb7fe796be08ab7898 Mon Sep 17 00:00:00 2001 From: Ada-King <64196404+Ada-King@users.noreply.github.com> Date: Fri, 25 Sep 2020 01:27:30 +0800 Subject: [PATCH] Expose qos setting for /rosout (#1247) * Expose qos setting for /rosout Signed-off-by: Ada-King * For re-trigger CI job Signed-off-by: Ada-King * Modify code based on comments Signed-off-by: Ada-King * Remove redundant parameter Signed-off-by: Ada-King * Simplify Test Signed-off-by: Ada-King * Modify Test as suggested Signed-off-by: Ada-King --- rclcpp/include/rclcpp/node_options.hpp | 15 +++++ rclcpp/include/rclcpp/qos.hpp | 23 ++++++++ rclcpp/src/rclcpp/node_options.cpp | 16 ++++++ rclcpp/src/rclcpp/qos.cpp | 4 ++ rclcpp/test/CMakeLists.txt | 6 ++ rclcpp/test/rclcpp/test_rosout_qos.cpp | 79 ++++++++++++++++++++++++++ 6 files changed, 143 insertions(+) create mode 100644 rclcpp/test/rclcpp/test_rosout_qos.cpp diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 4be29968f7..e476a4f55b 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -256,6 +256,19 @@ class NodeOptions NodeOptions & parameter_event_qos(const rclcpp::QoS & parameter_event_qos); + /// Return a reference to the rosout_qos QoS. + RCLCPP_PUBLIC + const rclcpp::QoS & + rosout_qos() const; + + /// Set the rosout_qos QoS. + /** + * The QoS settings to be used for the publisher on /rosout topic, if enabled. + */ + RCLCPP_PUBLIC + NodeOptions & + rosout_qos(const rclcpp::QoS & rosout_qos); + /// Return a reference to the parameter_event_publisher_options. RCLCPP_PUBLIC const rclcpp::PublisherOptionsBase & @@ -358,6 +371,8 @@ class NodeOptions rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events) ); + rclcpp::QoS rosout_qos_ = rclcpp::RosoutQoS(); + rclcpp::PublisherOptionsBase parameter_event_publisher_options_ = rclcpp::PublisherOptionsBase(); bool allow_undeclared_parameters_ {false}; diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index ac016efec5..11b80b4eeb 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -19,6 +19,7 @@ #include "rclcpp/duration.hpp" #include "rclcpp/visibility_control.hpp" +#include "rcl/logging_rosout.h" #include "rmw/incompatible_qos_events_statuses.h" #include "rmw/qos_profiles.h" #include "rmw/types.h" @@ -249,6 +250,28 @@ class RCLCPP_PUBLIC ParameterEventsQoS : public QoS )); }; +/** + * Rosout QoS class + * - History: Keep last, + * - Depth: 1000, + * - Reliability: Reliable, + * - Durability: TRANSIENT_LOCAL, + * - Deadline: Default, + * - Lifespan: {10, 0}, + * - Liveliness: System default, + * - Liveliness lease duration: default, + * - Avoid ros namespace conventions: false + */ +class RCLCPP_PUBLIC RosoutQoS : public QoS +{ +public: + explicit + RosoutQoS( + const QoSInitialization & rosout_qos_initialization = ( + QoSInitialization::from_rmw(rcl_qos_profile_rosout_default) + )); +}; + /** * System defaults QoS class * - History: System default, diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 179c5bffe7..017bc35c3a 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -75,6 +75,7 @@ NodeOptions::operator=(const NodeOptions & other) this->use_intra_process_comms_ = other.use_intra_process_comms_; this->enable_topic_statistics_ = other.enable_topic_statistics_; this->start_parameter_services_ = other.start_parameter_services_; + this->rosout_qos_ = other.rosout_qos_; this->allocator_ = other.allocator_; this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_; this->automatically_declare_parameters_from_overrides_ = @@ -94,6 +95,7 @@ NodeOptions::get_rcl_node_options() const node_options_->allocator = this->allocator_; node_options_->use_global_arguments = this->use_global_arguments_; node_options_->enable_rosout = this->enable_rosout_; + node_options_->rosout_qos = this->rosout_qos_.get_rmw_qos_profile(); int c_argc = 0; std::unique_ptr c_argv; @@ -266,6 +268,20 @@ NodeOptions::parameter_event_qos(const rclcpp::QoS & parameter_event_qos) return *this; } +const rclcpp::QoS & +NodeOptions::rosout_qos() const +{ + return this->rosout_qos_; +} + +NodeOptions & +NodeOptions::rosout_qos(const rclcpp::QoS & rosout_qos) +{ + this->node_options_.reset(); + this->rosout_qos_ = rosout_qos; + return *this; +} + const rclcpp::PublisherOptionsBase & NodeOptions::parameter_event_publisher_options() const { diff --git a/rclcpp/src/rclcpp/qos.cpp b/rclcpp/src/rclcpp/qos.cpp index b0b3987602..9c1565adfa 100644 --- a/rclcpp/src/rclcpp/qos.cpp +++ b/rclcpp/src/rclcpp/qos.cpp @@ -251,6 +251,10 @@ ParameterEventsQoS::ParameterEventsQoS(const QoSInitialization & qos_initializat : QoS(qos_initialization, rmw_qos_profile_parameter_events) {} +RosoutQoS::RosoutQoS(const QoSInitialization & rosout_initialization) +: QoS(rosout_initialization, rcl_qos_profile_rosout_default) +{} + SystemDefaultsQoS::SystemDefaultsQoS(const QoSInitialization & qos_initialization) : QoS(qos_initialization, rmw_qos_profile_system_default) {} diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 6d5386969a..3c643c06ff 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -627,6 +627,12 @@ if(TARGET test_rclcpp_gtest_macros) target_link_libraries(test_rclcpp_gtest_macros ${PROJECT_NAME}) endif() +ament_add_gtest(test_rosout_qos rclcpp/test_rosout_qos.cpp) +if(TARGET test_rosout_qos) + ament_target_dependencies(test_rosout_qos "rcl") + target_link_libraries(test_rosout_qos ${PROJECT_NAME}) +endif() + # Install test resources install( DIRECTORY resources diff --git a/rclcpp/test/rclcpp/test_rosout_qos.cpp b/rclcpp/test/rclcpp/test_rosout_qos.cpp new file mode 100644 index 0000000000..aacaf84189 --- /dev/null +++ b/rclcpp/test/rclcpp/test_rosout_qos.cpp @@ -0,0 +1,79 @@ +// Copyright 2020 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 +#include + +#include "rclcpp/node_options.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp/duration.hpp" +#include "rcl/logging_rosout.h" +#include "rcl/init.h" +#include "rmw/types.h" + +bool operator==( + const rmw_time_t & lhs, + const rmw_time_t & rhs) +{ + return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec; +} + +bool operator==( + const rmw_qos_profile_t & lhs, + const rmw_qos_profile_t & rhs) +{ + return lhs.history == rhs.history && + lhs.depth == rhs.depth && + lhs.reliability == rhs.reliability && + lhs.durability == rhs.durability && + lhs.deadline == rhs.deadline && + lhs.lifespan == rhs.lifespan && + lhs.liveliness == rhs.liveliness && + lhs.liveliness_lease_duration == rhs.liveliness_lease_duration && + lhs.avoid_ros_namespace_conventions == rhs.avoid_ros_namespace_conventions; +} + +bool operator!=( + const rmw_qos_profile_t & lhs, + const rmw_qos_profile_t & rhs) +{ + return !(lhs == rhs); +} + +/* + Test rosout_qos function with default value. + */ +TEST(TestRosoutQoS, test_rosout_qos_with_default_value) { + rclcpp::NodeOptions node_options; + rclcpp::QoS rosout_qos_profile = node_options.rosout_qos(); + rmw_qos_profile_t rmw_qos_profile = rosout_qos_profile.get_rmw_qos_profile(); + EXPECT_EQ(rcl_qos_profile_rosout_default, rmw_qos_profile); + EXPECT_EQ(rcl_qos_profile_rosout_default, node_options.get_rcl_node_options()->rosout_qos); +} + +/* + Test `rosout_qos` function with custom value. + */ +TEST(TestRosoutQoS, test_rosout_qos_with_custom_value) { + rmw_time_t life_span; + life_span.sec = 10; + life_span.nsec = 0; + auto qos_profile = rclcpp::QoS(rclcpp::KeepLast(1000)).transient_local().lifespan(life_span); + auto options = rclcpp::NodeOptions().rosout_qos(qos_profile); + rclcpp::QoS rosout_qos = options.rosout_qos(); + rmw_qos_profile_t rmw_qos_profile = rosout_qos.get_rmw_qos_profile(); + + EXPECT_EQ(rmw_qos_profile, qos_profile.get_rmw_qos_profile()); + EXPECT_EQ(rmw_qos_profile, options.get_rcl_node_options()->rosout_qos); +}