EventDetector.hpp

 
#pragma once
 
#include <map>
#include <memory>
#include <optional>
#include <string>
 
#include <tf2_ros/transform_listener.h>
#include <lifecycle_msgs/msg/transition.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/time.hpp>
#include <rclcpp/time_source.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
#include <rosbag2_cpp/reader.hpp>
#include <rosbag2_cpp/writer.hpp>
 
#include "event_detector/AnalysisManager.hpp"
#include "event_detector/BufferManager.hpp"
 
namespace event_detector {
 
class EventDetector;
 
class EventDetector : public rclcpp_lifecycle::LifecycleNode {
 public:
  explicit EventDetector(rclcpp::NodeOptions options);
 
  std::vector<ClientConnection> getConnectedClients() const;
 
 private:
    std::shared_ptr<rclcpp::TimeSource> time_source_;
 
 protected:
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
      const rclcpp_lifecycle::State& state) override;
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(
      const rclcpp_lifecycle::State& state) override;
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(
      const rclcpp_lifecycle::State& state) override;
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(
      const rclcpp_lifecycle::State& state) override;
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(
      const rclcpp_lifecycle::State& state) override;
 
  void registerClients();
 
  void insertTransformPassthrough(const std::shared_ptr<const tf2_msgs::msg::TFMessage>& transforms,
                                  const std::string& topic,
                                  std::optional<rclcpp::Time> stamp_override = std::optional<rclcpp::Time>());
 
 protected:
  void waitForTimeToStart();
 
  void loadParameters();
  void loadClientParameters();
  void setup();
  void initializeBufferFromBag();
  template <typename T>
  void insertPassthrough(const std::shared_ptr<const T>& sample, const std::string& client_id, const std::string& topic,
                         std::optional<rclcpp::Time> stamp_override = std::optional<rclcpp::Time>());
  template <typename T>
  rclcpp::Time getSampleStamp(const T& sample);
 
  rcl_interfaces::msg::SetParametersResult onParametersSetCallback(const std::vector<rclcpp::Parameter>& params);
 
  void postParameterSetCallback(rcl_interfaces::msg::ParameterEvent::UniquePtr event);
 
  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 
  rclcpp::SyncParametersClient::SharedPtr parameters_client_;
 
  rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr param_sub_;
 
  struct {
    double default_time;                           
    int default_queue_size;                        
    bool use_msg_stamp;                            
    std::vector<std::string> initialization_bags;  
  } buffer_config_;
 
  struct {
    double default_period;  
  } analysis_config_;
 
  std::shared_ptr<BufferManager> buffer_manager_;
 
  std::shared_ptr<AnalysisManager> analysis_manager_;
 
  std::vector<ClientConnection> connected_clients_;
 
  mutable std::shared_mutex reconfigure_mutex_;
 
  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr transforms_subscriber_;
 
  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr static_transforms_subscriber_;
 
#define DATATYPE(TYPE, VAR) std::map<std::string, rclcpp::Subscription<TYPE>::ConstSharedPtr> VAR##_subscribers_;
#include "event_detector/datatypes.macro"
#undef DATATYPE
};
 
}  // namespace event_detector
 
#include "event_detector/EventDetector.tpp"