@@ -32,8 +32,10 @@ SOFTWARE.
3232#include < memory>
3333#include < string>
3434
35- #include < mqtt/async_client.h>
35+ #define FMT_HEADER_ONLY
36+ #include < fmt/format.h>
3637#include < mqtt_client_interfaces/srv/is_connected.hpp>
38+ #include < mqtt/async_client.h>
3739#include < rclcpp/rclcpp.hpp>
3840#include < rclcpp/serialization.hpp>
3941#include < std_msgs/msg/float64.hpp>
@@ -125,6 +127,36 @@ class MqttClient : public rclcpp::Node,
125127 template <typename T>
126128 bool loadParameter (const std::string& key, T& value, const T& default_value);
127129
130+ /* *
131+ * @brief Loads requested ROS parameter from parameter server.
132+ *
133+ * @tparam T type (one of int, double, bool)
134+ *
135+ * @param[in] key parameter name
136+ * @param[out] value variable where to store the retrieved parameter
137+ *
138+ * @return true if parameter was successfully retrieved
139+ * @return false if parameter was not found
140+ */
141+ template <typename T>
142+ bool loadParameter (const std::string& key, std::vector<T>& value);
143+
144+ /* *
145+ * @brief Loads requested ROS parameter from parameter server, allows default
146+ * value.
147+ *
148+ * @tparam T type (one of int, double, bool)
149+ *
150+ * @param[in] key parameter name
151+ * @param[out] value variable where to store the retrieved parameter
152+ * @param[in] default_value default value
153+ *
154+ * @return true if parameter was successfully retrieved
155+ * @return false if parameter was not found or default was used
156+ */
157+ template <typename T>
158+ bool loadParameter (const std::string& key, std::vector<T>& value, const std::vector<T>& default_value);
159+
128160 /* *
129161 * @brief Converts a string to a path object resolving paths relative to
130162 * ROS_HOME.
@@ -321,9 +353,13 @@ class MqttClient : public rclcpp::Node,
321353 double keep_alive_interval; // /< keep-alive interval
322354 int max_inflight; // /< maximum number of inflight messages
323355 struct {
324- std::filesystem::path certificate; // /< client certificate
325- std::filesystem::path key; // /< client private keyfile
326- std::string password; // /< decryption password for private key
356+ std::filesystem::path certificate; // /< client certificate
357+ std::filesystem::path key; // /< client private keyfile
358+ std::string password; // /< decryption password for private key
359+ int version; // /< TLS version (https://github.com/eclipse/paho.mqtt.cpp/blob/master/src/mqtt/ssl_options.h#L305)
360+ bool verify; // /< Verify the client should conduct
361+ // /< post-connect checks
362+ std::vector<std::string> alpn_protos; // /< list of ALPN protocols
327363 } tls; // /< SSL/TLS-related variables
328364 };
329365
@@ -460,6 +496,32 @@ bool MqttClient::loadParameter(const std::string& key, T& value,
460496}
461497
462498
499+ template <typename T>
500+ bool MqttClient::loadParameter (const std::string& key, std::vector<T>& value)
501+ {
502+ const bool found = get_parameter (key, value);
503+ if (found)
504+ RCLCPP_WARN (get_logger (), " Retrieved parameter '%s' = '[%s]'" , key.c_str (),
505+ fmt::format (" {}" , fmt::join (value, " , " )).c_str ());
506+ return found;
507+ }
508+
509+
510+ template <typename T>
511+ bool MqttClient::loadParameter (const std::string& key, std::vector<T>& value,
512+ const std::vector<T>& default_value)
513+ {
514+ const bool found = get_parameter_or (key, value, default_value);
515+ if (!found)
516+ RCLCPP_WARN (get_logger (), " Parameter '%s' not set, defaulting to '%s'" ,
517+ key.c_str (), fmt::format (" {}" , fmt::join (value, " , " )).c_str ());
518+ if (found)
519+ RCLCPP_DEBUG (get_logger (), " Retrieved parameter '%s' = '%s'" , key.c_str (),
520+ fmt::format (" {}" , fmt::join (value, " , " )).c_str ());
521+ return found;
522+ }
523+
524+
463525/* *
464526 * Serializes a ROS message.
465527 *
0 commit comments