filter_utils/filter_chain_nodelet.hpp

Go to the documentation of this file.

#pragma once

#include <memory>
#include <mutex>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>

#include <boost/thread/recursive_mutex.hpp>

#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/DiagnosticStatusWrapper.h>
#include <dynamic_reconfigure/server.h>
#include <ros/duration.h>
#include <ros/message_traits.h>
#include <ros/node_handle.h>
#include <ros/publisher.h>
#include <ros/single_subscriber_publisher.h>
#include <ros/subscriber.h>
#include <ros/time.h>

#include <cras_cpp_common/FilterChainConfig.h>

#include <cras_cpp_common/diag_utils.hpp>
#include <cras_cpp_common/diag_utils/diagnosed_pub_sub.hpp>
#include <cras_cpp_common/diag_utils/duration_status.h>
#include <cras_cpp_common/filter_utils/filter_chain.hpp>
#include <cras_cpp_common/nodelet_utils.hpp>

namespace cras
{

template <typename F>
class FilterChainDiagnostics;

template<class F>
class FilterChainNodelet : public ::cras::Nodelet
{
public:
  typedef F DataType;

  typedef ::ros::message_traits::DataType<F> MsgDataType;

  FilterChainNodelet(const ::std::string& dataType, const ::std::string& topicIn, const ::std::string& topicFiltered,
    const ::std::string& configNamespace);

  FilterChainNodelet(const ::std::string& topicIn, const ::std::string& topicFiltered,
    const ::std::string& configNamespace);

  explicit FilterChainNodelet(const ::std::string& configNamespace);

  ~FilterChainNodelet() override;

  void enableFilterPublishers();

  void disableFilterPublishers();

  void setDisabledFilters(::std::unordered_set<::std::string> filters);

  void setMaxAge(const ::ros::Duration& maxAge);

protected:
  void dynreconfCallback(::cras_cpp_common::FilterChainConfig& config, uint32_t);

  void updateDynamicParams() const;

  void onInit() override;

  void subscribe();

  virtual void connectCb(const ::ros::SingleSubscriberPublisher&);

  virtual void disconnectCb(const ::ros::SingleSubscriberPublisher&);

  template <typename T, ::std::enable_if_t<::ros::message_traits::HasHeader<T>::value, bool> = true>
  ::ros::Time getStamp(const typename T::ConstPtr& data);

  template <typename T, ::std::enable_if_t<!::ros::message_traits::HasHeader<T>::value, bool> = true>
  ::ros::Time getStamp(const typename T::ConstPtr& data);

  virtual void dataCallback(const typename F::ConstPtr& data);

  virtual void filterStartCallback(const F& data, size_t filterNum, const ::std::string& name,
    const ::std::string& type);

  virtual void filterFinishedCallback(const F& data, size_t filterNum, const ::std::string& name,
    const ::std::string& type, bool success);

  ::ros::NodeHandle nodeHandle;

  ::ros::NodeHandle privateNodeHandle;

  ::ros::Subscriber subscriber;

  ::std::unique_ptr<::cras::DiagnosedSubscriber<F>> subscriberDiag;

  ::cras::FilterChain<F> filterChain;

  ::ros::Publisher filteredPublisher;

  ::std::unique_ptr<::cras::DiagnosedPublisher<F>> publisherDiag;

  ::std::string topicIn;

  ::std::string topicFiltered;

  ::std::string configNamespace;

  bool publishFilters {false};

  size_t publisherQueueSize {15_sz};

  ::std::unordered_map<::std::string, ::ros::Publisher> filterPublishers;

  ::std::mutex filterPublishersMutex;

  ::std::mutex connectMutex;

  bool lazySubscription {true};

  size_t subscriberQueueSize {15_sz};

  bool publishTopicDiagnostics {false};

  bool publishDurationDiagnostics {false};

  bool publishChainDiagnostics {false};

  ::ros::Duration maxAge;

  mutable ::boost::recursive_mutex configMutex;

  ::std::unique_ptr<::dynamic_reconfigure::Server<::cras_cpp_common::FilterChainConfig>> dynreconfServer;

  ::std::unique_ptr<::cras::DurationStatus> callbackDurationDiag;

  ::std::unordered_map<::std::string, ::std::unique_ptr<::cras::DurationStatus>> filterCallbackDurationDiags;

  ::std::unique_ptr<::cras::FilterChainDiagnostics<F>> chainDiag;
};

template <typename F>
class FilterChainDiagnostics : public ::diagnostic_updater::DiagnosticTask
{
public:
  FilterChainDiagnostics(const ::std::string& name, const ::cras::FilterChain<F>& chain);

  void addReport(const ::std::string& filterName, bool success);

  void run(::diagnostic_updater::DiagnosticStatusWrapper& stat) override;

protected:
  const ::cras::FilterChain<F>& chain;

  ::std::mutex mutex;

  ::std::unordered_map<::std::string, size_t> numCallbacks;

  ::std::unordered_map<::std::string, size_t> numSuccesses;

  ::std::unordered_map<::std::string, size_t> numFailures;
};

}

#include "impl/filter_chain_nodelet.hpp"