abstract_robot.h

#ifndef ABSTRACT_ROBOT_
#define ABSTRACT_ROBOT_

#include "ros/ros.h"
#include <tf2/LinearMath/Transform.h>

#include "impl/abstract_strategy.h"
#include "impl/abstract_robot_element.h"

#define right_size tf2::Vector3(0.7f, 0.5f, 0.01f)
#define left_size tf2::Vector3(0.7f, 0.5f, 0.01f)
#define mid_size tf2::Vector3(0.5f, 0.7f, 0.01f)

enum wing_config{
    RML = 7,
    RM_ = 6,
    R_L = 5,
    R__ = 4,
    _ML = 3,
    _M_ = 2,
    __L = 1,
    ___ = 0
};

class Abstract_robot {
    protected:
    std::string name_;
    tf2::Vector3 size_;
    tf2::Transform tf_;
    tf2::Transform root_tf_;
    std::vector<tf2::Transform> bounds_;
    std::vector<tf2::Transform> robot_root_bounds_;
    std::bitset<3> observer_mask_;



    public:
    Abstract_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : name_(name), tf_(tf), size_(size){
        root_tf_= tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.22f, 0, tf.getOrigin().getZ()));

        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, 0.4f, tf.getOrigin().getZ()))); // ++
        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, 0.4f, tf.getOrigin().getZ()))); // +-
        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, -0.4f, tf.getOrigin().getZ()))); //-+
        bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, -0.4f, tf.getOrigin().getZ()))); // --

        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, 0.095f, 0))); // ++
        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, 0.095f, 0))); // +-
        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, -0.095f, 0)));
        robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, -0.095f, 0)));

        observer_mask_ = std::bitset<3>(0);

    };


    
    inline std::string& name() { return name_;}
    inline tf2::Transform& tf() { return tf_;}
    inline tf2::Vector3& size() { return size_;}
    inline tf2::Transform& root_tf() { return root_tf_;}
    inline std::vector<tf2::Transform>& bounds() {return bounds_;}
    inline void size(tf2::Vector3& s) { size_ = s;}
    inline void set_tf(tf2::Transform& t) { tf_ = t;}
    inline std::vector<tf2::Transform>& robot_root_bounds() { return robot_root_bounds_;}
    inline void rotate(float deg) {tf2::Quaternion rot; rot.setRPY(0,0,deg); tf2::Transform t(rot, tf2::Vector3(0,0,0)); tf_= tf_* t;}
    inline void translate(tf2::Vector3 t) {tf2::Transform tf(tf2::Quaternion(0,0,0,1), t); tf_*=tf;}
    inline std::bitset<3> observer_mask() {return observer_mask_;}
    inline void set_observer_mask(int i) {observer_mask_ = std::bitset<3>(i);}



    virtual void notify()= 0;
    virtual bool check_single_object_collision(tf2::Transform& obj, std::string& b)= 0;
    virtual void workload_checker(std::vector<int>& count_vector, tf2::Transform& obj) =0;

};


#endif