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 "octomap/octomap.h"
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>

#define box_size tf2::Vector3(0.0318f, 0.0636f, 0.091f)

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<Abstract_robot_element*> wings_;




    public:
    Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
        wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right));
        wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,0.4425f -0.005f)), left));
        wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,0.4425f-0.005f)), mid));
    }
    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<pcl::PointXYZ>>& result_vector() {return result_vector_;}
    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);
    void add_wing(Robot* robot) {
        if (robot->observers().size() == 0){
            Abstract_robot_element* rviz_right = new Wing_rviz_decorator(wings_[0], robot->rviz_markers()[0]);
            robot->register_observers(rviz_right);
        } else if (robot->observers().size() == 1){
            Abstract_robot_element* rviz_left = new Wing_rviz_decorator(wings_[1], robot->rviz_markers()[0]);
            robot->register_observers(rviz_left);
        } else if (robot->observers().size() == 2){
            Abstract_robot_element* rviz_mid = new Wing_rviz_decorator(wings_[2], robot->rviz_markers()[0]);
            robot->register_observers(rviz_mid);
        } else {
            ROS_INFO("[Warning] %s got to many wings", robot->name().c_str());
        }
    }
    virtual bool check_collision( const int& robot)=0;
    virtual void mediate()=0;

};


#endif