13 #include <type_traits> 15 #include <boost/function.hpp> 16 #include <boost/shared_ptr.hpp> 38 struct NodeletWithDiagnosticsPrivate;
103 template <
typename NodeletType>
121 void startDiagTimer()
const;
127 void startDiagTimer(const ::ros::NodeHandle& nh)
const;
132 void stopDiagTimer()
const;
151 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
152 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
154 const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
155 const ::std::string& topic,
size_t queueSize,
bool latch =
false);
168 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
169 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
171 const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
187 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
188 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
190 const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
191 const ::std::string& topic,
size_t queueSize,
bool latch =
false);
204 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
205 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
207 const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
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);
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,
255 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
256 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
258 const ::std::string& topic,
size_t queueSize,
bool latch =
false);
271 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
272 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
288 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
289 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
291 const ::std::string& topic,
size_t queueSize,
bool latch =
false);
303 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
304 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
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);
331 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
332 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
346 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
347 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
349 const ::std::string& topic,
size_t queueSize,
bool latch =
false);
360 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
361 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
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);
388 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
389 ::std::unique_ptr<::cras::DiagnosedPublisher<Message>> advertiseDiagnosed(
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);
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);
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);
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);
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);
491 template <typename M, typename Enable = ::std::enable_if_t<::cras::IsMessageParam<M>::value>>
492 ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<M>>>
495 const ::std::string& topic, uint32_t queue_size,
void(*cb)(M), ::
ros::TransportHints hints = {});
510 template <typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
511 ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>
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 = {});
531 template <typename C, typename Enable = ::std::enable_if_t<::cras::IsMessageParam<C>::value>>
532 ::std::unique_ptr<::cras::DiagnosedSubscriber<::cras::BaseMessage<C>>>
535 const ::std::string& topic, uint32_t queue_size,
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>>>
557 const ::std::string& topic, uint32_t queue_size,
void(T::*cb)(M), T* obj, ::
ros::TransportHints hints = {});
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>>>
578 const ::std::string& topic, uint32_t queue_size,
void(T::*cb)(M)
const, T* obj, ::
ros::TransportHints hints = {});
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>>>
599 const ::std::string& topic, uint32_t queue_size,
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>>>
621 const ::std::string& topic, uint32_t queue_size,
622 void(T::*cb)(M)
const, const ::boost::shared_ptr<T>& obj, ::
ros::TransportHints hints = {});
634 template<typename Message, typename Enable = ::std::enable_if_t<::ros::message_traits::IsMessage<Message>::value>>
635 ::std::unique_ptr<::cras::DiagnosedSubscriber<Message>>
637 const ::cras::SimpleTopicStatusParam<Message>& defaultDiagParams, const ::std::string& diagNamespace,
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 = {}) \ 648 return ::std::move(this->template subscribeDiagnosed<M>(NH2 PARAM2 topic, queue_size, CB2, hints));\ 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 = {}) \ 656 return ::std::move(this->template subscribeDiagnosed<M>(NH2 PARAM2 topic, queue_size, cb, hints));\ 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 = {}) \ 664 return ::std::move(this->template subscribeDiagnosed<M, T>(NH2 PARAM2 topic, queue_size, cb, obj, hints)); \ 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) \ 672 const auto topic = options.topic; \ 673 return ::std::move(this->template subscribeDiagnosed<M>(NH2 PARAM2 options));\ 676 #define CRAS_SINGLE_ARG(...) __VA_ARGS__ 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)) 703 CRAS_SINGLE_ARG(subscriberNh, this->getDefaultDiagNh(subscriberNh, diagNamespace), ),
705 const ::std::string& diagNamespace, ),
712 const ::std::string& diagNamespace, ),
723 CRAS_SINGLE_ARG(subscriberNh, this->getDefaultDiagNh(subscriberNh, diagNamespace), ),
741 CRAS_SINGLE_ARG(subscriberNh, this->getDefaultDiagNh(subscriberNh,
""), ),
751 using NodeletType::getName;
762 const ::ros::NodeHandle& nh, const ::std::string& diagNamespace, const ::std::string& topic);
770 ::ros::NodeHandle getDefaultDiagNh(const ::ros::NodeHandle& pubSubNh, const ::std::string& diagNamespace);
774 ::std::unique_ptr<::cras::impl::NodeletWithDiagnosticsPrivate>
data;
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
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.
#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.