Program Listing for File swarm_metrics_manager.hpp

↰ Return to documentation for file (cosm/ros/metrics/swarm_metrics_manager.hpp)

#pragma once

/*******************************************************************************
 * Includes
 ******************************************************************************/
#include <vector>
#include <map>
#include <string>
#include <ros/ros.h>

#include "rcppsw/er/client.hpp"

#include "rcppsw/metrics/config/metrics_config.hpp"
#include "rcppsw/metrics/fs_output_manager.hpp"

#include "cosm/cosm.hpp"
#include "cosm/ros/topic.hpp"
#include "cosm/ros/kin/metrics/kinematics_metrics_msg.hpp"
#include "cosm/ros/spatial/metrics/interference_metrics_msg.hpp"
#include "cosm/ros/fsm/metrics/block_transporter_metrics_msg.hpp"
#include "cosm/ros/foraging/metrics/block_transportee_metrics_msg.hpp"
#include "cosm/ros/foraging/metrics/block_cluster_metrics_msg.hpp"
#include "cosm/hal/ros/sensors/metrics/battery_metrics_msg.hpp"
#include "cosm/ros/metrics/msg_tracking_map.hpp"

/*******************************************************************************
 * Namespaces
 ******************************************************************************/
namespace cosm::ros::metrics {
namespace fs = std::filesystem;

/*******************************************************************************
 * Class Definitions
 ******************************************************************************/
class swarm_metrics_manager : public rer::client<swarm_metrics_manager>,
                              public rmetrics::fs_output_manager {
 public:
  static constexpr const size_t kQueueBufferSize = 1000;

  swarm_metrics_manager(const rmconfig::metrics_config* mconfig,
                        const fs::path& root,
                        size_t n_robots);
  ~swarm_metrics_manager(void) override = default;

  /* fs_output_manager overrides */
  bool flush(const rmetrics::output_mode& mode,
             const rtypes::timestep&) override;

  void interval_reset(const rtypes::timestep&) override;

 protected:
  void register_standard(const rmconfig::metrics_config* mconfig,
                         size_t n_robots);

  void register_with_n_robots(const rmconfig::metrics_config* mconfig,
                              size_t n_robots);

  void register_with_n_block_clusters(const rmconfig::metrics_config* mconfig,
                                      size_t n_robots,
                                      size_t n_block_clusters);

  bool wait_for_connection(const ::ros::Subscriber& sub);

  msg_tracking_map* msg_tracking(void) { return &m_tracking; }

 private:
  void collect(const boost::shared_ptr<const crsmetrics::kinematics_metrics_msg>& msg);
  void collect(const boost::shared_ptr<const crsmetrics::interference_metrics_msg>& msg);
  void collect(const boost::shared_ptr<const crfsm::metrics::block_transporter_metrics_msg>& msg);
  void collect(const boost::shared_ptr<const crfmetrics::block_transportee_metrics_msg>& msg);
  void collect(const boost::shared_ptr<const chros::sensors::metrics::battery_metrics_msg>& msg);
  void collect(const boost::shared_ptr<const crfmetrics::block_cluster_metrics_msg>& msg);


  /* clang-format off */
  using expected_counts_map_type = std::map<rmetrics::output_mode, size_t>;

  const expected_counts_map_type mc_expected_counts{};

  msg_tracking_map               m_tracking{};
  std::vector<::ros::Subscriber> m_subs{};
  /* clang-format on */
};

} /* namespace cosm::ros::metrics */