nodelet_with_diagnostics.hpp
Go to the documentation of this file.
1 #pragma once
2 
11 #include <memory>
12 #include <string>
13 #include <type_traits>
14 
15 #include <boost/function.hpp>
16 #include <boost/shared_ptr.hpp>
17 
19 #include <ros/advertise_options.h>
20 #include <ros/message_traits.h>
21 #include <ros/node_handle.h>
22 #include <ros/rate.h>
23 #include <ros/subscribe_options.h>
24 #include <ros/transport_hints.h>
25 
31 
32 namespace cras
33 {
34 
35 namespace impl
36 {
37 // forward declaration
38 struct NodeletWithDiagnosticsPrivate;
39 }
40 
103 template <typename NodeletType>
104 struct NodeletWithDiagnostics : public virtual NodeletType
105 {
106 public:
108  virtual ~NodeletWithDiagnostics(); // we need to be polymorphic
109 
110 protected:
116  ::cras::DiagnosticUpdater& getDiagUpdater(bool forceNew = false) const;
117 
121  void startDiagTimer() const;
122 
127  void startDiagTimer(const ::ros::NodeHandle& nh) const;
128 
132  void stopDiagTimer() const;
133 
135  // ADVERTISE METHODS //
137 
151  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
152  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
153  ::ros::NodeHandle publisherNh, ::ros::NodeHandle diagNh,
154  const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
155  const ::std::string& topic, size_t queueSize, bool latch = false);
156 
168  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
169  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
170  ::ros::NodeHandle publisherNh, ::ros::NodeHandle diagNh,
171  const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
173 
187  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
188  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
189  ::ros::NodeHandle publisherNh,
190  const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
191  const ::std::string& topic, size_t queueSize, bool latch = false);
192 
204  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
205  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
206  ::ros::NodeHandle publisherNh,
207  const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
208  ::ros::AdvertiseOptions& options);
209 
222  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
223  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
224  const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
225  const ::std::string& topic, size_t queueSize, bool latch = false);
226 
237  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
238  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
239  const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
240  ::ros::AdvertiseOptions& options);
241 
255  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
256  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
257  ::ros::NodeHandle publisherNh, ::ros::NodeHandle diagNh, const ::std::string& diagNamespace,
258  const ::std::string& topic, size_t queueSize, bool latch = false);
259 
271  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
272  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
273  ::ros::NodeHandle publisherNh, ::ros::NodeHandle diagNh, const ::std::string& diagNamespace,
274  ::ros::AdvertiseOptions& options);
275 
288  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
289  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
290  ::ros::NodeHandle publisherNh, const ::std::string& diagNamespace,
291  const ::std::string& topic, size_t queueSize, bool latch = false);
292 
303  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
304  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
305  ::ros::NodeHandle publisherNh, const ::std::string& diagNamespace, ::ros::AdvertiseOptions& options);
306 
318  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
319  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
320  const ::std::string& diagNamespace, const ::std::string& topic, size_t queueSize, bool latch = false);
321 
331  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
332  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
333  const ::std::string& diagNamespace, ::ros::AdvertiseOptions& options);
334 
346  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
347  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
348  ::ros::NodeHandle publisherNh, ::ros::NodeHandle diagNh,
349  const ::std::string& topic, size_t queueSize, bool latch = false);
350 
360  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
361  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
362  ::ros::NodeHandle publisherNh, ::ros::NodeHandle diagNh, ros::AdvertiseOptions& options);
363 
375  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
376  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
377  ::ros::NodeHandle publisherNh, const ::std::string& topic, size_t queueSize, bool latch = false);
378 
388  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
389  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
390  ::ros::NodeHandle publisherNh, ros::AdvertiseOptions& options);
391 
402  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
403  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
404  const ::std::string& topic, size_t queueSize, bool latch = false);
405 
414  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
415  ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(ros::AdvertiseOptions& options);
416 
418  // DEPRECATED //
420 
434  template<typename T>
435  [[deprecated("Use advertiseDiagnosed() instead")]]
436  ::std::unique_ptr<::cras::DiagnosedPublisher<T>> createDiagnosedPublisher(
437  ::ros::NodeHandle nh, const ::std::string& topic, size_t queueSize, const ::std::string& paramNamespace,
438  const ::ros::Rate& defaultRate, const ::ros::Rate& defaultMinRate, const ::ros::Rate& defaultMaxRate);
439 
452  template<typename T>
453  [[deprecated("Use advertiseDiagnosed() instead")]]
454  ::std::unique_ptr<::cras::DiagnosedPublisher<T>> createDiagnosedPublisher(
455  ::ros::NodeHandle nh, const ::std::string& topic, size_t queueSize, const ::std::string& paramNamespace,
456  const ::ros::Rate& defaultRate);
457 
469  template<typename T>
470  [[deprecated("Use advertiseDiagnosed() instead")]]
471  ::std::unique_ptr<::cras::DiagnosedPublisher<T>> createDiagnosedPublisher(
472  ::ros::NodeHandle nh, const ::std::string& topic, size_t queueSize, const ::std::string& paramNamespace);
473 
475  // SUBSCRIBE METHODS //
477 
491  template <typename M, typename Enable = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
492  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
493  subscribeDiagnosed(::ros::NodeHandle subscriberNh, ::ros::NodeHandle diagNh,
494  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultDiagParams, const ::std::string& diagNamespace,
495  const ::std::string& topic, uint32_t queue_size, void(*cb)(M), ::ros::TransportHints hints = {});
496 
510  template <typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
511  ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>
512  subscribeDiagnosed(::ros::NodeHandle subscriberNh, ::ros::NodeHandle diagNh,
513  const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
514  const ::std::string& topic, uint32_t queue_size,
515  const ::boost::function<void(const ::boost::shared_ptr<Message>&)>& cb, ::ros::TransportHints hints = {});
516 
531  template <typename C, typename Enable = ::std::enable_if_t<::cras::IsMessageParam<C>::value>>
532  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<C>>>
533  subscribeDiagnosed(::ros::NodeHandle subscriberNh, ::ros::NodeHandle diagNh,
534  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<C>>& defaultDiagParams, const ::std::string& diagNamespace,
535  const ::std::string& topic, uint32_t queue_size,
536  const ::boost::function<void(C)>& cb, ::ros::VoidConstPtr obj = {}, ::ros::TransportHints hints = {});
537 
553  template<typename M, class T, typename Enable = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
554  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
555  subscribeDiagnosed(::ros::NodeHandle subscriberNh, ::ros::NodeHandle diagNh,
556  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultDiagParams, const ::std::string& diagNamespace,
557  const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M), T* obj, ::ros::TransportHints hints = {});
558 
574  template<typename M, class T, typename Enable = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
575  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
576  subscribeDiagnosed(::ros::NodeHandle subscriberNh, ::ros::NodeHandle diagNh,
577  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultDiagParams, const ::std::string& diagNamespace,
578  const ::std::string& topic, uint32_t queue_size, void(T::*cb)(M) const, T* obj, ::ros::TransportHints hints = {});
579 
595  template<typename M, class T, typename Enable = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
596  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
597  subscribeDiagnosed(::ros::NodeHandle subscriberNh, ::ros::NodeHandle diagNh,
598  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultDiagParams, const ::std::string& diagNamespace,
599  const ::std::string& topic, uint32_t queue_size,
600  void(T::*cb)(M), const ::boost::shared_ptr<T>& obj, ::ros::TransportHints hints = {});
601 
617  template<typename M, class T, typename Enable = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
618  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
619  subscribeDiagnosed(::ros::NodeHandle subscriberNh, ::ros::NodeHandle diagNh,
620  const ::cras::SimpleTopicStatusParam<::cras::BaseMessage<M>>& defaultDiagParams, const ::std::string& diagNamespace,
621  const ::std::string& topic, uint32_t queue_size,
622  void(T::*cb)(M) const, const ::boost::shared_ptr<T>& obj, ::ros::TransportHints hints = {});
623 
634  template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
635  ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>
636  subscribeDiagnosed(::ros::NodeHandle subscriberNh, ::ros::NodeHandle diagNh,
637  const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
638  ::ros::SubscribeOptions& options);
639 
640  // The following macros create a cartesian product of overloads for all the optional options. The defined functions
641  // are the same as above, but with a part of parameters before `topic` missing.
642 
643 #define CRAS_NODELET_DIAG_GENERATE_OVERLOAD_M(NH, NH2, PARAM, PARAM2, CB, CB2) \
644  template <typename M, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>> \
645  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>> \
646  subscribeDiagnosed(NH PARAM const ::std::string& topic, uint32_t queue_size, CB, ::ros::TransportHints hints = {}) \
647  { \
648  return ::std::move(this->template subscribeDiagnosed<M>(NH2 PARAM2 topic, queue_size, CB2, hints));\
649  }
650 
651 #define CRAS_NODELET_DIAG_GENERATE_OVERLOAD_MESSAGE(NH, NH2, PARAM, PARAM2, CB) \
652  template <typename M, typename = ::std::enable_if_t<::ros::message_traits::IsMessage<M>::value>> \
653  ::std::unique_ptr<::cras::DiagnosedSubscriber<M>> \
654  subscribeDiagnosed(NH PARAM const ::std::string& topic, uint32_t queue_size, CB, ::ros::TransportHints hints = {}) \
655  { \
656  return ::std::move(this->template subscribeDiagnosed<M>(NH2 PARAM2 topic, queue_size, cb, hints));\
657  }
658 
659 #define CRAS_NODELET_DIAG_GENERATE_OVERLOAD_MT(NH, NH2, PARAM, PARAM2, CB) \
660  template <typename M, class T, typename = ::std::enable_if_t<::cras::IsMessageParam<M>::value>> \
661  ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>> \
662  subscribeDiagnosed(NH PARAM const ::std::string& topic, uint32_t queue_size, CB, ::ros::TransportHints hints = {}) \
663  { \
664  return ::std::move(this->template subscribeDiagnosed<M, T>(NH2 PARAM2 topic, queue_size, cb, obj, hints)); \
665  }
666 
667 #define CRAS_NODELET_DIAG_GENERATE_OVERLOAD_OPTIONS(NH, NH2, PARAM, PARAM2) \
668  template<typename M, typename = ::std::enable_if_t<::ros::message_traits::IsMessage<M>::value>> \
669  ::std::unique_ptr<::cras::DiagnosedSubscriber<M>> \
670  subscribeDiagnosed(NH PARAM ::ros::SubscribeOptions& options) \
671  { \
672  const auto topic = options.topic; \
673  return ::std::move(this->template subscribeDiagnosed<M>(NH2 PARAM2 options));\
674  }
675 
676 #define CRAS_SINGLE_ARG(...) __VA_ARGS__
677 
678 #define CRAS_NODELET_DIAG_GENERATE_OVERLOADS(NH, NH2, PARAM, PARAM2) \
679  CRAS_NODELET_DIAG_GENERATE_OVERLOAD_M(CRAS_SINGLE_ARG(NH), CRAS_SINGLE_ARG(NH2), \
680  CRAS_SINGLE_ARG(PARAM), CRAS_SINGLE_ARG(PARAM2), CRAS_SINGLE_ARG(void(*cb)(M)), CRAS_SINGLE_ARG(cb)) \
681  CRAS_NODELET_DIAG_GENERATE_OVERLOAD_MESSAGE(CRAS_SINGLE_ARG(NH), CRAS_SINGLE_ARG(NH2), \
682  CRAS_SINGLE_ARG(PARAM), CRAS_SINGLE_ARG(PARAM2), \
683  CRAS_SINGLE_ARG(const ::boost::function<void(const ::boost::shared_ptr<M>&)>& cb)) \
684  CRAS_NODELET_DIAG_GENERATE_OVERLOAD_M(CRAS_SINGLE_ARG(NH), CRAS_SINGLE_ARG(NH2), \
685  CRAS_SINGLE_ARG(PARAM), CRAS_SINGLE_ARG(PARAM2), \
686  CRAS_SINGLE_ARG(const ::boost::function<void(M)>& cb, ::ros::VoidConstPtr obj = {}), \
687  CRAS_SINGLE_ARG(cb, obj)) \
688  CRAS_NODELET_DIAG_GENERATE_OVERLOAD_MT(CRAS_SINGLE_ARG(NH), CRAS_SINGLE_ARG(NH2), \
689  CRAS_SINGLE_ARG(PARAM), CRAS_SINGLE_ARG(PARAM2), CRAS_SINGLE_ARG(void(T::*cb)(M), T* obj)) \
690  CRAS_NODELET_DIAG_GENERATE_OVERLOAD_MT(CRAS_SINGLE_ARG(NH), CRAS_SINGLE_ARG(NH2), \
691  CRAS_SINGLE_ARG(PARAM), CRAS_SINGLE_ARG(PARAM2), CRAS_SINGLE_ARG(void(T::*cb)(M) const, T* obj)) \
692  CRAS_NODELET_DIAG_GENERATE_OVERLOAD_MT(CRAS_SINGLE_ARG(NH), CRAS_SINGLE_ARG(NH2), \
693  CRAS_SINGLE_ARG(PARAM), CRAS_SINGLE_ARG(PARAM2), \
694  CRAS_SINGLE_ARG(void(T::*cb)(M), const ::boost::shared_ptr<T>& obj)) \
695  CRAS_NODELET_DIAG_GENERATE_OVERLOAD_MT(CRAS_SINGLE_ARG(NH), CRAS_SINGLE_ARG(NH2), \
696  CRAS_SINGLE_ARG(PARAM), CRAS_SINGLE_ARG(PARAM2), \
697  CRAS_SINGLE_ARG(void(T::*cb)(M) const, const ::boost::shared_ptr<T>& obj)) \
698  CRAS_NODELET_DIAG_GENERATE_OVERLOAD_OPTIONS(CRAS_SINGLE_ARG(NH), CRAS_SINGLE_ARG(NH2), \
699  CRAS_SINGLE_ARG(PARAM), CRAS_SINGLE_ARG(PARAM2))
700 
702  CRAS_SINGLE_ARG(::ros::NodeHandle subscriberNh, ),
703  CRAS_SINGLE_ARG(subscriberNh, this->getDefaultDiagNh(subscriberNh, diagNamespace), ),
705  const ::std::string& diagNamespace, ),
706  CRAS_SINGLE_ARG(defaultDiagParams, diagNamespace, ));
707 
709  CRAS_SINGLE_ARG(),
710  CRAS_SINGLE_ARG(this->getNodeHandle(), ),
712  const ::std::string& diagNamespace, ),
713  CRAS_SINGLE_ARG(defaultDiagParams, diagNamespace, ));
714 
716  CRAS_SINGLE_ARG(::ros::NodeHandle subscriberNh, ::ros::NodeHandle diagNh, ),
717  CRAS_SINGLE_ARG(subscriberNh, diagNh, ),
718  CRAS_SINGLE_ARG(const ::std::string& diagNamespace, ),
720 
722  CRAS_SINGLE_ARG(::ros::NodeHandle subscriberNh, ),
723  CRAS_SINGLE_ARG(subscriberNh, this->getDefaultDiagNh(subscriberNh, diagNamespace), ),
724  CRAS_SINGLE_ARG(const ::std::string& diagNamespace, ),
726 
728  CRAS_SINGLE_ARG(),
729  CRAS_SINGLE_ARG(this->getNodeHandle(), ),
730  CRAS_SINGLE_ARG(const ::std::string& diagNamespace, ),
732 
734  CRAS_SINGLE_ARG(::ros::NodeHandle subscriberNh, ::ros::NodeHandle diagNh, ),
735  CRAS_SINGLE_ARG(subscriberNh, diagNh, ),
736  CRAS_SINGLE_ARG(),
737  CRAS_SINGLE_ARG("", ));
738 
740  CRAS_SINGLE_ARG(::ros::NodeHandle subscriberNh, ),
741  CRAS_SINGLE_ARG(subscriberNh, this->getDefaultDiagNh(subscriberNh, ""), ),
742  CRAS_SINGLE_ARG(),
743  CRAS_SINGLE_ARG("", ));
744 
746  CRAS_SINGLE_ARG(),
747  CRAS_SINGLE_ARG(this->getNodeHandle(), ),
748  CRAS_SINGLE_ARG(),
749  CRAS_SINGLE_ARG("", ));
750 
751  using NodeletType::getName;
752 
753 protected:
761  ::cras::BoundParamHelperPtr getDiagParams(
762  const ::ros::NodeHandle& nh, const ::std::string& diagNamespace, const ::std::string& topic);
763 
770  ::ros::NodeHandle getDefaultDiagNh(const ::ros::NodeHandle& pubSubNh, const ::std::string& diagNamespace);
771 
772 private:
774  ::std::unique_ptr<::cras::impl::NodeletWithDiagnosticsPrivate> data;
775 };
776 
777 }
778 
779 #include "impl/nodelet_with_diagnostics.hpp"
Utilities for working with ROS message files.
Definitions of parameters for a TopicStatus diagnostic task.
::std::unique_ptr<::cras::impl::NodeletWithDiagnosticsPrivate > data
PIMPL.
::std::conditional_t<::ros::message_traits::HasHeader< Message >::value, ::cras::SimpleTopicStatusParamWithHeader, ::cras::SimpleTopicStatusParamNoHeader > SimpleTopicStatusParam
Helper struct for easy brace-initialization of TopicStatusParam objects.
ROS message publisher and subscriber with automatic rate and delay diagnostics.
::std::shared_ptr<::cras::BoundParamHelper > BoundParamHelperPtr
options
Utilities for working with diagnostics-related tools.
typename ::ros::ParameterAdapter< M >::Message BaseMessage
Return the base message type of M (i.e. removes MessageEvent, const, &, shared_ptr etc...
Diagnostic updater that automatically sets its Hardware ID to hostname of the machine.
Definition: updater.h:23
Definition: any.hpp:15
#define CRAS_SINGLE_ARG(...)
#define CRAS_NODELET_DIAG_GENERATE_OVERLOADS(NH, NH2, PARAM, PARAM2)
Nodelet mixin that provides helper functions for running a diagnostics updater.
Diagnostic updater that automatically sets its Hardware ID to hostname of the machine.


cras_cpp_common
Author(s): Martin Pecka
autogenerated on Sat Jun 17 2023 02:32:53