parameter_interface.cpp
Go to the documentation of this file.
2 
3 #include <nlohmann/json.hpp>
4 #include <rclcpp/qos.hpp>
5 #include <rclcpp/version.h>
6 
9 
10 namespace {
11 
12 constexpr char PARAM_SEP = '.';
13 
14 #if RCLCPP_VERSION_MAJOR > 16
15 const rclcpp::ParametersQoS parameterQoS;
16 #else
17 const rmw_qos_profile_t& parameterQoS = rmw_qos_profile_parameters;
18 #endif
19 
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)};
24 }
25 
26 static std::string prependNodeNameToParamName(const std::string& paramName,
27  const std::string& nodeName) {
28  return nodeName + PARAM_SEP + paramName;
29 }
30 
31 static rclcpp::Parameter toRosParam(const foxglove::Parameter& p) {
32  using foxglove::Parameter;
34 
35  const auto paramType = p.getType();
36  const auto value = p.getValue();
37 
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>>();
50 
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>());
56  }
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>());
62  }
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>());
68  }
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>());
74  }
75  return rclcpp::Parameter(p.getName(), vec);
76  }
77  throw std::runtime_error("Unsupported parameter type");
78  } else if (paramType == ParameterType::PARAMETER_NOT_SET) {
79  return rclcpp::Parameter(p.getName());
80  } else {
81  throw std::runtime_error("Unsupported parameter type");
82  }
83 
84  return rclcpp::Parameter();
85 }
86 
87 static foxglove::Parameter fromRosParam(const rclcpp::Parameter& p) {
88  const auto type = p.get_type();
89 
90  if (type == rclcpp::ParameterType::PARAMETER_NOT_SET) {
91  return foxglove::Parameter(p.get_name(), foxglove::ParameterValue());
92  } else if (type == rclcpp::ParameterType::PARAMETER_BOOL) {
93  return foxglove::Parameter(p.get_name(), p.as_bool());
94  } else if (type == rclcpp::ParameterType::PARAMETER_INTEGER) {
95  return foxglove::Parameter(p.get_name(), p.as_int());
96  } else if (type == rclcpp::ParameterType::PARAMETER_DOUBLE) {
97  return foxglove::Parameter(p.get_name(), p.as_double());
98  } else if (type == rclcpp::ParameterType::PARAMETER_STRING) {
99  return foxglove::Parameter(p.get_name(), p.as_string());
100  } else if (type == rclcpp::ParameterType::PARAMETER_BYTE_ARRAY) {
101  return foxglove::Parameter(p.get_name(), p.as_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()) {
105  paramVec.push_back(foxglove::ParameterValue(value));
106  }
107  return foxglove::Parameter(p.get_name(), paramVec);
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);
112  }
113  return foxglove::Parameter(p.get_name(), paramVec);
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);
118  }
119  return foxglove::Parameter(p.get_name(), paramVec);
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);
124  }
125  return foxglove::Parameter(p.get_name(), paramVec);
126  } else {
127  throw std::runtime_error("Unsupported parameter type");
128  }
129 }
130 
131 } // namespace
132 
133 namespace foxglove_bridge {
134 
136 
138  std::vector<std::regex> paramWhitelistPatterns)
139  : _node(node)
140  , _paramWhitelistPatterns(paramWhitelistPatterns)
141  , _callbackGroup(node->create_callback_group(rclcpp::CallbackGroupType::Reentrant)) {}
142 
143 ParameterList ParameterInterface::getParams(const std::vector<std::string>& paramNames,
144  const std::chrono::duration<double>& timeout) {
145  std::lock_guard<std::mutex> lock(_mutex);
146 
147  std::unordered_map<std::string, std::vector<std::string>> paramNamesByNodeName;
148  const auto thisNode = _node->get_fully_qualified_name();
149 
150  if (!paramNames.empty()) {
151  // Break apart fully qualified {node_name}.{param_name} strings and build a
152  // mape of node names to the list of parameters for each node
153  for (const auto& fullParamName : paramNames) {
154  const auto& [nodeName, paramName] = getNodeAndParamName(fullParamName);
155  paramNamesByNodeName[nodeName].push_back(paramName);
156  }
157 
158  RCLCPP_DEBUG(_node->get_logger(), "Getting %zu parameters from %zu nodes...", paramNames.size(),
159  paramNamesByNodeName.size());
160  } else {
161  // Make a map of node names to empty parameter lists
162  // Only consider nodes that offer services to list & get parameters.
163  for (const auto& fqnNodeName : _node->get_node_names()) {
164  if (fqnNodeName == thisNode) {
165  continue;
166  }
167  const auto [nodeNs, nodeName] = getNodeAndNodeNamespace(fqnNodeName);
168  const auto serviceNamesAndTypes =
169  _node->get_service_names_and_types_by_node(nodeName, nodeNs);
170 
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";
175 
176  if (!getParamsSrvFound) {
177  getParamsSrvFound = std::find(serviceTypes.begin(), serviceTypes.end(),
178  GET_PARAMS_SERVICE_TYPE) != serviceTypes.end();
179  }
180  if (!listParamsSrvFound) {
181  listParamsSrvFound = std::find(serviceTypes.begin(), serviceTypes.end(),
182  LIST_PARAMS_SERVICE_TYPE) != serviceTypes.end();
183  }
184  }
185 
186  if (listParamsSrvFound && getParamsSrvFound) {
187  paramNamesByNodeName.insert({fqnNodeName, {}});
188  }
189  }
190 
191  if (!paramNamesByNodeName.empty()) {
192  RCLCPP_DEBUG(_node->get_logger(), "Getting all parameters from %zu nodes...",
193  paramNamesByNodeName.size());
194  }
195  }
196 
197  std::vector<std::future<ParameterList>> getParametersFuture;
198  for (const auto& [nodeName, nodeParamNames] : paramNamesByNodeName) {
199  if (nodeName == thisNode) {
200  continue;
201  }
202 
203  auto paramClientIt = _paramClientsByNode.find(nodeName);
204  if (paramClientIt == _paramClientsByNode.end()) {
205  const auto insertedPair = _paramClientsByNode.emplace(
206  nodeName,
207  rclcpp::AsyncParametersClient::make_shared(_node, nodeName, parameterQoS, _callbackGroup));
208  paramClientIt = insertedPair.first;
209  }
210 
211  getParametersFuture.emplace_back(
212  std::async(std::launch::async, &ParameterInterface::getNodeParameters, this,
213  paramClientIt->second, nodeName, nodeParamNames, timeout));
214  }
215 
216  ParameterList result;
217  for (auto& future : getParametersFuture) {
218  try {
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());
223  }
224  }
225 
226  return result;
227 }
228 
230  const std::chrono::duration<double>& timeout) {
231  std::lock_guard<std::mutex> lock(_mutex);
232 
233  rclcpp::ParameterMap paramsByNode;
234  for (const auto& param : parameters) {
235  if (!isWhitelisted(param.getName(), _paramWhitelistPatterns)) {
236  return;
237  }
238 
239  const auto rosParam = toRosParam(param);
240  const auto& [nodeName, paramName] = getNodeAndParamName(rosParam.get_name());
241  paramsByNode[nodeName].emplace_back(paramName, rosParam.get_parameter_value());
242  }
243 
244  std::vector<std::future<void>> setParametersFuture;
245  for (const auto& [nodeName, params] : paramsByNode) {
246  auto paramClientIt = _paramClientsByNode.find(nodeName);
247  if (paramClientIt == _paramClientsByNode.end()) {
248  const auto insertedPair = _paramClientsByNode.emplace(
249  nodeName,
250  rclcpp::AsyncParametersClient::make_shared(_node, nodeName, parameterQoS, _callbackGroup));
251  paramClientIt = insertedPair.first;
252  }
253 
254  setParametersFuture.emplace_back(std::async(std::launch::async,
256  paramClientIt->second, nodeName, params, timeout));
257  }
258 
259  for (auto& future : setParametersFuture) {
260  try {
261  future.get();
262  } catch (const std::exception& e) {
263  RCLCPP_ERROR(_node->get_logger(), "Exception when setting parameters: %s", e.what());
264  }
265  }
266 }
267 
268 void ParameterInterface::subscribeParams(const std::vector<std::string>& paramNames) {
269  std::lock_guard<std::mutex> lock(_mutex);
270 
271  std::unordered_set<std::string> nodesToSubscribe;
272  for (const auto& paramName : paramNames) {
273  if (!isWhitelisted(paramName, _paramWhitelistPatterns)) {
274  return;
275  }
276 
277  const auto& [nodeName, paramN] = getNodeAndParamName(paramName);
278  auto [subscribedParamsit, wasNewlyCreated] = _subscribedParamsByNode.try_emplace(nodeName);
279 
280  auto& subscribedNodeParams = subscribedParamsit->second;
281  subscribedNodeParams.insert(paramN);
282 
283  if (wasNewlyCreated) {
284  nodesToSubscribe.insert(nodeName);
285  }
286  }
287 
288  for (const auto& nodeName : nodesToSubscribe) {
289  auto paramClientIt = _paramClientsByNode.find(nodeName);
290  if (paramClientIt == _paramClientsByNode.end()) {
291  const auto insertedPair = _paramClientsByNode.emplace(
292  nodeName,
293  rclcpp::AsyncParametersClient::make_shared(_node, nodeName, parameterQoS, _callbackGroup));
294  paramClientIt = insertedPair.first;
295  }
296 
297  auto& paramClient = paramClientIt->second;
298 
299  _paramSubscriptionsByNode[nodeName] = paramClient->on_parameter_event(
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());
303 
304  ParameterList result;
305  const auto& subscribedNodeParams = _subscribedParamsByNode[nodeName];
306  for (const auto& param : msg->changed_parameters) {
307  if (subscribedNodeParams.find(param.name) != subscribedNodeParams.end()) {
308  result.push_back(fromRosParam(
309  rclcpp::Parameter(prependNodeNameToParamName(param.name, nodeName), param.value)));
310  }
311  }
312 
313  if (!result.empty() && _paramUpdateFunc) {
314  _paramUpdateFunc(result);
315  }
316  });
317  }
318 }
319 
320 void ParameterInterface::unsubscribeParams(const std::vector<std::string>& paramNames) {
321  std::lock_guard<std::mutex> lock(_mutex);
322 
323  for (const auto& paramName : paramNames) {
324  const auto& [nodeName, paramN] = getNodeAndParamName(paramName);
325 
326  const auto subscribedNodeParamsIt = _subscribedParamsByNode.find(nodeName);
327  if (subscribedNodeParamsIt != _subscribedParamsByNode.end()) {
328  subscribedNodeParamsIt->second.erase(subscribedNodeParamsIt->second.find(paramN));
329 
330  if (subscribedNodeParamsIt->second.empty()) {
331  _subscribedParamsByNode.erase(subscribedNodeParamsIt);
333  }
334  }
335  }
336 }
337 
339  std::lock_guard<std::mutex> lock(_mutex);
340  _paramUpdateFunc = paramUpdateFunc;
341 }
342 
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");
348  }
349 
350  auto paramsToRequest = paramNames;
351  if (paramsToRequest.empty()) {
352  // `paramNames` is empty, list all parameter names for this node
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 + "'");
356  }
357  paramsToRequest = future.get().names;
358  }
359 
360  // Start parameter fetches and wait for them to complete
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 + "'");
365  }
366  const auto params = getParamsFuture.get();
367 
368  ParameterList result;
369  for (const auto& param : params) {
370  const auto fullParamName = prependNodeNameToParamName(param.get_name(), nodeName);
371  if (isWhitelisted(fullParamName, _paramWhitelistPatterns)) {
372  result.push_back(fromRosParam(rclcpp::Parameter(fullParamName, param.get_parameter_value())));
373  }
374  }
375  return result;
376 }
377 
378 void ParameterInterface::setNodeParameters(rclcpp::AsyncParametersClient::SharedPtr paramClient,
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");
384  }
385 
386  auto future = paramClient->set_parameters(params);
387 
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());
392  }
393  }
394 
395  if (!paramsToDelete.empty()) {
396  auto deleteFuture = paramClient->delete_parameters(paramsToDelete);
397  if (std::future_status::ready != deleteFuture.wait_for(timeout)) {
398  RCLCPP_WARN(
399  _node->get_logger(),
400  "Param client failed to delete %zu parameter(s) for node '%s' within the given timeout",
401  paramsToDelete.size(), nodeName.c_str());
402  }
403  }
404 
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");
408  }
409 
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());
415  }
416  }
417 }
418 
419 } // namespace foxglove_bridge
foxglove::isWhitelisted
bool isWhitelisted(const std::string &name, const std::vector< std::regex > &regexPatterns)
Definition: regex_utils.hpp:10
foxglove_bridge::ParameterInterface::_subscribedParamsByNode
std::unordered_map< std::string, std::unordered_set< std::string > > _subscribedParamsByNode
Definition: parameter_interface.hpp:37
utils.hpp
foxglove_bridge::ParameterInterface::setParams
void setParams(const ParameterList &params, const std::chrono::duration< double > &timeout)
Definition: parameter_interface.cpp:229
foxglove_bridge::ParameterInterface::setNodeParameters
void setNodeParameters(rclcpp::AsyncParametersClient::SharedPtr paramClient, const std::string &nodeName, const std::vector< rclcpp::Parameter > &params, const std::chrono::duration< double > &timeout)
Definition: parameter_interface.cpp:378
foxglove_bridge::ParameterInterface::getNodeParameters
ParameterList getNodeParameters(rclcpp::AsyncParametersClient::SharedPtr paramClient, const std::string &nodeName, const std::vector< std::string > &paramNames, const std::chrono::duration< double > &timeout)
Definition: parameter_interface.cpp:343
foxglove_bridge::ParameterInterface::getParams
ParameterList getParams(const std::vector< std::string > &paramNames, const std::chrono::duration< double > &timeout)
Definition: parameter_interface.cpp:143
foxglove::ParameterType
ParameterType
Definition: parameter.hpp:16
regex_utils.hpp
foxglove_bridge::ParameterInterface::subscribeParams
void subscribeParams(const std::vector< std::string > &paramNames)
Definition: parameter_interface.cpp:268
foxglove_bridge::ParameterInterface::_paramClientsByNode
std::unordered_map< std::string, rclcpp::AsyncParametersClient::SharedPtr > _paramClientsByNode
Definition: parameter_interface.hpp:36
foxglove::Parameter::getType
ParameterType getType() const
Definition: parameter.hpp:64
foxglove_bridge::ParamUpdateFunc
std::function< void(const ParameterList &)> ParamUpdateFunc
Definition: parameter_interface.hpp:18
foxglove_bridge::ParameterInterface::setParamUpdateCallback
void setParamUpdateCallback(ParamUpdateFunc paramUpdateFunc)
Definition: parameter_interface.cpp:338
foxglove::ParameterValue
Definition: parameter.hpp:27
parameter_interface.hpp
foxglove::Parameter::getName
const std::string & getName() const
Definition: parameter.hpp:60
foxglove_bridge::ParameterInterface::_paramUpdateFunc
ParamUpdateFunc _paramUpdateFunc
Definition: parameter_interface.hpp:39
foxglove_bridge::ParameterInterface::unsubscribeParams
void unsubscribeParams(const std::vector< std::string > &paramNames)
Definition: parameter_interface.cpp:320
foxglove::Parameter
Definition: parameter.hpp:54
foxglove_bridge
Definition: generic_service.hpp:9
foxglove_bridge::toRosParam
XmlRpc::XmlRpcValue toRosParam(const foxglove::ParameterValue &param)
Definition: ros1_foxglove_bridge/src/param_utils.cpp:43
foxglove_bridge::ParameterInterface::_node
rclcpp::Node * _node
Definition: parameter_interface.hpp:32
foxglove_bridge::ParameterList
std::vector< foxglove::Parameter > ParameterList
Definition: parameter_interface.hpp:17
foxglove_bridge::ParameterInterface::_paramWhitelistPatterns
std::vector< std::regex > _paramWhitelistPatterns
Definition: parameter_interface.hpp:33
foxglove_bridge::fromRosParam
foxglove::Parameter fromRosParam(const std::string &name, const XmlRpc::XmlRpcValue &value)
Definition: ros1_foxglove_bridge/src/param_utils.cpp:39
foxglove_bridge::ParameterInterface::_callbackGroup
rclcpp::CallbackGroup::SharedPtr _callbackGroup
Definition: parameter_interface.hpp:34
foxglove::Parameter::getValue
const ParameterValue & getValue() const
Definition: parameter.hpp:68
param
T param(const std::string &param_name, const T &default_val)
foxglove_bridge::ParameterInterface::_paramSubscriptionsByNode
std::unordered_map< std::string, rclcpp::SubscriptionBase::SharedPtr > _paramSubscriptionsByNode
Definition: parameter_interface.hpp:38
foxglove_bridge::ParameterInterface::_mutex
std::mutex _mutex
Definition: parameter_interface.hpp:35
foxglove_bridge::getNodeAndNodeNamespace
std::pair< std::string, std::string > getNodeAndNodeNamespace(const std::string &fqnNodeName)
Definition: utils.hpp:12
foxglove_bridge::ParameterInterface::ParameterInterface
ParameterInterface(rclcpp::Node *node, std::vector< std::regex > paramWhitelistPatterns)
Definition: parameter_interface.cpp:137


foxglove_bridge
Author(s): Foxglove
autogenerated on Wed Mar 5 2025 03:34:31