robot.h

#ifndef ROBOT_
#define ROBOT_

#include "ros/ros.h"
#include "impl/abstract_robot.h"
#include "impl/abstract_robot_element.h"
#include "impl/abstract_robot_element_decorator.h"
#include "impl/wing.h"
#include "impl/wing_rviz_decorator.h"
#include "impl/field.h"

#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/ApplyPlanningScene.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>

class Robot : public Abstract_robot{
    protected:
    std::vector<Abstract_robot_element*> observers_;
    std::vector<Abstract_robot_element*> access_fields_;
    std::vector<visualization_msgs::MarkerArray*> rviz_markers_;
    std::vector<moveit_msgs::CollisionObject*> coll_markers_;


    public:
    Robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : Abstract_robot(name, tf, size){ 
        generate_access_fields();
    }
    
    inline void add_rviz_markers(visualization_msgs::MarkerArray* marker) {rviz_markers_.push_back(marker);}
    inline std::vector<visualization_msgs::MarkerArray*> rviz_markers(){return rviz_markers_;}
    inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
    inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}

    inline void add_coll_markers(moveit_msgs::CollisionObject* marker) {coll_markers_.push_back(marker);}
    inline std::vector<moveit_msgs::CollisionObject*> coll_markers(){ return coll_markers_;}

    void register_observers(Abstract_robot_element* wd);
    void reset();
    void generate_access_fields();
    float area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C);
    bool check_robot_collision(Robot* R);

    bool check_single_object_collision(tf2::Transform& obj, std::string& str) override;
    void workload_checker(std::vector<int>& count_vector, tf2::Transform& obj) override;
    void notify() override;


};




#endif