abstract_robot_element.h

#ifndef ABSTRACT_ROBOT_ELEMENT_
#define ABSTRACT_ROBOT_ELEMENT_

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


class Abstract_robot_element {
    protected:
    tf2::Transform relative_tf_;
    tf2::Transform world_tf_;

    public:
    Abstract_robot_element() = default;
    
    inline void set_relative_tf(tf2::Transform tf) { relative_tf_= tf;}
    inline tf2::Transform& relative_tf(){ return relative_tf_;}
    inline void calc_world_tf(tf2::Transform& tf) {world_tf_= tf * relative_tf_;}
    inline void set_world_tf(tf2::Transform& tf) { world_tf_ = tf;}
    inline tf2::Transform& world_tf() { return world_tf_;}
    
    virtual void update(tf2::Transform& tf)= 0;
};


#endif