abstract_mediator.h

#ifndef ABSTRACT_MEDIATOR_
#define ABSTRACT_MEDIATOR_

#include <ros/ros.h>
#include "impl/abstract_robot.h"
#include "impl/abstract_robot_element.h"
#include "impl/robot.h"

#include <ros/package.h>
#include <yaml-cpp/yaml.h>
#include <fstream>

#include <octomap/octomap.h>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>

#define box_size tf2::Vector3(0.0318f, 0.0636f, 0.091f)
#define R_POS tf2::Vector3(0,0.6525f,0.4425f -0.005f)
#define L_POS tf2::Vector3(0,-0.6525f,0.4425f -0.005f)
#define M_POS tf2::Vector3(0.6525f,0,0.4425f-0.005f)

struct wing_BP {
    std::string name_;
    tf2::Transform pos_;
    tf2::Vector3 size_;
};


class Abstract_mediator {
    protected:
    std::vector<Abstract_robot*> robots_;
    std::vector<std::vector<tf2::Transform>> objects_;
    std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
    ros::Publisher* pub_;
    std::vector<std::vector<pcl::PointXYZ>> result_vector_;
    std::vector<std::vector<Abstract_robot_element*>> wings_;
    std::string dirname_;





    public:
    Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
    }

    inline void connect_robots(Abstract_robot* robot) {robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());}
    inline void set_result_vector(std::vector<std::vector<pcl::PointXYZ>>& res) {result_vector_ = res;}
    inline std::vector<std::vector<Abstract_robot_element*>> wings() {return wings_;}
    inline std::vector<std::vector<pcl::PointXYZ>>& result_vector() {return result_vector_;}
    inline std::vector<Abstract_robot*> robots(){return robots_;}


    inline void set_dirname(std::string& dirn) {dirname_ = dirn;}
    inline std::string& dirname() {return dirname_;}



    pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
    std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
    
    virtual void set_wings(std::vector<std::vector<wing_BP>>& wbp)=0;
    virtual bool check_collision( const int& robot)=0;
    virtual void mediate()=0;
    virtual void build_wings(std::bitset<3>& wing, int& robot)=0;


};


#endif