From cb20529e5e218cde0106c59b12067a9ca9f43022 Mon Sep 17 00:00:00 2001 From: Vinnam Kim Date: Thu, 28 Mar 2019 13:05:42 +0900 Subject: [PATCH 1/3] Add parameter-related templates to LifecycleNode (#645) * Add parameter-related templates to LifecycleNode Signed-off-by: vinnamkim * Update rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp Co-Authored-By: vinnamkim * Update rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp --- .../rclcpp_lifecycle/lifecycle_node.hpp | 63 ++++++++++++++ .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 87 +++++++++++++++++++ 2 files changed, 150 insertions(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index c93837ab9e..32459d3593 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -255,6 +255,26 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, rcl_interfaces::msg::SetParametersResult set_parameters_atomically(const std::vector & parameters); + template + void + set_parameter_if_not_set( + const std::string & name, + const ParameterT & value); + + /// Set a map of parameters with the same prefix. + /** + * For each key in the map, a parameter with a name of "name.key" will be set + * to the value in the map. + * + * \param[in] name The prefix of the parameters to set. + * \param[in] values The parameters to set in the given prefix. + */ + template + void + set_parameters_if_not_set( + const std::string & name, + const std::map & values); + RCLCPP_LIFECYCLE_PUBLIC std::vector get_parameters(const std::vector & names) const; @@ -273,6 +293,49 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, bool get_parameter(const std::string & name, ParameterT & parameter) const; + /// Assign the value of the map parameter if set into the values argument. + /** + * Parameter names that are part of a map are of the form "name.member". + * This API gets all parameters that begin with "name", storing them into the + * map with the name of the parameter and their value. + * If there are no members in the named map, then the "values" argument is not changed. + * + * \param[in] name The prefix of the parameters to get. + * \param[out] values The map of output values, with one std::string,MapValueT + * per parameter. + * \returns true if values was changed, false otherwise + */ + template + bool + get_parameters( + const std::string & name, + std::map & values) const; + + template + bool + get_parameter_or( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value) const; + + /// Get the parameter value; if not set, set the "alternative value" and store it in the node. + /** + * If the parameter is set, then the "value" argument is assigned the value + * in the parameter. + * If the parameter is not set, then the "value" argument is assigned the "alternative_value", + * and the parameter is set to the "alternative_value" on the node. + * + * \param[in] name The name of the parameter to get. + * \param[out] value The output where the value of the parameter should be assigned. + * \param[in] alternative_value Value to be stored in output and parameter if the parameter was not set. + */ + template + void + get_parameter_or_set( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value); + RCLCPP_LIFECYCLE_PUBLIC std::vector describe_parameters(const std::vector & names) const; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 69d8a48e96..71e638de7b 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +#include #include #include #include @@ -201,5 +202,91 @@ LifecycleNode::register_param_change_callback(CallbackT && callback) this->node_parameters_->register_param_change_callback(std::forward(callback)); } +template +void +LifecycleNode::set_parameter_if_not_set( + const std::string & name, + const ParameterT & value) +{ + rclcpp::Parameter parameter; + if (!this->get_parameter(name, parameter)) { + this->set_parameters({ + rclcpp::Parameter(name, value), + }); + } +} + +// this is a partially-specialized version of set_parameter_if_not_set above, +// where our concrete type for ParameterT is std::map, but the to-be-determined +// type is the value in the map. +template +void +LifecycleNode::set_parameters_if_not_set( + const std::string & name, + const std::map & values) +{ + std::vector params; + + for (const auto & val : values) { + std::string param_name = name + "." + val.first; + rclcpp::Parameter parameter; + if (!this->get_parameter(param_name, parameter)) { + params.push_back(rclcpp::Parameter(param_name, val.second)); + } + } + + this->set_parameters(params); +} + +// this is a partially-specialized version of get_parameter above, +// where our concrete type for ParameterT is std::map, but the to-be-determined +// type is the value in the map. +template +bool +LifecycleNode::get_parameters( + const std::string & name, + std::map & values) const +{ + std::map params; + bool result = node_parameters_->get_parameters_by_prefix(name, params); + if (result) { + for (const auto & param : params) { + values[param.first] = param.second.get_value(); + } + } + + return result; +} + +template +bool +LifecycleNode::get_parameter_or( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value) const +{ + bool got_parameter = get_parameter(name, value); + if (!got_parameter) { + value = alternative_value; + } + return got_parameter; +} + +template +void +LifecycleNode::get_parameter_or_set( + const std::string & name, + ParameterT & value, + const ParameterT & alternative_value) +{ + bool got_parameter = get_parameter(name, value); + if (!got_parameter) { + this->set_parameters({ + rclcpp::Parameter(name, alternative_value), + }); + value = alternative_value; + } +} + } // namespace rclcpp_lifecycle #endif // RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ From 4f2f8def9819a3e37216254a21e3471655529bc0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 28 Mar 2019 11:01:52 -0700 Subject: [PATCH 2/3] fix linter errors in rclcpp_lifecycle (#672) Signed-off-by: Karsten Knese --- .../include/rclcpp_lifecycle/lifecycle_node_impl.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 71e638de7b..941f31495b 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/intra_process_manager.hpp" From 1f2904f980aaaa7ccbfb40d722795b2d9a55a9aa Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Mon, 1 Apr 2019 17:55:08 -0300 Subject: [PATCH 3/3] Add function to get publisher actual qos settings (#667) * Added get_actual_qos method to publisher. Signed-off-by: ivanpauno --- rclcpp/include/rclcpp/publisher.hpp | 14 ++++++++++++++ rclcpp/src/rclcpp/publisher.cpp | 14 +++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 4500fedab5..27a417730c 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -128,6 +128,20 @@ class PublisherBase size_t get_intra_process_subscription_count() const; + /// Get the actual QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the publisher, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * \return The actual qos settings. + */ + RCLCPP_PUBLIC + rmw_qos_profile_t + get_actual_qos() const; + /// Compare this publisher to a gid. /** * Note that this function calls the next function. diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index ce6ebadaee..275e145a52 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -159,7 +159,7 @@ PublisherBase::get_subscription_count() const { size_t inter_process_subscription_count = 0; - rmw_ret_t status = rcl_publisher_get_subscription_count( + rcl_ret_t status = rcl_publisher_get_subscription_count( &publisher_handle_, &inter_process_subscription_count); @@ -197,6 +197,18 @@ PublisherBase::get_intra_process_subscription_count() const return ipm->get_subscription_count(intra_process_publisher_id_); } +rmw_qos_profile_t +PublisherBase::get_actual_qos() const +{ + const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(&publisher_handle_); + if (!qos) { + auto msg = std::string("failed to get qos settings: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + return *qos; +} + bool PublisherBase::operator==(const rmw_gid_t & gid) const {