3 #include <nlohmann/json.hpp>
4 #include <rclcpp/qos.hpp>
5 #include <rclcpp/version.h>
12 constexpr
char PARAM_SEP =
'.';
14 #if RCLCPP_VERSION_MAJOR > 16
15 const rclcpp::ParametersQoS parameterQoS;
17 const rmw_qos_profile_t& parameterQoS = rmw_qos_profile_parameters;
20 static std::pair<std::string, std::string> getNodeAndParamName(
21 const std::string& nodeNameAndParamName) {
22 return {nodeNameAndParamName.substr(0UL, nodeNameAndParamName.find(PARAM_SEP)),
23 nodeNameAndParamName.substr(nodeNameAndParamName.find(PARAM_SEP) + 1UL)};
26 static std::string prependNodeNameToParamName(
const std::string& paramName,
27 const std::string& nodeName) {
28 return nodeName + PARAM_SEP + paramName;
35 const auto paramType = p.
getType();
38 if (paramType == ParameterType::PARAMETER_BOOL) {
39 return rclcpp::Parameter(p.
getName(), value.getValue<
bool>());
40 }
else if (paramType == ParameterType::PARAMETER_INTEGER) {
41 return rclcpp::Parameter(p.
getName(), value.getValue<int64_t>());
42 }
else if (paramType == ParameterType::PARAMETER_DOUBLE) {
43 return rclcpp::Parameter(p.
getName(), value.getValue<
double>());
44 }
else if (paramType == ParameterType::PARAMETER_STRING) {
45 return rclcpp::Parameter(p.
getName(), value.getValue<std::string>());
46 }
else if (paramType == ParameterType::PARAMETER_BYTE_ARRAY) {
47 return rclcpp::Parameter(p.
getName(), value.getValue<std::vector<unsigned char>>());
48 }
else if (paramType == ParameterType::PARAMETER_ARRAY) {
49 const auto paramVec = value.getValue<std::vector<foxglove::ParameterValue>>();
51 const auto elementType = paramVec.front().getType();
52 if (elementType == ParameterType::PARAMETER_BOOL) {
53 std::vector<bool> vec;
54 for (
const auto& paramValue : paramVec) {
55 vec.push_back(paramValue.getValue<
bool>());
57 return rclcpp::Parameter(p.
getName(), vec);
58 }
else if (elementType == ParameterType::PARAMETER_INTEGER) {
59 std::vector<int64_t> vec;
60 for (
const auto& paramValue : paramVec) {
61 vec.push_back(paramValue.getValue<int64_t>());
63 return rclcpp::Parameter(p.
getName(), vec);
64 }
else if (elementType == ParameterType::PARAMETER_DOUBLE) {
65 std::vector<double> vec;
66 for (
const auto& paramValue : paramVec) {
67 vec.push_back(paramValue.getValue<
double>());
69 return rclcpp::Parameter(p.
getName(), vec);
70 }
else if (elementType == ParameterType::PARAMETER_STRING) {
71 std::vector<std::string> vec;
72 for (
const auto& paramValue : paramVec) {
73 vec.push_back(paramValue.getValue<std::string>());
75 return rclcpp::Parameter(p.
getName(), vec);
77 throw std::runtime_error(
"Unsupported parameter type");
78 }
else if (paramType == ParameterType::PARAMETER_NOT_SET) {
79 return rclcpp::Parameter(p.
getName());
81 throw std::runtime_error(
"Unsupported parameter type");
84 return rclcpp::Parameter();
88 const auto type = p.get_type();
90 if (type == rclcpp::ParameterType::PARAMETER_NOT_SET) {
92 }
else if (type == rclcpp::ParameterType::PARAMETER_BOOL) {
94 }
else if (type == rclcpp::ParameterType::PARAMETER_INTEGER) {
96 }
else if (type == rclcpp::ParameterType::PARAMETER_DOUBLE) {
98 }
else if (type == rclcpp::ParameterType::PARAMETER_STRING) {
100 }
else if (type == rclcpp::ParameterType::PARAMETER_BYTE_ARRAY) {
102 }
else if (type == rclcpp::ParameterType::PARAMETER_BOOL_ARRAY) {
103 std::vector<foxglove::ParameterValue> paramVec;
104 for (
const auto value : p.as_bool_array()) {
108 }
else if (type == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) {
109 std::vector<foxglove::ParameterValue> paramVec;
110 for (
const auto value : p.as_integer_array()) {
111 paramVec.push_back(value);
114 }
else if (type == rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY) {
115 std::vector<foxglove::ParameterValue> paramVec;
116 for (
const auto value : p.as_double_array()) {
117 paramVec.push_back(value);
120 }
else if (type == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) {
121 std::vector<foxglove::ParameterValue> paramVec;
122 for (
const auto& value : p.as_string_array()) {
123 paramVec.push_back(value);
127 throw std::runtime_error(
"Unsupported parameter type");
138 std::vector<std::regex> paramWhitelistPatterns)
140 , _paramWhitelistPatterns(paramWhitelistPatterns)
141 , _callbackGroup(node->create_callback_group(rclcpp::CallbackGroupType::Reentrant)) {}
144 const std::chrono::duration<double>& timeout) {
145 std::lock_guard<std::mutex> lock(
_mutex);
147 std::unordered_map<std::string, std::vector<std::string>> paramNamesByNodeName;
148 const auto thisNode =
_node->get_fully_qualified_name();
150 if (!paramNames.empty()) {
153 for (
const auto& fullParamName : paramNames) {
154 const auto& [nodeName, paramName] = getNodeAndParamName(fullParamName);
155 paramNamesByNodeName[nodeName].push_back(paramName);
158 RCLCPP_DEBUG(
_node->get_logger(),
"Getting %zu parameters from %zu nodes...", paramNames.size(),
159 paramNamesByNodeName.size());
163 for (
const auto& fqnNodeName :
_node->get_node_names()) {
164 if (fqnNodeName == thisNode) {
168 const auto serviceNamesAndTypes =
169 _node->get_service_names_and_types_by_node(nodeName, nodeNs);
171 bool listParamsSrvFound =
false, getParamsSrvFound =
false;
172 for (
const auto& [serviceName, serviceTypes] : serviceNamesAndTypes) {
173 constexpr
char GET_PARAMS_SERVICE_TYPE[] =
"rcl_interfaces/srv/GetParameters";
174 constexpr
char LIST_PARAMS_SERVICE_TYPE[] =
"rcl_interfaces/srv/ListParameters";
176 if (!getParamsSrvFound) {
177 getParamsSrvFound = std::find(serviceTypes.begin(), serviceTypes.end(),
178 GET_PARAMS_SERVICE_TYPE) != serviceTypes.end();
180 if (!listParamsSrvFound) {
181 listParamsSrvFound = std::find(serviceTypes.begin(), serviceTypes.end(),
182 LIST_PARAMS_SERVICE_TYPE) != serviceTypes.end();
186 if (listParamsSrvFound && getParamsSrvFound) {
187 paramNamesByNodeName.insert({fqnNodeName, {}});
191 if (!paramNamesByNodeName.empty()) {
192 RCLCPP_DEBUG(
_node->get_logger(),
"Getting all parameters from %zu nodes...",
193 paramNamesByNodeName.size());
197 std::vector<std::future<ParameterList>> getParametersFuture;
198 for (
const auto& [nodeName, nodeParamNames] : paramNamesByNodeName) {
199 if (nodeName == thisNode) {
207 rclcpp::AsyncParametersClient::make_shared(
_node, nodeName, parameterQoS,
_callbackGroup));
208 paramClientIt = insertedPair.first;
211 getParametersFuture.emplace_back(
213 paramClientIt->second, nodeName, nodeParamNames, timeout));
217 for (
auto& future : getParametersFuture) {
219 const auto params = future.get();
220 result.insert(result.begin(), params.begin(), params.end());
221 }
catch (
const std::exception& e) {
222 RCLCPP_ERROR(
_node->get_logger(),
"Exception when getting parameters: %s", e.what());
230 const std::chrono::duration<double>& timeout) {
231 std::lock_guard<std::mutex> lock(
_mutex);
233 rclcpp::ParameterMap paramsByNode;
234 for (
const auto&
param : parameters) {
240 const auto& [nodeName, paramName] = getNodeAndParamName(rosParam.get_name());
241 paramsByNode[nodeName].emplace_back(paramName, rosParam.get_parameter_value());
244 std::vector<std::future<void>> setParametersFuture;
245 for (
const auto& [nodeName, params] : paramsByNode) {
250 rclcpp::AsyncParametersClient::make_shared(
_node, nodeName, parameterQoS,
_callbackGroup));
251 paramClientIt = insertedPair.first;
254 setParametersFuture.emplace_back(std::async(std::launch::async,
256 paramClientIt->second, nodeName, params, timeout));
259 for (
auto& future : setParametersFuture) {
262 }
catch (
const std::exception& e) {
263 RCLCPP_ERROR(
_node->get_logger(),
"Exception when setting parameters: %s", e.what());
269 std::lock_guard<std::mutex> lock(
_mutex);
271 std::unordered_set<std::string> nodesToSubscribe;
272 for (
const auto& paramName : paramNames) {
277 const auto& [nodeName, paramN] = getNodeAndParamName(paramName);
280 auto& subscribedNodeParams = subscribedParamsit->second;
281 subscribedNodeParams.insert(paramN);
283 if (wasNewlyCreated) {
284 nodesToSubscribe.insert(nodeName);
288 for (
const auto& nodeName : nodesToSubscribe) {
293 rclcpp::AsyncParametersClient::make_shared(
_node, nodeName, parameterQoS,
_callbackGroup));
294 paramClientIt = insertedPair.first;
297 auto& paramClient = paramClientIt->second;
300 [
this, nodeName](rcl_interfaces::msg::ParameterEvent::ConstSharedPtr msg) {
301 RCLCPP_DEBUG(
_node->get_logger(),
"Retrieved param update for node %s: %zu params changed",
302 nodeName.c_str(), msg->changed_parameters.size());
306 for (
const auto&
param : msg->changed_parameters) {
307 if (subscribedNodeParams.find(
param.name) != subscribedNodeParams.end()) {
309 rclcpp::Parameter(prependNodeNameToParamName(
param.name, nodeName),
param.value)));
321 std::lock_guard<std::mutex> lock(
_mutex);
323 for (
const auto& paramName : paramNames) {
324 const auto& [nodeName, paramN] = getNodeAndParamName(paramName);
328 subscribedNodeParamsIt->second.erase(subscribedNodeParamsIt->second.find(paramN));
330 if (subscribedNodeParamsIt->second.empty()) {
339 std::lock_guard<std::mutex> lock(
_mutex);
344 const rclcpp::AsyncParametersClient::SharedPtr paramClient,
const std::string& nodeName,
345 const std::vector<std::string>& paramNames,
const std::chrono::duration<double>& timeout) {
346 if (!paramClient->service_is_ready()) {
347 throw std::runtime_error(
"Parameter service for node '" + nodeName +
"' is not ready");
350 auto paramsToRequest = paramNames;
351 if (paramsToRequest.empty()) {
353 auto future = paramClient->list_parameters({}, 0UL);
354 if (std::future_status::ready != future.wait_for(timeout)) {
355 throw std::runtime_error(
"Failed to retrieve parameter names for node '" + nodeName +
"'");
357 paramsToRequest = future.get().names;
361 auto getParamsFuture = paramClient->get_parameters(paramsToRequest);
362 if (std::future_status::ready != getParamsFuture.wait_for(timeout)) {
363 throw std::runtime_error(
"Timed out waiting for " + std::to_string(paramsToRequest.size()) +
364 " parameter(s) from node '" + nodeName +
"'");
366 const auto params = getParamsFuture.get();
369 for (
const auto&
param : params) {
370 const auto fullParamName = prependNodeNameToParamName(
param.get_name(), nodeName);
372 result.push_back(
fromRosParam(rclcpp::Parameter(fullParamName,
param.get_parameter_value())));
379 const std::string& nodeName,
380 const std::vector<rclcpp::Parameter>& params,
381 const std::chrono::duration<double>& timeout) {
382 if (!paramClient->service_is_ready()) {
383 throw std::runtime_error(
"Parameter service for node '" + nodeName +
"' is not ready");
386 auto future = paramClient->set_parameters(params);
388 std::vector<std::string> paramsToDelete;
389 for (
const auto& p : params) {
390 if (p.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) {
391 paramsToDelete.push_back(p.get_name());
395 if (!paramsToDelete.empty()) {
396 auto deleteFuture = paramClient->delete_parameters(paramsToDelete);
397 if (std::future_status::ready != deleteFuture.wait_for(timeout)) {
400 "Param client failed to delete %zu parameter(s) for node '%s' within the given timeout",
401 paramsToDelete.size(), nodeName.c_str());
405 if (std::future_status::ready != future.wait_for(timeout)) {
406 throw std::runtime_error(
"Param client failed to set " + std::to_string(params.size()) +
407 " parameter(s) for node '" + nodeName +
"' within the given timeout");
410 const auto setParamResults = future.get();
411 for (
auto& result : setParamResults) {
412 if (!result.successful) {
413 RCLCPP_WARN(
_node->get_logger(),
"Failed to set one or more parameters for node '%s': %s",
414 nodeName.c_str(), result.reason.c_str());