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"