robot.cpp
Go to the documentation of this file.
1 #include "impl/robot.h"
2 
4  for (int i = 0; i <= 2; i++){
5  for (int j = 0; j <= 2; j++){
6  if(i == 0 && j == 0) {continue;}
7  if(i == 2 && j == 2) {continue;}
8  if(i == 0) {
9 
10  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.8f*j,0))));
11  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.8f*j,0))));
12  } else if (j == 0){
13  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,0,0))));
14  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,0,0))));
15  } else {
16  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,0.8f*j,0))));
17  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,0.8f*j,0))));
18  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,-0.8f*j,0))));
19  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,-0.8f*j,0))));
20  }
21 
22  };
23  };
24  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1.305f,0,0))));
25  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-1.305f,0,0))));
26  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,1.305f,0))));
27  access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-1.305f,0))));
28 
29 }
30 
31 float Robot::area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C){
32  return std::abs(
33  (B.getOrigin().getX() * A.getOrigin().getY()) - (A.getOrigin().getX() * B.getOrigin().getY()) +
34  (C.getOrigin().getX() * B.getOrigin().getY()) - (B.getOrigin().getX() * C.getOrigin().getY()) +
35  (A.getOrigin().getX() * C.getOrigin().getY()) - (C.getOrigin().getX() * A.getOrigin().getY()))*0.5f;
36 }
37 
38 void Robot::workload_checker(std::vector<int>& count_vector, tf2::Transform& obj){
39  for (int i = 0; i < observers_.size(); i++){
41  Wing* ceti = dynamic_cast<Wing*>(deco->wing());
42  tf2::Transform A = ceti->world_tf() * ceti->bounds()[0];
43  tf2::Transform B = ceti->world_tf() * ceti->bounds()[1];
44  tf2::Transform C = ceti->world_tf() * ceti->bounds()[2];
45  tf2::Transform D = ceti->world_tf() * ceti->bounds()[3];
46 
47  float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
48  float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
49 
50  if ((std::floor(sum*100)/100.f) <= full_area) {
51  count_vector[i]++; return;
52  } else {
53  continue;
54  }
55  }
56 
57 
58  tf2::Transform A = tf_ * bounds_[0];
59  tf2::Transform B = tf_ * bounds_[1];
60  tf2::Transform C = tf_ * bounds_[2];
61  tf2::Transform D = tf_ * bounds_[3];
62 
63  float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
64  float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
65  if ((std::floor(sum*100)/100.f) <= full_area) count_vector.back()++; return;
66 }
67 
68 
69 
70 bool Robot::check_single_object_collision(tf2::Transform& obj, std::string& str){
71 
72  tf2::Transform A = tf_ * root_tf_ * robot_root_bounds_[0];
73  tf2::Transform B = tf_ * root_tf_ * robot_root_bounds_[1];
74  tf2::Transform C = tf_ * root_tf_ * robot_root_bounds_[2];
75  tf2::Transform D = tf_ * root_tf_ * robot_root_bounds_[3];
76 
77  float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
78  float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
79  if ((std::floor(sum*100)/100.f) <= full_area) {return false; }
80 
81  std::stringstream ss;
82  ss << "base_" << name_.back();
83  A = tf_ * bounds_[0];
84  B = tf_ * bounds_[1];
85  C = tf_ * bounds_[2];
86  D = tf_ * bounds_[3];
87 
88  full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
89  sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
90  if ((std::floor(sum*100)/100.f) <= full_area){
91  str = ss.str();
92  return true;
93  }
94 
95  if (!observers_.empty()){
96  std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();
97  while (it != observers_.end()) {
99  Wing* ceti = dynamic_cast<Wing*>(deco->wing());
100  tf2::Transform A = ceti->world_tf() * ceti->bounds()[0];
101  tf2::Transform B = ceti->world_tf() * ceti->bounds()[1];
102  tf2::Transform C = ceti->world_tf() * ceti->bounds()[2];
103  tf2::Transform D = ceti->world_tf() * ceti->bounds()[3];
104 
105  full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
106  sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
107  if ((std::floor(sum*100)/100.f) <= full_area) {
108  str = ceti->name();
109  return true;
110  } else {
111  ++it;
112  }
113  }
114  }
115  return false;
116 }
117 
119  observers_.clear();
120  access_fields_.clear();
122  tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0, tf().getOrigin().getZ()));
123 }
124 
126  for(Abstract_robot_element* wing : observers_) wing->update(tf_);
127  for(Abstract_robot_element* field : access_fields_) field->update(tf_);
128 }
129 
131  tf2::Transform A = tf_ * bounds_[0];
132  tf2::Transform B = tf_ * bounds_[1];
133  tf2::Transform C = tf_ * bounds_[2];
134  tf2::Transform D = tf_ * bounds_[3];
135 
136  float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
137  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);
138  if ((std::floor(sum*100)/100.f) <= full_area) return true;
139 
140  for(Abstract_robot_element* are : rob->observers()) {
141  Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(are);
142  Wing* w = dynamic_cast<Wing*>(wrd->wing());
143 
144  tf2::Transform WA = w->world_tf() * w->bounds()[0];
145  tf2::Transform WB = w->world_tf() * w->bounds()[1];
146  tf2::Transform WC = w->world_tf() * w->bounds()[2];
147  tf2::Transform WD = w->world_tf() * w->bounds()[3];
148  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;
149  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;
150  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;
151  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;
152  }
153 
155  Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(ar);
156  Wing* w = dynamic_cast<Wing*>(wrd->wing());
157 
158  tf2::Transform WA = w->world_tf() * w->bounds()[0];
159  tf2::Transform WB = w->world_tf() * w->bounds()[1];
160  tf2::Transform WC = w->world_tf() * w->bounds()[2];
161  tf2::Transform WD = w->world_tf() * w->bounds()[3];
162 
163  full_area = area_calculation(WA,WB,WC) + area_calculation(WA,WD,WC);
164  for(Abstract_robot_element* are : rob->observers()) {
165  Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(are);
166  Wing* w = dynamic_cast<Wing*>(wrd->wing());
167 
168  tf2::Transform WA2 = w->world_tf() * w->bounds()[0];
169  tf2::Transform WB2 = w->world_tf() * w->bounds()[1];
170  tf2::Transform WC2 = w->world_tf() * w->bounds()[2];
171  tf2::Transform WD2 = w->world_tf() * w->bounds()[3];
172  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;
173  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;
174  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;
175  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;
176  }
177  }
178 }
179 
180 
181 
184  observers_.push_back(wd);
185  std::vector<tf2::Transform> plane;
186  bool found = false;
187  int index = 0;
188 
189  if (decorator->wing()->relative_tf().getOrigin().getY()>0){
190  for(long unsigned int i = 0; i < access_fields_.size(); i++){
191  if ((access_fields_[i]->relative_tf().getOrigin().getY() > 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
192  found = true;
193  index = i;
194  break;
195  }
196  }
197  if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
198  } else if (decorator->wing()->relative_tf().getOrigin().getY()<0) {
199  for(long unsigned int i = 0; i < access_fields_.size(); i++){
200  if ((access_fields_[i]->relative_tf().getOrigin().getY() < 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
201  found = true;
202  index = i;
203  break;
204  }
205  }
206  if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
207  } else {
208  for(long unsigned int i = 0; i < access_fields_.size(); i++){
209  if (access_fields_[i]->relative_tf().getOrigin().getX() > 0){
210  found = true;
211  index = i;
212  break;
213  }
214  }
215  if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
216  }
217 };
Wing::bounds
std::vector< tf2::Transform > & bounds()
Definition: wing.h:28
robot.h
Robot::observers
std::vector< Abstract_robot_element * > & observers()
Definition: robot.h:29
Abstract_robot::root_tf_
tf2::Transform root_tf_
Robot root on table top.
Definition: impl/abstract_robot.h:30
Wing::name
std::string & name()
Definition: wing.h:23
Robot::access_fields_
std::vector< Abstract_robot_element * > access_fields_
Definition: robot.h:22
Robot::notify
void notify() override
Definition: robot.cpp:125
Robot::reset
void reset()
Definition: robot.cpp:118
Abstract_robot_element_decorator
Definition: impl/abstract_robot_element_decorator.h:7
Robot::generate_access_fields
void generate_access_fields()
Definition: robot.cpp:3
Abstract_robot::bounds_
std::vector< tf2::Transform > bounds_
Bounds of table top surface.
Definition: impl/abstract_robot.h:31
Wing_rviz_decorator
Definition: wing_rviz_decorator.h:9
Abstract_robot::tf_
tf2::Transform tf_
Pose of table.
Definition: impl/abstract_robot.h:29
Abstract_robot_element_decorator::wing
Abstract_robot_element * wing()
Definition: impl/abstract_robot_element_decorator.h:15
Robot::workload_checker
void workload_checker(std::vector< int > &count_vector, tf2::Transform &obj) override
Definition: robot.cpp:38
Robot::area_calculation
float area_calculation(tf2::Transform &A, tf2::Transform &B, tf2::Transform &C)
Definition: robot.cpp:31
Robot
Definition: robot.h:19
Abstract_robot_element
Definition: impl/abstract_robot_element.h:9
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
Robot::observers_
std::vector< Abstract_robot_element * > observers_
Definition: robot.h:21
Abstract_robot_element::relative_tf
tf2::Transform & relative_tf()
Definition: impl/abstract_robot_element.h:18
Abstract_robot_element::world_tf
tf2::Transform & world_tf()
Definition: impl/abstract_robot_element.h:21
Robot::check_single_object_collision
bool check_single_object_collision(tf2::Transform &obj, std::string &str) override
Definition: robot.cpp:70
Field
Definition: impl/field.h:7
Wing
Definition: wing.h:7
Robot::register_observers
void register_observers(Abstract_robot_element *wd)
Definition: robot.cpp:182
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
Robot::check_robot_collision
bool check_robot_collision(Robot *R)
Definition: robot.cpp:130


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