impl/abstract_robot.h
Go to the documentation of this file.
1 #ifndef ABSTRACT_ROBOT_
2 #define ABSTRACT_ROBOT_
3 
4 #include "ros/ros.h"
5 #include <tf2/LinearMath/Transform.h>
6 
9 
10 #define right_size tf2::Vector3(0.7f, 0.5f, 0.01f)
11 #define left_size tf2::Vector3(0.7f, 0.5f, 0.01f)
12 #define mid_size tf2::Vector3(0.5f, 0.7f, 0.01f)
13 
15  RML = 7,
16  RM_ = 6,
17  R_L = 5,
18  R__ = 4,
19  _ML = 3,
20  _M_ = 2,
21  __L = 1,
22  ___ = 0
23 };
24 
26  protected:
27  std::string name_;
28  tf2::Vector3 size_;
29  tf2::Transform tf_;
30  tf2::Transform root_tf_;
31  std::vector<tf2::Transform> bounds_;
32  std::vector<tf2::Transform> robot_root_bounds_;
33  std::bitset<3> observer_mask_;
34 
35 
36 
37  public:
38  Abstract_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : name_(name), tf_(tf), size_(size){
39  root_tf_= tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.22f, 0, tf.getOrigin().getZ()));
40 
41  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, 0.4f, tf.getOrigin().getZ()))); // ++
42  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, 0.4f, tf.getOrigin().getZ()))); // +-
43  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, -0.4f, tf.getOrigin().getZ()))); //-+
44  bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, -0.4f, tf.getOrigin().getZ()))); // --
45 
46  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, 0.095f, 0))); // ++
47  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, 0.095f, 0))); // +-
48  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, -0.095f, 0)));
49  robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, -0.095f, 0)));
50 
51  observer_mask_ = std::bitset<3>(0);
52 
53  };
54 
55 
56 
57  inline std::string& name() { return name_;}
58  inline tf2::Transform& tf() { return tf_;}
59  inline tf2::Vector3& size() { return size_;}
60  inline tf2::Transform& root_tf() { return root_tf_;}
61  inline std::vector<tf2::Transform>& bounds() {return bounds_;}
62  inline void size(tf2::Vector3& s) { size_ = s;}
63  inline void set_tf(tf2::Transform& t) { tf_ = t;}
64  inline std::vector<tf2::Transform>& robot_root_bounds() { return robot_root_bounds_;}
65  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;}
66  inline void translate(tf2::Vector3 t) {tf2::Transform tf(tf2::Quaternion(0,0,0,1), t); tf_*=tf;}
67  inline std::bitset<3> observer_mask() {return observer_mask_;}
68  inline void set_observer_mask(int i) {observer_mask_ = std::bitset<3>(i);}
69 
70 
71 
72  virtual void notify()= 0;
73  virtual bool check_single_object_collision(tf2::Transform& obj, std::string& b)= 0;
74  virtual void workload_checker(std::vector<int>& count_vector, tf2::Transform& obj) =0;
75 
76 };
77 
78 
79 #endif
Abstract_robot::notify
virtual void notify()=0
Abstract_robot::observer_mask_
std::bitset< 3 > observer_mask_
Definition: impl/abstract_robot.h:33
abstract_robot_element.h
Abstract_robot::root_tf_
tf2::Transform root_tf_
Robot root on table top.
Definition: impl/abstract_robot.h:30
Abstract_robot::rotate
void rotate(float deg)
Definition: impl/abstract_robot.h:65
RML
@ RML
Definition: impl/abstract_robot.h:15
Abstract_robot::check_single_object_collision
virtual bool check_single_object_collision(tf2::Transform &obj, std::string &b)=0
Abstract_robot::root_tf
tf2::Transform & root_tf()
Definition: impl/abstract_robot.h:60
R__
@ R__
Definition: impl/abstract_robot.h:18
Abstract_robot::size
void size(tf2::Vector3 &s)
Definition: impl/abstract_robot.h:62
RM_
@ RM_
Definition: impl/abstract_robot.h:16
Abstract_robot::workload_checker
virtual void workload_checker(std::vector< int > &count_vector, tf2::Transform &obj)=0
Abstract_robot::set_tf
void set_tf(tf2::Transform &t)
Definition: impl/abstract_robot.h:63
__L
@ __L
Definition: impl/abstract_robot.h:21
Abstract_robot::bounds_
std::vector< tf2::Transform > bounds_
Bounds of table top surface.
Definition: impl/abstract_robot.h:31
Abstract_robot::translate
void translate(tf2::Vector3 t)
Definition: impl/abstract_robot.h:66
Abstract_robot::tf_
tf2::Transform tf_
Pose of table.
Definition: impl/abstract_robot.h:29
Abstract_robot::name
std::string & name()
Definition: impl/abstract_robot.h:57
Abstract_robot::size
tf2::Vector3 & size()
Definition: impl/abstract_robot.h:59
_M_
@ _M_
Definition: impl/abstract_robot.h:20
Abstract_robot::Abstract_robot
Abstract_robot(std::string &name, tf2::Transform tf, tf2::Vector3 size)
Definition: impl/abstract_robot.h:38
_ML
@ _ML
Definition: impl/abstract_robot.h:19
Abstract_robot::tf
tf2::Transform & tf()
Definition: impl/abstract_robot.h:58
Abstract_robot::name_
std::string name_
Name of robot.
Definition: impl/abstract_robot.h:27
Abstract_robot::observer_mask
std::bitset< 3 > observer_mask()
Definition: impl/abstract_robot.h:67
R_L
@ R_L
Definition: impl/abstract_robot.h:17
Abstract_robot::size_
tf2::Vector3 size_
Size of table.
Definition: impl/abstract_robot.h:28
Abstract_robot::bounds
std::vector< tf2::Transform > & bounds()
Definition: impl/abstract_robot.h:61
Abstract_robot::robot_root_bounds
std::vector< tf2::Transform > & robot_root_bounds()
Definition: impl/abstract_robot.h:64
___
@ ___
Definition: impl/abstract_robot.h:22
wing_config
wing_config
Definition: impl/abstract_robot.h:14
abstract_strategy.h
Abstract_robot
Definition: impl/abstract_robot.h:25
Abstract_robot::set_observer_mask
void set_observer_mask(int i)
Definition: impl/abstract_robot.h:68
Abstract_robot::robot_root_bounds_
std::vector< tf2::Transform > robot_root_bounds_
Bounds of robot arm as sub-region of table top.
Definition: impl/abstract_robot.h:32


multi_cell_builder
Author(s): MA
autogenerated on Thu Jan 12 2023 23:45:43