diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 6b95429c8d..40945bdd09 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -141,6 +141,20 @@ class PublisherBase RCLCPP_PUBLIC void assert_liveliness() {} + + /// 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. /** 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 { 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 3a79b4486d..9fa16fc174 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -15,9 +15,11 @@ #ifndef RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ #define RCLCPP_LIFECYCLE__LIFECYCLE_NODE_IMPL_HPP_ +#include #include #include #include +#include #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/intra_process_manager.hpp" @@ -204,5 +206,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_