mediator.h

#ifndef MEDIATOR_
#define MEDIATOR_

#include "ros/ros.h"
#include "impl/abstract_mediator.h"
#include "impl/abstract_robot.h"
#include "impl/robot.h"

class Mediator : public Abstract_mediator{
    protected:
    visualization_msgs::MarkerArray* rviz_markers_ = new visualization_msgs::MarkerArray();

    public:
    Mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
    void check_collision() override {
        //for (int i = 0; i < objects_.size(); i++) {
            for(int j = 0; j < objects_[0].size(); j++){
                visualization_msgs::Marker marker;
                marker.header.frame_id = "map";
                marker.header.stamp = ros::Time();
                marker.ns = "objects";
                marker.id = j;
                marker.type = visualization_msgs::Marker::CUBE;
                marker.action = visualization_msgs::Marker::ADD;
                marker.pose.position.x = objects_[0][j].getOrigin().getX();
                marker.pose.position.y = objects_[0][j].getOrigin().getY();
                marker.pose.position.z = objects_[0][j].getOrigin().getZ();
                marker.pose.orientation.x = objects_[0][j].getRotation().getX();
                marker.pose.orientation.y = objects_[0][j].getRotation().getY();
                marker.pose.orientation.z = objects_[0][j].getRotation().getZ();
                marker.pose.orientation.w = objects_[0][j].getRotation().getW();
                marker.scale.x = box_size.getX();
                marker.scale.y = box_size.getY();
                marker.scale.z = box_size.getZ();
                if(robots_[0]->check_single_object_collision(objects_[0][j])){
                    ROS_INFO("true");
                    marker.color.r = 0;
                    marker.color.g = 1.0;
                    marker.color.b = 0;
                    marker.color.a = 1;
                } else {
                    ROS_INFO("false");
                    marker.color.r = 1;
                    marker.color.g = 0;
                    marker.color.b = 0;
                    marker.color.a = 1.0;
                }
                rviz_markers_->markers.push_back(marker);
            }
        //}
        ROS_INFO("alle punkte durch");
    }

    void mediate() override {
        ros::Rate loop_rate(1);
        for (Abstract_robot* robot : robots_){
            Robot* ceti = dynamic_cast<Robot*>(robot);
            while (ros::ok()){
                for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear();
                rviz_markers_->markers.clear();

                ceti->rotate(-M_PI/4);
                ceti->notify();
                check_collision();
                for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub_->publish(*markers);
                pub_->publish(*rviz_markers_);
                loop_rate.sleep();
            }
        }
    }
};


#endif