node_handle.h
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2021, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 #ifndef SWRI_ROSCPP_NODE_HANDLE_H_
30 #define SWRI_ROSCPP_NODE_HANDLE_H_
31 
32 #include <ros/node_handle.h>
33 
35 #include <swri_roscpp/subscriber.h>
38 
40 
41 #include <marti_introspection_msgs/NodeInfo.h>
42 
43 // Macro which adds line number info
44 #define SWRI_NODE_HANDLE(nh, pnh, description) swri::NodeHandle(nh, pnh, description, __FILE__)
45 
46 // This is a smart nodehandle that handles storing documentation as well as tracking node names in nodelet managers.
47 
48 namespace swri
49 {
50 class DynamicParameters;
52 {
53  friend class DynamicParameters;
55  {
56  std::string node_name_;
59 
60  // This is additional details about the node
62  marti_introspection_msgs::NodeInfo info_msg_;
64  };
65 
67  std::string namespace_;
68  std::string grouping_;
69 
70  // Resolves the the relative namespace name, namely handles globals
71  std::string resolveName(const std::string& name) const
72  {
73  if (name.length() && name[0] == '/')
74  {
75  return name;
76  }
77  return namespace_ + name;
78  }
79 
80  // Get's the nodehandle to place dynamic parameter topics in
82  {
83  std::string ns = namespace_;
84  if (ns.length() && ns[0] == '/')
85  {
86  ns = ns.substr(1);
87  }
88  return ros::NodeHandle(nh_->pnh_.resolveName(ns));
89  }
90 
91  // check if we should add a parameter or not (have we already added it?)
92  // also returns false if docs are disabled for simplicity
93  bool shouldAddParameter(const std::string& name) const
94  {
95  if (!nh_->enable_docs_)
96  {
97  return false;
98  }
99 
100  for (size_t i = 0; i < nh_->info_msg_.parameters.size(); i++)
101  {
102  if (nh_->info_msg_.parameters[i].name == name)
103  {
104  return false;
105  }
106  }
107  return true;
108  }
109 
110 public:
111 
113  {
114  // we arent valid here
115  }
116 
118  const std::string description,
119  const char* source_file = "")
120  {
121  // create a new nh
123  inh->nh_ = nh;
124  inh->pnh_ = pnh;
125  inh->node_name_ = pnh.getNamespace();
126  nh_.reset(inh);
127 
128  // read a global parameter indicating if we should advertise debug info
129  // this lets people turn it on/off for deployments as it can add a lot of topics
130  nh.param("/swridocs", inh->enable_docs_, true);
131 
132  if (inh->enable_docs_)
133  {
134  // create the node specific publisher and initialize message
135  inh->info_pub_ = pnh.advertise<marti_introspection_msgs::NodeInfo>("documentation", 1, true);
136 
137  inh->info_msg_.name = inh->node_name_;
138  inh->info_msg_.description = description;
139  inh->info_msg_.location = source_file;
140  std::string nm = ros::NodeHandle("~").getNamespace();
141  if (inh->info_msg_.name != nm)
142  {
143  inh->info_msg_.nodelet_manager = nm;// we are indeed running in a nodelet manager
144  }
145  inh->info_pub_.publish(inh->info_msg_);// do the initial publish
146  }
147  }
148 
149  operator void*() const { return nh_ ? (void*)1 : (void*)0; }
150 
151  // Get node handles internal setting for wether or not it is generating docs
152  inline bool getEnableDocs() const
153  {
154  if (nh_)
155  {
156  return nh_->enable_docs_;
157  }
158  else
159  {
160  return false;
161  }
162  }
163 
164  // Get a copy of the current documentation message from the node handle
165  inline marti_introspection_msgs::NodeInfo getDocMsg() const
166  {
167  if (nh_)
168  {
169  return nh_->info_msg_;
170  }
171  else
172  {
173  return marti_introspection_msgs::NodeInfo();
174  }
175  }
176 
177  // Gets a handle relative the base swri::NodeHandle
178  swri::NodeHandle getNodeHandle(const std::string& ns,
179  const std::string& group = "")
180  {
181  swri::NodeHandle ret = *this;
182  ret.namespace_ = ns;
183  if (ns.length())
184  {
185  if (ns.length() > 1 && ns[0] == '~')
186  {
187  ret.namespace_ = nh_->node_name_ + ns.substr(1);
188  }
189  else if (ns == "~")
190  {
191  ret.namespace_ = nh_->node_name_;
192  }
193  ret.namespace_ += '/';
194  }
195  // Only change the group if a new one is indicated, otherwise use our parents
196  if (group.length())
197  {
198  ret.grouping_ = group;
199  }
200  return ret;
201  }
202 
203  template <class T>
204  inline bool getParam(const std::string& name, T& value,
205  const std::string description)
206  {
207  return param(name, value, value, description);
208  }
209 
210  inline bool getParam(const std::string& name, XmlRpc::XmlRpcValue& value,
211  const std::string description)
212  {
213  std::string real_name = resolveName(name);
214  bool set = nh_->pnh_.getParam(real_name, value);
215  ROS_INFO("Read XMLRPC parameter %s", name.c_str());
216 
217  if (shouldAddParameter(real_name))
218  {
219  marti_introspection_msgs::ParamInfo info;
220  info.name = real_name;
221  info.description = description;
222  info.group = grouping_;
223  info.resolved_name = nh_->pnh_.resolveName(real_name);
224  info.type = marti_introspection_msgs::ParamInfo::TYPE_XMLRPC;
225  info.dynamic = false;
226  nh_->info_msg_.parameters.push_back(info);
227  nh_->info_pub_.publish(nh_->info_msg_);
228  }
229  return set;
230  }
231 
232  inline bool getParam(const std::string& name, std::vector<std::string>& value,
233  const std::string description)
234  {
235  std::string real_name = resolveName(name);
236  bool set = nh_->pnh_.getParam(real_name, value);
237  ROS_INFO("Read parameter %s", name.c_str());
238 
239  if (shouldAddParameter(real_name))
240  {
241  marti_introspection_msgs::ParamInfo info;
242  info.name = real_name;
243  info.description = description;
244  info.group = grouping_;
245  info.resolved_name = nh_->pnh_.resolveName(real_name);
246  info.type = marti_introspection_msgs::ParamInfo::TYPE_STRING;
247  info.dynamic = false;
248  nh_->info_msg_.parameters.push_back(info);
249  nh_->info_pub_.publish(nh_->info_msg_);
250  }
251  return set;
252  }
253 
254  template <class T>
255  inline void setParam(const std::string& name, T& value)
256  {
257  nh_->pnh_.setParam(name, value);
258  }
259 
260  // param always uses the private namespace
261  inline
262  bool param(const std::string &name,
263  double &variable,
264  const double default_value,
265  const std::string description,
266  const bool dynamic = false)
267  {
268  std::string real_name = resolveName(name);
269  bool set = nh_->pnh_.param(real_name, variable, default_value);
270  if (!dynamic)
271  {
272  ROS_INFO("Read parameter %s = %lf", real_name.c_str(), variable);
273  }
274 
275  if (shouldAddParameter(real_name))
276  {
277  marti_introspection_msgs::ParamInfo info;
278  info.name = real_name;
279  info.description = description;
280  info.group = grouping_;
281  info.resolved_name = nh_->pnh_.resolveName(real_name);
282  info.type = marti_introspection_msgs::ParamInfo::TYPE_DOUBLE;
283  info.default_double = default_value;
284  info.dynamic = dynamic;
285  nh_->info_msg_.parameters.push_back(info);
286  nh_->info_pub_.publish(nh_->info_msg_);
287  }
288  return set;
289  }
290 
291  // param always uses the private namespace
292  // this function clamps the parameter to the indicated range
293  inline
294  bool ranged_param(const std::string &name,
295  double &variable,
296  const double default_value,
297  const std::string description,
298  const double min_value = 0.0,
299  const double max_value = 0.0,
300  const bool dynamic = false)
301  {
302  std::string real_name = resolveName(name);
303  bool set = nh_->pnh_.param(real_name, variable, default_value);
304  if (!dynamic)
305  {
306  ROS_INFO("Read parameter %s = %lf", real_name.c_str(), variable);
307  }
308 
309  if (variable < min_value)
310  {
311  ROS_ERROR("Parameter '%s' is out of range. Clamping to %f.", real_name.c_str(), min_value);
312  variable = min_value;
313  }
314  else if (variable > max_value)
315  {
316  ROS_ERROR("Parameter '%s' is out of range. Clamping to %f.", real_name.c_str(), max_value);
317  variable = max_value;
318  }
319 
320  if (shouldAddParameter(real_name))
321  {
322  marti_introspection_msgs::ParamInfo info;
323  info.name = real_name;
324  info.description = description;
325  info.group = grouping_;
326  info.resolved_name = nh_->pnh_.resolveName(real_name);
327  info.type = marti_introspection_msgs::ParamInfo::TYPE_DOUBLE;
328  info.default_double = default_value;
329  info.dynamic = dynamic;
330  info.max_value = max_value;
331  info.min_value = min_value;
332  nh_->info_msg_.parameters.push_back(info);
333  nh_->info_pub_.publish(nh_->info_msg_);
334  }
335  return set;
336  }
337 
338  inline
339  bool ranged_param(const std::string &name,
340  int &variable,
341  const int default_value,
342  const std::string description,
343  const int min_value = 0.0,
344  const int max_value = 0.0,
345  const bool dynamic = false)
346  {
347  std::string real_name = resolveName(name);
348  bool set = nh_->pnh_.param(real_name, variable, default_value);
349  if (!dynamic)
350  {
351  ROS_INFO("Read parameter %s = %i", real_name.c_str(), variable);
352  }
353 
354  if (variable < min_value)
355  {
356  ROS_ERROR("Parameter '%s' is out of range. Clamping to %i.", real_name.c_str(), min_value);
357  variable = min_value;
358  }
359  else if (variable > max_value)
360  {
361  ROS_ERROR("Parameter '%s' is out of range. Clamping to %i.", real_name.c_str(), max_value);
362  variable = max_value;
363  }
364 
365  if (shouldAddParameter(real_name))
366  {
367  marti_introspection_msgs::ParamInfo info;
368  info.name = real_name;
369  info.description = description;
370  info.group = grouping_;
371  info.resolved_name = nh_->pnh_.resolveName(real_name);
372  info.type = marti_introspection_msgs::ParamInfo::TYPE_INT;
373  info.default_int = default_value;
374  info.dynamic = dynamic;
375  info.max_value = max_value;
376  info.min_value = min_value;
377  nh_->info_msg_.parameters.push_back(info);
378  nh_->info_pub_.publish(nh_->info_msg_);
379  }
380  return set;
381  }
382 
383  inline
384  bool ranged_param(const std::string &name,
385  float &variable,
386  const float default_value,
387  const std::string description,
388  const float min_value = 0.0,
389  const float max_value = 0.0,
390  const bool dynamic = false)
391  {
392  std::string real_name = resolveName(name);
393  bool set = nh_->pnh_.param(real_name, variable, default_value);
394  if (!dynamic)
395  {
396  ROS_INFO("Read parameter %s = %lf", real_name.c_str(), variable);
397  }
398 
399  if (variable < min_value)
400  {
401  ROS_ERROR("Parameter '%s' is out of range. Clamping to %f.", real_name.c_str(), min_value);
402  variable = min_value;
403  }
404  else if (variable > max_value)
405  {
406  ROS_ERROR("Parameter '%s' is out of range. Clamping to %f.", real_name.c_str(), max_value);
407  variable = max_value;
408  }
409 
410  if (shouldAddParameter(real_name))
411  {
412  marti_introspection_msgs::ParamInfo info;
413  info.name = real_name;
414  info.description = description;
415  info.group = grouping_;
416  info.resolved_name = nh_->pnh_.resolveName(real_name);
417  info.type = marti_introspection_msgs::ParamInfo::TYPE_FLOAT;
418  info.default_float = default_value;
419  info.dynamic = dynamic;
420  info.max_value = max_value;
421  info.min_value = min_value;
422  nh_->info_msg_.parameters.push_back(info);
423  nh_->info_pub_.publish(nh_->info_msg_);
424  }
425  return set;
426  }
427 
428  inline
429  bool param(const std::string &name,
430  float &variable,
431  const float default_value,
432  const std::string description,
433  const bool dynamic = false)
434  {
435  std::string real_name = resolveName(name);
436  bool set = nh_->pnh_.param(real_name, variable, default_value);
437  if (!dynamic)
438  {
439  ROS_INFO("Read parameter %s = %lf", real_name.c_str(), variable);
440  }
441 
442  if (shouldAddParameter(real_name))
443  {
444  marti_introspection_msgs::ParamInfo info;
445  info.name = real_name;
446  info.description = description;
447  info.group = grouping_;
448  info.resolved_name = nh_->pnh_.resolveName(real_name);
449  info.type = marti_introspection_msgs::ParamInfo::TYPE_DOUBLE;
450  info.default_double = default_value;
451  info.dynamic = dynamic;
452  nh_->info_msg_.parameters.push_back(info);
453  nh_->info_pub_.publish(nh_->info_msg_);
454  }
455  return set;
456  }
457 
458  // param always uses the private namespace
459  inline
460  bool param(const std::string &name,
461  int &variable,
462  const int default_value,
463  const std::string description,
464  const bool dynamic = false)
465  {
466  std::string real_name = resolveName(name);
467  bool set = nh_->pnh_.param(real_name, variable, default_value);
468  if (!dynamic)
469  {
470  ROS_INFO("Read parameter %s = %i", real_name.c_str(), variable);
471  }
472 
473  if (shouldAddParameter(real_name))
474  {
475  marti_introspection_msgs::ParamInfo info;
476  info.name = real_name;
477  info.description = description;
478  info.group = grouping_;
479  info.resolved_name = nh_->pnh_.resolveName(real_name);
480  info.type = marti_introspection_msgs::ParamInfo::TYPE_INT;
481  info.default_int = default_value;
482  info.dynamic = dynamic;
483  nh_->info_msg_.parameters.push_back(info);
484  nh_->info_pub_.publish(nh_->info_msg_);
485  }
486  return set;
487  }
488 
489  // param always uses the private namespace
490  inline
491  bool param(const std::string &name,
492  std::string &variable,
493  const std::string default_value,
494  const std::string description,
495  const bool dynamic = false)
496  {
497  std::string real_name = resolveName(name);
498  bool set = nh_->pnh_.param(real_name, variable, default_value);
499  if (!dynamic)
500  {
501  ROS_INFO("Read parameter %s = %s", real_name.c_str(), variable.c_str());
502  }
503 
504  if (shouldAddParameter(real_name))
505  {
506  marti_introspection_msgs::ParamInfo info;
507  info.name = real_name;
508  info.description = description;
509  info.group = grouping_;
510  info.resolved_name = nh_->pnh_.resolveName(real_name);
511  info.type = marti_introspection_msgs::ParamInfo::TYPE_STRING;
512  info.default_string = default_value;
513  info.dynamic = dynamic;
514  nh_->info_msg_.parameters.push_back(info);
515  nh_->info_pub_.publish(nh_->info_msg_);
516  }
517  return set;
518  }
519 
520  // param always uses the private namespace
521  inline
522  bool param(const std::string &name,
523  bool &variable,
524  const bool default_value,
525  const std::string description,
526  const bool dynamic = false)
527  {
528  std::string real_name = resolveName(name);
529  bool set = nh_->pnh_.param(real_name, variable, default_value);
530  if (!dynamic)
531  {
532  ROS_INFO("Read parameter %s = %s", real_name.c_str(), variable ? "true" : "false");
533  }
534 
535  if (shouldAddParameter(real_name))
536  {
537  marti_introspection_msgs::ParamInfo info;
538  info.name = real_name;
539  info.description = description;
540  info.group = grouping_;
541  info.resolved_name = nh_->pnh_.resolveName(real_name);
542  info.type = marti_introspection_msgs::ParamInfo::TYPE_BOOL;
543  info.default_bool = default_value;
544  info.dynamic = dynamic;
545  nh_->info_msg_.parameters.push_back(info);
546  nh_->info_pub_.publish(nh_->info_msg_);
547  }
548  return set;
549  }
550 
551  // Using class method callback.
552  template<class M , class T >
554  uint32_t queue_size,
555  void(T::*fp)(const boost::shared_ptr< M const > &),
556  T *obj,
557  const std::string description,
558  const ros::TransportHints &transport_hints=ros::TransportHints())
559  {
560  std::string real_name = resolveName(name);
561 
562  if (nh_->enable_docs_)
563  {
564  const std::string resolved_name = nh_->nh_.resolveName(real_name);
565  marti_introspection_msgs::TopicInfo info;
566  info.name = real_name;
567  info.resolved_name = resolved_name;
568  info.group = grouping_;
569  info.message_type = ros::message_traits::DataType<M>().value();
570  info.advertised = false;
571  info.description = description;
572  nh_->info_msg_.topics.push_back(info);
573  nh_->info_pub_.publish(nh_->info_msg_);
574  }
575 
576  return swri::Subscriber(nh_->nh_, real_name, queue_size, fp, obj, transport_hints);
577  }
578 
579  // Using boost function callback.
580  template<class M>
581  swri::Subscriber subscribe_swri(const std::string &name,
582  uint32_t queue_size,
583  boost::function<void(const boost::shared_ptr<M const> &)> fp,
584  const std::string description,
585  const ros::TransportHints &transport_hints=ros::TransportHints())
586  {
587  std::string real_name = resolveName(name);
588 
589  if (nh_->enable_docs_)
590  {
591  const std::string resolved_name = nh_->nh_.resolveName(real_name);
592  marti_introspection_msgs::TopicInfo info;
593  info.name = real_name;
594  info.resolved_name = resolved_name;
595  info.group = grouping_;
596  info.message_type = ros::message_traits::DataType<M>().value();
597  info.advertised = false;
598  info.description = description;
599  nh_->info_msg_.topics.push_back(info);
600  nh_->info_pub_.publish(nh_->info_msg_);
601  }
602 
603  return swri::Subscriber(nh_->nh_, real_name, queue_size, fp, transport_hints);
604  }
605 
606  // Using class method callback.
607  template<class M , class T >
608  ros::Subscriber subscribe(const std::string &name,
609  uint32_t queue_size,
610  void(T::*fp)(const boost::shared_ptr< M const > &),
611  T *obj,
612  const std::string description,
613  const ros::TransportHints &transport_hints=ros::TransportHints())
614  {
615  std::string real_name = resolveName(name);
616  if (nh_->enable_docs_)
617  {
618  const std::string resolved_name = nh_->nh_.resolveName(real_name);
619  marti_introspection_msgs::TopicInfo info;
620  info.name = real_name;
621  info.resolved_name = resolved_name;
622  info.group = grouping_;
623  info.message_type = ros::message_traits::DataType<M>().value();
624  info.advertised = false;
625  info.description = description;
626  nh_->info_msg_.topics.push_back(info);
627  nh_->info_pub_.publish(nh_->info_msg_);
628  }
629 
630  return nh_->nh_.subscribe(real_name, queue_size, fp, obj, transport_hints);
631  }
632 
633  // The const class method overload
634  template<class M , class T >
635  ros::Subscriber subscribe(const std::string &name,
636  uint32_t queue_size,
637  void(T::*fp)(const boost::shared_ptr< M const > &) const,
638  T *obj,
639  const std::string description,
640  const ros::TransportHints &transport_hints=ros::TransportHints())
641  {
642  std::string real_name = resolveName(name);
643  if (nh_->enable_docs_)
644  {
645  const std::string resolved_name = nh_->nh_.resolveName(real_name);
646  marti_introspection_msgs::TopicInfo info;
647  info.name = real_name;
648  info.resolved_name = resolved_name;
649  info.group = grouping_;
650  info.message_type = ros::message_traits::DataType<M>().value();
651  info.advertised = false;
652  info.description = description;
653  nh_->info_msg_.topics.push_back(info);
654  nh_->info_pub_.publish(nh_->info_msg_);
655  }
656 
657  return nh_->nh_.subscribe(real_name, queue_size, fp, obj, transport_hints);
658  }
659 
660  // Using boost function callback.
661  template<class M>
662  ros::Subscriber subscribe(const std::string &name,
663  uint32_t queue_size,
664  const boost::function<void(const boost::shared_ptr< M const > &)>& callback,
665  const std::string description,
666  const ros::TransportHints &transport_hints=ros::TransportHints())
667  {
668  std::string real_name = resolveName(name);
669  if (nh_->enable_docs_)
670  {
671  const std::string resolved_name = nh_->nh_.resolveName(real_name);
672  marti_introspection_msgs::TopicInfo info;
673  info.name = real_name;
674  info.resolved_name = resolved_name;
675  info.group = grouping_;
676  info.message_type = ros::message_traits::DataType<M>().value();
677  info.advertised = false;
678  info.description = description;
679  nh_->info_msg_.topics.push_back(info);
680  nh_->info_pub_.publish(nh_->info_msg_);
681  }
682 
683  return nh_->nh_.subscribe<M>(real_name, queue_size, callback, ros::VoidConstPtr(), transport_hints);
684  }
685 
686  // Using class method callback.
687  template<class M , class T >
688  ros::Subscriber subscribe(const std::string &name,
689  uint32_t queue_size,
690  void(T::*fp)(const ros::MessageEvent< M const > &),
691  T *obj,
692  const std::string description,
693  const ros::TransportHints &transport_hints=ros::TransportHints())
694  {
695  std::string real_name = resolveName(name);
696  if (nh_->enable_docs_)
697  {
698  const std::string resolved_name = nh_->nh_.resolveName(real_name);
699  marti_introspection_msgs::TopicInfo info;
700  info.name = real_name;
701  info.resolved_name = resolved_name;
702  info.group = grouping_;
703  info.message_type = ros::message_traits::DataType<M>().value();
704  info.advertised = false;
705  info.description = description;
706  nh_->info_msg_.topics.push_back(info);
707  nh_->info_pub_.publish(nh_->info_msg_);
708  }
709 
710  return nh_->nh_.subscribe(real_name, queue_size, fp, obj, transport_hints);
711  }
712 
713  // Using const class method callback.
714  template<class M , class T >
715  ros::Subscriber subscribe(const std::string &name,
716  uint32_t queue_size,
717  void(T::*fp)(const ros::MessageEvent< M const > &) const,
718  T *obj,
719  const std::string description,
720  const ros::TransportHints &transport_hints=ros::TransportHints())
721  {
722  std::string real_name = resolveName(name);
723  if (nh_->enable_docs_)
724  {
725  const std::string resolved_name = nh_->nh_.resolveName(real_name);
726  marti_introspection_msgs::TopicInfo info;
727  info.name = real_name;
728  info.resolved_name = resolved_name;
729  info.group = grouping_;
730  info.message_type = ros::message_traits::DataType<M>().value();
731  info.advertised = false;
732  info.description = description;
733  nh_->info_msg_.topics.push_back(info);
734  nh_->info_pub_.publish(nh_->info_msg_);
735  }
736 
737  return nh_->nh_.subscribe(real_name, queue_size, fp, obj, transport_hints);
738  }
739 
740  template<class M>
742  const std::string &name,
744  const std::string description,
745  const ros::TransportHints &transport_hints=ros::TransportHints())
746  {
747  std::string real_name = resolveName(name);
748  if (nh_->enable_docs_)
749  {
750  const std::string resolved_name = nh_->nh_.resolveName(real_name);
751  marti_introspection_msgs::TopicInfo info;
752  info.name = real_name;
753  info.resolved_name = resolved_name;
754  info.group = grouping_;
755  info.message_type = ros::message_traits::DataType<M>().value();
756  info.advertised = false;
757  info.description = description;
758  nh_->info_msg_.topics.push_back(info);
759  nh_->info_pub_.publish(nh_->info_msg_);
760  }
761 
762  return swri::Subscriber(nh_->nh_, real_name, dest, transport_hints);
763  }
764 
765  // Using public node handle and class method callback.
766  // Only use this for strange things like message filters
767  template<class M>
768  void subscribe_later(const std::string &name,
769  const std::string description)
770  {
771  std::string real_name = resolveName(name);
772  if (nh_->enable_docs_)
773  {
774  const std::string resolved_name = nh_->nh_.resolveName(real_name);
775  marti_introspection_msgs::TopicInfo info;
776  info.name = real_name;
777  info.resolved_name = resolved_name;
778  info.group = grouping_;
779  info.message_type = ros::message_traits::DataType<M>().value();
780  info.advertised = false;
781  info.description = description;
782  nh_->info_msg_.topics.push_back(info);
783  nh_->info_pub_.publish(nh_->info_msg_);
784  }
785  }
786 
787  // Using public node handle and class method callback.
788  // Only use this for strange things like image transports.
789  template<class M>
790  void advertise_later(const std::string &name,
791  const std::string description)
792  {
793  std::string real_name = resolveName(name);
794  if (nh_->enable_docs_)
795  {
796  const std::string resolved_name = nh_->nh_.resolveName(real_name);
797  marti_introspection_msgs::TopicInfo info;
798  info.name = real_name;
799  info.resolved_name = resolved_name;
800  info.group = grouping_;
801  info.message_type = ros::message_traits::DataType<M>().value();
802  info.advertised = true;
803  info.description = description;
804  nh_->info_msg_.topics.push_back(info);
805  nh_->info_pub_.publish(nh_->info_msg_);
806  }
807  }
808 
809  // Using public node handle and class method callback.
810  template<class M , class T >
812  uint32_t queue_size,
813  void(T::*fp)(const boost::shared_ptr< M const > &),
814  T *obj,
815  const std::string description,
816  const ros::TransportHints &transport_hints=ros::TransportHints())
817  {
818  std::string real_name = resolveName(name);
819  if (nh_->enable_docs_)
820  {
821  const std::string resolved_name = nh_->nh_.resolveName(real_name);
822  marti_introspection_msgs::TopicInfo info;
823  info.name = real_name;
824  info.resolved_name = resolved_name;
825  info.group = grouping_;
826  info.message_type = ros::message_traits::DataType<M>().value();
827  info.advertised = false;
828  info.description = description;
829  nh_->info_msg_.topics.push_back(info);
830  nh_->info_pub_.publish(nh_->info_msg_);
831  }
832 
833  return swri::OptionalSubscriber(nh_->nh_, real_name, queue_size, fp, obj);
834  }
835 
836  // Uses the public node handle
837  template<class M>
839  const std::string description)
840  {
841  std::string real_name = resolveName(name);
842  if (nh_->enable_docs_)
843  {
844  const std::string resolved_name = nh_->nh_.resolveName(real_name);
845  marti_introspection_msgs::ServiceInfo info;
846  info.name = real_name;
847  info.resolved_name = resolved_name;
848  info.group = grouping_;
849  info.message_type = ros::message_traits::DataType<typename M:: Request>().value();
850  info.topic_service = true;
851  info.server = false;
852  info.description = description;
853  nh_->info_msg_.services.push_back(info);
854  nh_->info_pub_.publish(nh_->info_msg_);
855  }
856 
858  tsc.initialize(nh_->nh_, real_name);
859  return tsc;
860  }
861 
862  // Uses the public node handle
863  template<class MReq, class MRes, class T>
865  bool(T::*srv_func)(const MReq &, MRes &),
866  T *obj,
867  const std::string description)
868  {
869  std::string real_name = resolveName(name);
870  if (nh_->enable_docs_)
871  {
872  const std::string resolved_name = nh_->nh_.resolveName(real_name);
873  marti_introspection_msgs::ServiceInfo info;
874  info.name = real_name;
875  info.resolved_name = resolved_name;
876  info.group = grouping_;
877  info.message_type = ros::message_traits::DataType<MReq>().value();
878  info.topic_service = true;
879  info.server = true;
880  info.description = description;
881  nh_->info_msg_.services.push_back(info);
882  nh_->info_pub_.publish(nh_->info_msg_);
883  }
884 
886  tss.initialize(nh_->nh_, real_name, srv_func, obj);
887  return tss;
888  }
889 
890  template<class T>
891  ros::ServiceClient serviceClient(const std::string& name,
892  const std::string& description)
893  {
894  std::string real_name = resolveName(name);
895  if (nh_->enable_docs_)
896  {
897  const std::string resolved_name = nh_->nh_.resolveName(real_name);
898  marti_introspection_msgs::ServiceInfo info;
899  info.name = real_name;
900  info.resolved_name = resolved_name;
901  info.group = grouping_;
902  info.message_type = ros::service_traits::DataType<T>().value();
903  info.topic_service = false;
904  info.server = false;
905  info.description = description;
906  nh_->info_msg_.services.push_back(info);
907  nh_->info_pub_.publish(nh_->info_msg_);
908  }
909 
910  return nh_->nh_.serviceClient<T>(name);
911  }
912 
913  // Uses the public node handle
914  template<class MReq, class MRes, class T>
915  ros::ServiceServer advertiseService(const std::string &name,
916  bool(T::*srv_func)(MReq &, MRes &),
917  T *obj,
918  const std::string description)
919  {
920  std::string real_name = resolveName(name);
921  if (nh_->enable_docs_)
922  {
923  const std::string resolved_name = nh_->nh_.resolveName(real_name);
924  marti_introspection_msgs::ServiceInfo info;
925  info.name = real_name;
926  info.resolved_name = resolved_name;
927  info.group = grouping_;
928  info.message_type = ros::service_traits::DataType<MReq>().value();
929  info.topic_service = false;
930  info.server = true;
931  info.description = description;
932  nh_->info_msg_.services.push_back(info);
933  nh_->info_pub_.publish(nh_->info_msg_);
934  }
935 
936  return nh_->nh_.advertiseService(real_name, srv_func, obj);
937  }
938 
939  template<class MReq, class MRes, class T>
941  bool(T::*srv_func)(MReq &, MRes &),
942  T *obj,
943  const std::string description)
944  {
945  std::string real_name = resolveName(name);
946  if (nh_->enable_docs_)
947  {
948  const std::string resolved_name = nh_->nh_.resolveName(real_name);
949  marti_introspection_msgs::ServiceInfo info;
950  info.name = real_name;
951  info.resolved_name = resolved_name;
952  info.group = grouping_;
953  info.message_type = ros::service_traits::DataType<MReq>().value();
954  info.topic_service = false;
955  info.server = true;
956  info.description = description;
957  nh_->info_msg_.services.push_back(info);
958  nh_->info_pub_.publish(nh_->info_msg_);
959  }
960 
961  return swri::ServiceServer(nh_->pnh_, real_name, srv_func, obj);
962  }
963 
964  template<class MReq, class MRes, class T>
966  bool(T::*srv_func)(ros::ServiceEvent< MReq, MRes > &),
967  T *obj,
968  const std::string description)
969  {
970  std::string real_name = resolveName(name);
971  if (nh_->enable_docs_)
972  {
973  const std::string resolved_name = nh_->nh_.resolveName(real_name);
974  marti_introspection_msgs::ServiceInfo info;
975  info.name = real_name;
976  info.resolved_name = resolved_name;
977  info.group = grouping_;
978  info.message_type = ros::service_traits::DataType<MReq>().value();
979  info.topic_service = false;
980  info.server = true;
981  info.description = description;
982  nh_->info_msg_.services.push_back(info);
983  nh_->info_pub_.publish(nh_->info_msg_);
984  }
985 
986  return swri::ServiceServer(nh_->pnh_, real_name, srv_func, obj);
987  }
988 
989  template<class MReq, class MRes, class T>
991  bool(T::*srv_func)(const std::string &, const MReq &, MRes &),
992  T *obj,
993  const std::string description)
994  {
995  std::string real_name = resolveName(name);
996  if (nh_->enable_docs_)
997  {
998  const std::string resolved_name = nh_->nh_.resolveName(real_name);
999  marti_introspection_msgs::ServiceInfo info;
1000  info.name = real_name;
1001  info.resolved_name = resolved_name;
1002  info.group = grouping_;
1003  info.message_type = ros::service_traits::DataType<MReq>().value();
1004  info.topic_service = false;
1005  info.server = true;
1006  info.description = description;
1007  nh_->info_msg_.services.push_back(info);
1008  nh_->info_pub_.publish(nh_->info_msg_);
1009  }
1010 
1011  return swri::ServiceServer(nh_->pnh_, real_name, srv_func, obj);
1012  }
1013 
1014  // Advertising uses the public nh
1015  template<typename M>
1017  const std::string name,
1018  uint32_t queue_size,
1019  bool latched,
1020  const std::string description)
1021  {
1022  std::string real_name = resolveName(name);
1023  const std::string resolved_name = nh_->nh_.resolveName(real_name);
1024  ROS_INFO("Publishing [%s] to '%s' from node %s.",
1025  real_name.c_str(),
1026  resolved_name.c_str(),
1027  nh_->node_name_.c_str());
1028 
1029  if (nh_->enable_docs_)
1030  {
1031  marti_introspection_msgs::TopicInfo info;
1032  info.name = real_name;
1033  info.resolved_name = resolved_name;
1034  info.group = grouping_;
1035  info.message_type = ros::message_traits::DataType<M>().value();
1036  info.advertised = true;
1037  info.description = description;
1038  nh_->info_msg_.topics.push_back(info);
1039  nh_->info_pub_.publish(nh_->info_msg_);
1040  }
1041 
1042  return nh_->nh_.advertise<M>(real_name, queue_size, latched);
1043  }
1044 
1045  // Advertising uses the public nh
1046  template<typename M>
1048  const std::string name,
1049  uint32_t queue_size,
1050  const char* description)
1051  {
1052  std::string real_name = resolveName(name);
1053  const std::string resolved_name = nh_->nh_.resolveName(real_name);
1054  ROS_INFO("Publishing [%s] to '%s' from node %s.",
1055  real_name.c_str(),
1056  resolved_name.c_str(),
1057  nh_->node_name_.c_str());
1058 
1059  if (nh_->enable_docs_)
1060  {
1061  marti_introspection_msgs::TopicInfo info;
1062  info.name = real_name;
1063  info.resolved_name = resolved_name;
1064  info.group = grouping_;
1065  info.message_type = ros::message_traits::DataType<M>().value();
1066  info.advertised = true;
1067  info.description = description;
1068  nh_->info_msg_.topics.push_back(info);
1069  nh_->info_pub_.publish(nh_->info_msg_);
1070  }
1071 
1072  return nh_->nh_.advertise<M>(real_name, queue_size, false);
1073  }
1074 
1075  // Advertising uses the public nh
1076  template<typename M>
1078  const std::string &topic,
1079  uint32_t queue_size,
1080  const ros::SubscriberStatusCallback &connect_cb,
1082  const ros::VoidConstPtr &tracked_object = ros::VoidConstPtr(),
1083  bool latch=false,
1084  const std::string &description = "")
1085  {
1086  std::string real_topic_name = resolveName(topic);
1087  const std::string resolved_name = nh_->nh_.resolveName(real_topic_name);
1088  ROS_INFO("Publishing [%s] to '%s' from node %s.",
1089  real_topic_name.c_str(),
1090  resolved_name.c_str(),
1091  nh_->node_name_.c_str());
1092  if (nh_->enable_docs_)
1093  {
1094  marti_introspection_msgs::TopicInfo info;
1095  info.name = real_topic_name;
1096  info.resolved_name = resolved_name;
1097  info.group = grouping_;
1098  info.message_type = ros::message_traits::DataType<M>().value();
1099  info.advertised = true;
1100  info.description = description;
1101  nh_->info_msg_.topics.push_back(info);
1102  nh_->info_pub_.publish(nh_->info_msg_);
1103  }
1105  ops.init<M>(real_topic_name, queue_size, connect_cb, disconnect_cb);
1106  ops.tracked_object = tracked_object;
1107  ops.latch = latch;
1108  return nh_->nh_.advertise(ops);
1109  }
1110 
1111  // Advertising uses the public nh
1113  const std::string &description = "")
1114  {
1115  std::string real_topic_name = resolveName(ops.topic);
1116  const std::string resolved_name = nh_->nh_.resolveName(real_topic_name);
1117  ROS_INFO("Publishing [%s] to '%s' from node %s.",
1118  real_topic_name.c_str(),
1119  resolved_name.c_str(),
1120  nh_->node_name_.c_str());
1121  if (nh_->enable_docs_)
1122  {
1123  marti_introspection_msgs::TopicInfo info;
1124  info.name = real_topic_name;
1125  info.resolved_name = resolved_name;
1126  info.group = grouping_;
1127  info.message_type = ops.datatype;
1128  info.advertised = true;
1129  info.description = description;
1130  nh_->info_msg_.topics.push_back(info);
1131  nh_->info_pub_.publish(nh_->info_msg_);
1132  }
1133  return nh_->nh_.advertise(ops);
1134  }
1135 
1136  // Using class method callback.
1137  template<class T >
1139  void(T::*fp)(const ros::TimerEvent &),
1140  T *obj,
1141  const bool oneshot = false,
1142  const bool autostart = true)
1143  {
1144  return nh_->nh_.createTimer(duration, fp, obj, oneshot, autostart);
1145  }
1146 
1147  // Using class method callback.
1148  template<class T >
1150  void(T::*fp)(const ros::WallTimerEvent &),
1151  T *obj,
1152  const bool oneshot = false,
1153  const bool autostart = true)
1154  {
1155  return nh_->nh_.createWallTimer(duration, fp, obj, oneshot, autostart);
1156  }
1157 };
1158 
1159 inline void param(swri::NodeHandle& nh,
1160  const std::string name,
1161  std::string& value,
1162  const std::string def,
1163  const std::string description)
1164 {
1165  nh.param(name, value, def, description);
1166 }
1167 
1169  const std::string name,
1170  double& value,
1171  const double def,
1172  const std::string description = "",
1173  const double min = -std::numeric_limits<double>::infinity(),
1174  const double max = std::numeric_limits<double>::infinity())
1175 {
1176  nh.ranged_param(name, value, def, description, min, max);
1177 }
1178 
1179 template<typename T>
1181  const std::string name,
1182  T& value,
1183  const T def)
1184 {
1185  nh.param(name, value, def, "");
1186 }
1187 
1188 template<typename T>
1190  const std::string name,
1191  T& value,
1192  const T def,
1193  const std::string description)
1194 {
1195  nh.param(name, value, def, description);
1196 }
1197 
1198 // Requires the parameter be set
1199 template<typename T>
1201  const std::string name,
1202  T& value,
1203  const std::string description)
1204 {
1205  bool res = nh.getParam(name, value, description);
1206  if (!res)
1207  {
1208  ROS_ERROR("Required parameter %s does not exist", name.c_str());
1209  }
1210  return res;
1211 }
1212 
1213 template<typename T>
1215  const std::string& name,
1216  T& value)
1217 {
1218  nh.setParam(name, value);
1219 }
1220 
1221 // some simple utility functions
1222 template<typename M>
1224  const std::string name,
1225  uint32_t queue_size,
1226  bool latched,
1227  const std::string description)
1228 {
1229  return nh.advertise<M>(name, queue_size, latched, description);
1230 }
1231 
1232 template<typename M>
1234  const std::string name,
1235  uint32_t queue_size,
1236  const char* description)
1237 {
1238  return nh.advertise<M>(name, queue_size, false, description);
1239 }
1240 
1241 // Using class method callback.
1242 template<class M , class T >
1244  const std::string &name,
1245  uint32_t queue_size,
1246  void(T::*fp)(const boost::shared_ptr< M const > &),
1247  T *obj,
1248  const std::string description,
1249  const ros::TransportHints &transport_hints=ros::TransportHints())
1250 {
1251  return nh.subscribe_swri(name, queue_size, fp, obj, description, transport_hints);
1252 }
1253 
1254 template<class M>
1256  const std::string &name,
1258  const std::string description,
1259  const ros::TransportHints &transport_hints=ros::TransportHints())
1260 {
1261  return nh.subscribe_swri(name, dest, description, transport_hints);
1262 }
1263 
1264 template<class MReq, class MRes, class T>
1266  const std::string &service,
1267  bool(T::*srv_func)(MReq &, MRes &),
1268  T *obj,
1269  const std::string description)
1270 {
1271  return nh.advertise_service_swri(service, srv_func, obj, description);
1272 }
1273 
1274 template<class MReq, class MRes, class T>
1276  const std::string &service,
1277  bool(T::*srv_func)(ros::ServiceEvent< MReq, MRes > &),
1278  T *obj,
1279  const std::string description)
1280 {
1281  return nh.advertise_service_swri(service, srv_func, obj, description);
1282 }
1283 
1284 template<class MReq, class MRes, class T>
1286  const std::string &service,
1287  bool(T::*srv_func)(const std::string &, const MReq &, MRes &),
1288  T *obj,
1289  const std::string description)
1290 {
1291  return nh.advertise_service_swri(service, srv_func, obj, description);
1292 }
1293 
1295  swri::Subscriber& sub,
1296  const std::string name,
1297  const double timeout,
1298  const std::string desc)
1299 {
1300  double to = timeout;
1301  nh.param(name, to, to, desc);
1302  sub.setTimeout(to);
1303 }
1304 
1305 } // namespace swri
1306 #endif // SWRI_ROSCPP_NODE_HANDLE_H_
ros::NodeHandle getDynamicParameterNodeHandle() const
Definition: node_handle.h:81
ros::Subscriber subscribe(const std::string &name, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr< M const > &) const, T *obj, const std::string description, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: node_handle.h:635
swri::ServiceServer advertise_service_swri(const std::string &name, bool(T::*srv_func)(MReq &, MRes &), T *obj, const std::string description)
Definition: node_handle.h:940
NodeHandle(ros::NodeHandle nh, ros::NodeHandle pnh, const std::string description, const char *source_file="")
Definition: node_handle.h:117
std::string grouping_
Definition: node_handle.h:68
boost::shared_ptr< void const > VoidConstPtr
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
marti_introspection_msgs::NodeInfo info_msg_
Definition: node_handle.h:62
swri::NodeHandle getNodeHandle(const std::string &ns, const std::string &group="")
Definition: node_handle.h:178
bool shouldAddParameter(const std::string &name) const
Definition: node_handle.h:93
swri::Subscriber subscribe_swri(const std::string &name, boost::shared_ptr< M const > *dest, const std::string description, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: node_handle.h:741
ros::Publisher advertise(ros::AdvertiseOptions &ops, const std::string &description="")
Definition: node_handle.h:1112
ros::Subscriber subscribe(const std::string &name, uint32_t queue_size, void(T::*fp)(const ros::MessageEvent< M const > &), T *obj, const std::string description, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: node_handle.h:688
void init(const std::string &_topic, uint32_t _queue_size, const SubscriberStatusCallback &_connect_cb=SubscriberStatusCallback(), const SubscriberStatusCallback &_disconnect_cb=SubscriberStatusCallback())
ros::Publisher advertise(const std::string &topic, uint32_t queue_size, const ros::SubscriberStatusCallback &connect_cb, const ros::SubscriberStatusCallback &disconnect_cb=ros::SubscriberStatusCallback(), const ros::VoidConstPtr &tracked_object=ros::VoidConstPtr(), bool latch=false, const std::string &description="")
Definition: node_handle.h:1077
swri::ServiceServer advertise_service_swri(const std::string &name, bool(T::*srv_func)(const std::string &, const MReq &, MRes &), T *obj, const std::string description)
Definition: node_handle.h:990
void initialize(ros::NodeHandle &nh, const std::string &service, bool(T::*srv_func)(const MReq &, MRes &), T *obj)
void timeoutParam(swri::NodeHandle &nh, swri::Subscriber &sub, const std::string name, const double timeout, const std::string desc)
Definition: node_handle.h:1294
bool ranged_param(const std::string &name, float &variable, const float default_value, const std::string description, const float min_value=0.0, const float max_value=0.0, const bool dynamic=false)
Definition: node_handle.h:384
boost::shared_ptr< NodeHandleInternal > nh_
Definition: node_handle.h:66
bool param(const std::string &name, int &variable, const int default_value, const std::string description, const bool dynamic=false)
Definition: node_handle.h:460
bool param(const std::string &name, bool &variable, const bool default_value, const std::string description, const bool dynamic=false)
Definition: node_handle.h:522
bool ranged_param(const std::string &name, int &variable, const int default_value, const std::string description, const int min_value=0.0, const int max_value=0.0, const bool dynamic=false)
Definition: node_handle.h:339
bool ranged_param(const std::string &name, double &variable, const double default_value, const std::string description, const double min_value=0.0, const double max_value=0.0, const bool dynamic=false)
Definition: node_handle.h:294
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool getParam(const std::string &name, T &value, const std::string description)
Definition: node_handle.h:204
void publish(const boost::shared_ptr< M > &message) const
marti_introspection_msgs::NodeInfo getDocMsg() const
Definition: node_handle.h:165
bool getEnableDocs() const
Definition: node_handle.h:152
swri::OptionalSubscriber subscribe_optional(const std::string &name, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr< M const > &), T *obj, const std::string description, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: node_handle.h:811
ros::WallTimer createWallTimer(ros::WallDuration duration, void(T::*fp)(const ros::WallTimerEvent &), T *obj, const bool oneshot=false, const bool autostart=true)
Definition: node_handle.h:1149
ros::Timer createTimer(ros::Duration duration, void(T::*fp)(const ros::TimerEvent &), T *obj, const bool oneshot=false, const bool autostart=true)
Definition: node_handle.h:1138
std::string namespace_
Definition: node_handle.h:67
void initialize(ros::NodeHandle &nh, const std::string &service, const std::string &client_name="")
VoidConstPtr tracked_object
#define ROS_INFO(...)
swri::TopicServiceClient< M > topic_service_client(const std::string &name, const std::string description)
Definition: node_handle.h:838
ros::Publisher advertise(const std::string name, uint32_t queue_size, const char *description)
Definition: node_handle.h:1047
bool param(const std::string &name, std::string &variable, const std::string default_value, const std::string description, const bool dynamic=false)
Definition: node_handle.h:491
ros::Subscriber subscribe(const std::string &name, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr< M const > &), T *obj, const std::string description, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: node_handle.h:608
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Subscriber subscribe(const std::string &name, uint32_t queue_size, void(T::*fp)(const ros::MessageEvent< M const > &) const, T *obj, const std::string description, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: node_handle.h:715
void setTimeout(const ros::Duration &time_out)
Definition: subscriber.h:412
void setParam(const std::string &name, T &value)
Definition: node_handle.h:255
ros::ServiceClient serviceClient(const std::string &name, const std::string &description)
Definition: node_handle.h:891
void advertise_later(const std::string &name, const std::string description)
Definition: node_handle.h:790
swri::ServiceServer advertise_service_swri(const std::string &name, bool(T::*srv_func)(ros::ServiceEvent< MReq, MRes > &), T *obj, const std::string description)
Definition: node_handle.h:965
void subscribe_later(const std::string &name, const std::string description)
Definition: node_handle.h:768
bool param(const std::string &name, float &variable, const float default_value, const std::string description, const bool dynamic=false)
Definition: node_handle.h:429
swri::Subscriber subscribe_swri(const std::string &name, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr< M const > &), T *obj, const std::string description, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: node_handle.h:553
const std::string & getNamespace() const
bool getParam(const std::string &name, XmlRpc::XmlRpcValue &value, const std::string description)
Definition: node_handle.h:210
ros::ServiceServer advertiseService(const std::string &name, bool(T::*srv_func)(MReq &, MRes &), T *obj, const std::string description)
Definition: node_handle.h:915
int min(int a, int b)
bool param(const std::string &name, double &variable, const double default_value, const std::string description, const bool dynamic=false)
Definition: node_handle.h:262
std::string resolveName(const std::string &name) const
Definition: node_handle.h:71
swri::Subscriber subscribe_swri(const std::string &name, uint32_t queue_size, boost::function< void(const boost::shared_ptr< M const > &)> fp, const std::string description, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: node_handle.h:581
ros::Subscriber subscribe(const std::string &name, uint32_t queue_size, const boost::function< void(const boost::shared_ptr< M const > &)> &callback, const std::string description, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: node_handle.h:662
bool getParam(const std::string &name, std::vector< std::string > &value, const std::string description)
Definition: node_handle.h:232
#define ROS_ERROR(...)
ros::Publisher advertise(const std::string name, uint32_t queue_size, bool latched, const std::string description)
Definition: node_handle.h:1016
swri::TopicServiceServer topic_service_server(const std::string &name, bool(T::*srv_func)(const MReq &, MRes &), T *obj, const std::string description)
Definition: node_handle.h:864


swri_roscpp
Author(s):
autogenerated on Sat Jan 21 2023 03:13:16