ceti_robot.cpp
Go to the documentation of this file.
1 #include "robot/ceti_robot.h"
2 
3 Ceti_robot::Ceti_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size)
4 : Abstract_robot(name, tf, size)
5 , observer_mask_(std::bitset<3>(0))
6 {
8 }
9 
10 
11 
13  for (int i = 0; i <= 2; i++){
14  for (int j = 0; j <= 2; j++){
15  if(i == 0 && j == 0) {continue;}
16  if(i == 2 && j == 2) {continue;}
17  if(i == 0) {
18  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,size_.getY()*j,0))));
19  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-size_.getY()*j,0))));
20  } else if (j == 0){
21  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()*i,0,0))));
22  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-size_.getX()*i,0,0))));
23  } else {
24  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()*i,size_.getY()*j,0))));
25  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-size_.getX()*i,size_.getY()*j,0))));
26  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()*i,-size_.getY()*j,0))));
27  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-size_.getX()*i,-size_.getY()*j,0))));
28  }
29 
30  };
31  };
32  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1.305f,0,0))));
33  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-1.305f,0,0))));
34  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,1.305f,0))));
35  access_fields_.push_back(std::make_unique<Field>(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-1.305f,0))));
36 }
37 
38 void Ceti_robot::workload_checker(std::vector<int>& count_vector, tf2::Transform& obj){
39  for (int i = 0; i < observers_.size(); i++){
40  tf2::Transform A = observers_[i]->world_tf() * observers_[i]->bounds()[0];
41  tf2::Transform B = observers_[i]->world_tf() * observers_[i]->bounds()[1];
42  tf2::Transform C = observers_[i]->world_tf() * observers_[i]->bounds()[2];
43  tf2::Transform D = observers_[i]->world_tf() * observers_[i]->bounds()[3];
44 
45  float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
46  float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
47 
48  if ((std::floor(sum*100)/100.f) <= full_area) {
49  count_vector[i]++; return;
50  } else {
51  continue;
52  }
53  }
54 
55  tf2::Transform A = tf_ * bounds_[0];
56  tf2::Transform B = tf_ * bounds_[1];
57  tf2::Transform C = tf_ * bounds_[2];
58  tf2::Transform D = tf_ * bounds_[3];
59 
60  float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
61  float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
62  if ((std::floor(sum*100)/100.f) <= full_area) count_vector.back()++; return;
63 }
64 
65 bool Ceti_robot::check_single_object_collision(tf2::Transform& obj, std::string& str){
66 
67  tf2::Transform A = tf_ * root_tf_ * robot_root_bounds_[0];
68  tf2::Transform B = tf_ * root_tf_ * robot_root_bounds_[1];
69  tf2::Transform C = tf_ * root_tf_ * robot_root_bounds_[2];
70  tf2::Transform D = tf_ * root_tf_ * robot_root_bounds_[3];
71 
72  float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
73  float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
74  if ((std::floor(sum*100)/100.f) <= full_area) {return false; }
75 
76  std::stringstream ss;
77  ss << "base_" << name_.back();
78  A = tf_ * bounds_[0];
79  B = tf_ * bounds_[1];
80  C = tf_ * bounds_[2];
81  D = tf_ * bounds_[3];
82 
83  full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
84  sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
85  if ((std::floor(sum*100)/100.f) <= full_area){
86  str = ss.str();
87  return true;
88  }
89 
90  for (int i = 0; i < observers_.size(); i++){
91  tf2::Transform A = observers_[i]->world_tf() * observers_[i]->bounds()[0];
92  tf2::Transform B = observers_[i]->world_tf() * observers_[i]->bounds()[1];
93  tf2::Transform C = observers_[i]->world_tf() * observers_[i]->bounds()[2];
94  tf2::Transform D = observers_[i]->world_tf() * observers_[i]->bounds()[3];
95 
96  full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
97  sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
98  if ((std::floor(sum*100)/100.f) <= full_area) {
99  str = observers_[i]->name();
100  return true;
101  }
102  }
103  return false;
104 }
105 
107  observers_.clear();
108  access_fields_.clear();
110  tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0, tf().getOrigin().getZ()));
111 }
112 
114  for(int i = 0; i < observers_.size(); i++) observers_[i]->update(tf_);
115  for(int i = 0; i < access_fields_.size(); i++) access_fields_[i]->update(tf_);
116 }
117 
119  tf2::Transform A = tf_ * bounds_[0];
120  tf2::Transform B = tf_ * bounds_[1];
121  tf2::Transform C = tf_ * bounds_[2];
122  tf2::Transform D = tf_ * bounds_[3];
123 
124  float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
125  float sum = area_calculation(A,rob->tf(),D) + area_calculation(D,rob->tf(),C) + area_calculation(C,rob->tf(),B) + area_calculation(rob->tf(), B, A);
126  if ((std::floor(sum*100)/100.f) <= full_area) return true;
127 
128  for(int i = 0; i < rob->observers().size(); i++) {
129  tf2::Transform WA = rob->observers()[i]->world_tf() * rob->observers()[i]->bounds()[0];
130  tf2::Transform WB = rob->observers()[i]->world_tf() * rob->observers()[i]->bounds()[1];
131  tf2::Transform WC = rob->observers()[i]->world_tf() * rob->observers()[i]->bounds()[2];
132  tf2::Transform WD = rob->observers()[i]->world_tf() * rob->observers()[i]->bounds()[3];
133  sum = area_calculation(A,WA,D) + area_calculation(D,WA,C) + area_calculation(C,WA,B) + area_calculation(WA, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
134  sum = area_calculation(A,WB,D) + area_calculation(D,WB,C) + area_calculation(C,WB,B) + area_calculation(WB, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
135  sum = area_calculation(A,WC,D) + area_calculation(D,WC,C) + area_calculation(C,WC,B) + area_calculation(WC, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
136  sum = area_calculation(A,WD,D) + area_calculation(D,WD,C) + area_calculation(C,WD,B) + area_calculation(WD, B, A); if ((std::floor(sum*100)/100.f) <= full_area) return true;
137  }
138 
139  for (int i = 0; i < observers_.size(); i++){
140  tf2::Transform WA = observers_[i]->world_tf() * observers_[i]->bounds()[0];
141  tf2::Transform WB = observers_[i]->world_tf() * observers_[i]->bounds()[1];
142  tf2::Transform WC = observers_[i]->world_tf() * observers_[i]->bounds()[2];
143  tf2::Transform WD = observers_[i]->world_tf() * observers_[i]->bounds()[3];
144 
145  full_area = area_calculation(WA,WB,WC) + area_calculation(WA,WD,WC);
146  for(int j = 0; j < rob->observers().size(); j++) {
147  tf2::Transform WA2 = rob->observers()[j]->world_tf() * observers_[j]->bounds()[0];
148  tf2::Transform WB2 = rob->observers()[j]->world_tf() * observers_[j]->bounds()[1];
149  tf2::Transform WC2 = rob->observers()[j]->world_tf() * observers_[j]->bounds()[2];
150  tf2::Transform WD2 = rob->observers()[j]->world_tf() * observers_[j]->bounds()[3];
151  sum = area_calculation(WA,WA2,WD) + area_calculation(WD,WA2,WC) + area_calculation(WC,WA2,WB) + area_calculation(WA2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
152  sum = area_calculation(WA,WB2,WD) + area_calculation(WD,WB2,WC) + area_calculation(WC,WB2,WB) + area_calculation(WB2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
153  sum = area_calculation(WA,WC2,WD) + area_calculation(WD,WC2,WC) + area_calculation(WC,WC2,WB) + area_calculation(WC2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
154  sum = area_calculation(WA,WD2,WD) + area_calculation(WD,WD2,WC) + area_calculation(WC,WD2,WB) + area_calculation(WD2, WB, WA); if ((std::floor(sum*100)/100.f) <= full_area) return true;
155  }
156  }
157  return false;
158 }
159 
160 
161 
162 void Ceti_robot::register_observers(std::unique_ptr<Abstract_robot_element> wd){
163  /*
164  auto deco = dynamic_cast<Abstract_robot_element_decorator*>(wd.get());
165  bool found = false;
166  int index = 0;
167 
168  if (deco->wing()->relative_tf().getOrigin().getY()>0){
169  for(long unsigned int i = 0; i < access_fields_.size(); i++){
170  if ((access_fields_[i]->relative_tf().getOrigin().getY() > 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
171  found = true;
172  index = i;
173  break;
174  }
175  }
176  if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
177  } else if (deco->wing()->relative_tf().getOrigin().getY()<0) {
178  for(long unsigned int i = 0; i < access_fields_.size(); i++){
179  if ((access_fields_[i]->relative_tf().getOrigin().getY() < 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
180  found = true;
181  index = i;
182  break;
183  }
184  }
185  if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
186  } else {
187  for(long unsigned int i = 0; i < access_fields_.size(); i++){
188  if (access_fields_[i]->relative_tf().getOrigin().getX() > 0){
189  found = true;
190  index = i;
191  break;
192  }
193  }
194  if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
195  }
196  observers_.push_back(std::move(wd));
197  */
198 };
Ceti_robot::notify
void notify() override
Observer pattern.
Definition: ceti_robot.cpp:113
Ceti_robot::generate_access_fields
void generate_access_fields()
Generate access fields as surrounding poses for possible robots.
Definition: ceti_robot.cpp:12
Abstract_robot::root_tf_
tf2::Transform root_tf_
Robot root on table top.
Definition: impl/abstract_robot.h:30
Abstract_robot::area_calculation
float area_calculation(tf2::Transform &A, tf2::Transform &B, tf2::Transform &C)
Triangle area calculator.
Definition: abstract_robot.cpp:22
Ceti_robot::tf
tf2::Transform & tf() override
Definition: ceti_robot.h:43
Ceti_robot::register_observers
void register_observers(std::unique_ptr< Abstract_robot_element > wd)
Register observers in vector.
Definition: ceti_robot.cpp:162
ceti_robot.h
Ceti_robot::size
tf2::Vector3 & size() override
Definition: ceti_robot.h:44
Ceti_robot::access_fields_
std::vector< std::unique_ptr< Abstract_robot_element > > access_fields_
Serrounding Fields shared pointers.
Definition: ceti_robot.h:25
Abstract_robot::bounds_
std::vector< tf2::Transform > bounds_
Bounds of table top surface.
Definition: impl/abstract_robot.h:31
Ceti_robot::in_collision
bool in_collision(Ceti_robot *R)
Collsion calculation.
Definition: ceti_robot.cpp:118
Abstract_robot::tf_
tf2::Transform tf_
Pose of table.
Definition: impl/abstract_robot.h:29
Ceti_robot::observers
std::vector< std::unique_ptr< Abstract_robot_element > > & observers()
Get observers.
Definition: ceti_robot.h:53
Ceti_robot::reset
void reset()
Resset all robot properties.
Definition: ceti_robot.cpp:106
Ceti_robot
Concrete Ceti-Robot.
Definition: ceti_robot.h:22
Ceti_robot::observers_
std::vector< std::unique_ptr< Abstract_robot_element > > observers_
Wing shared pointers.
Definition: ceti_robot.h:24
Abstract_robot::name_
std::string name_
Name of robot.
Definition: impl/abstract_robot.h:27
Abstract_robot::size_
tf2::Vector3 size_
Size of table.
Definition: impl/abstract_robot.h:28
Ceti_robot::check_single_object_collision
bool check_single_object_collision(tf2::Transform &obj, std::string &str) override
Collsion calculation for single objects.
Definition: ceti_robot.cpp:65
Ceti_robot::workload_checker
void workload_checker(std::vector< int > &count_vector, tf2::Transform &obj) override
Object counter.
Definition: ceti_robot.cpp:38
Abstract_robot
Definition: impl/abstract_robot.h:25
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
Ceti_robot::Ceti_robot
Ceti_robot(std::string &name, tf2::Transform tf, tf2::Vector3 size)
Ceti robot constructor.
Definition: ceti_robot.cpp:3


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