EC-thor/include/modules/Manager.hpp at master · victhormoreno/EC-thor · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
/**
 * @file Manager.hpp
 * @author Víctor Moreno Borràs (victor.moreno.borras@estudiantat.upc.edu)
 * @brief It contains the specification of the Manager module.
 * @version 2.0
 * @date 2024-04-20
 *
 * @copyright Copyright (c) 2024 BCN eMotorsport
 *
 */

#ifndef MANAGER_HPP
#define MANAGER_HPP

#include <as_msgs/ObservationArray.h>
#include <geometry_msgs/PoseArray.h>
#include <nav_msgs/Odometry.h>

#include "modules/Processor.hpp"
#include "modules/Accumulator.hpp"
#include "modules/Detector.hpp"
#include "modules/Reconstructor.hpp"

/**
 * @brief The Manager module is what coordinates all different Modules.
 * It is responsible for:
 * - Dynamic reconfigure objects
 * - Pipeline calls
 * - Data receiving
 * - Data publishing
 */

class Manager {
 private:
  /* ------------------ Private Constructor And Destructor ------------------ */

  Manager();
  ~Manager();

  /* -------------------------------- Modules ------------------------------- */
  Processor *proc;          /**< Processor module */
  Accumulator *accum;       /**< Accumulator module */
  Detector *detect;         /**< Detector module */
  Reconstructor *reconstr;  /**< Reconstructor module */

  /* -------------------------- Private Attributes -------------------------- */

  ros::Publisher pubObs_, pubState_;
  ros::Publisher pubWalls_, pubGround_, pubObstacle_;
  ros::Publisher pubBuffer_, pubClusters_;

  std_msgs::Header header_;
  nav_msgs::Odometry last_state_;

  Params::Manager params_;

  /* ---------------------------- Private Methods --------------------------- */


  /**
   * @brief Testing methode for saving Clusters on a csv file.
   *
   * @param clusters
   */
  void saveCSV(const std::vector<Cluster> &clusters) const;

  /**
   * @brief Creates the ObservationArray message from the clusters.
   *
   * @param clusters
   * @param obs
   */
  void createObservations(std::vector<Cluster> &clusters, as_msgs::ObservationArray &obs_vector) const;

  /* ---------------------------- Publish Methods --------------------------- */
  void publishWalls(const pcl::PointCloud<MyPointType>::ConstPtr &walls) const;
  void publishGround(const pcl::PointCloud<MyPointType>::ConstPtr &ground) const;
  void publishObstacles(const pcl::PointCloud<MyPointType>::ConstPtr &obstacles) const;
  void publishBuff(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr &cloud) const;
  void publishClusters(const std::vector<Cluster> &clusters) const;
  void publishObservations(std::vector<Cluster> &clusters) const;

 public:
  /* --------------------------- Singleton Pattern -------------------------- */
  static Manager& getInstance() {
    static Manager instance;
    return instance;
  }

  Manager(Manager const &) = delete;
  void operator=(Manager const &) = delete;

  /**
   * @brief It initalizes the module.
   *
   * @param params
   * @param pubObs
   * @param pubGround
   * @param pubObstacle
   * @param pubBuffer
   * @param pubTester
   */
  void init(const Params &params,
            const ros::Publisher &pubObs,
            const ros::Publisher &pubState,
            const ros::Publisher &pubWalls,
            const ros::Publisher &pubGround,
            const ros::Publisher &pubObstacle,
            const ros::Publisher &pubBuffer,
            const ros::Publisher &pubClusters);

  /* ---------------------------- Public Methods ---------------------------- */

  /**
   * @brief Recive the limovelo data and filters the point cloud.
   *
   * @param pointcloud
   * @param state
   */
  void callbackNewData(const sensor_msgs::PointCloud2::ConstPtr &pointcloud, const nav_msgs::Odometry::ConstPtr &state);

  /**
   * @brief Runs the pipeline with last synchronized data.
   */
  void run();

  void getGrid(nav_msgs::OccupancyGrid &grid){
    proc->getGrid(grid);
  }
};

#endif