mediator.cpp
Go to the documentation of this file.
1 #include "impl/mediator.h"
2 
4 #include <tf2/LinearMath/Scalar.h>
5 
6 void Mediator::connect_robots(Abstract_robot* robot) {robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());}
7 
9  visualization_msgs::MarkerArray ma;
10  for (int i = 0; i < wings_.size(); i++){
11  Robot* r = dynamic_cast<Robot*>(robots_[i]);
12  for (int j = 0; j < wings_[i].size(); j++){
13  Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(wings_[i][j]);
14  Wing* w = dynamic_cast<Wing*>(wrd->wing());
15  visualization_msgs::Marker marker;
16  marker.header.frame_id = "map";
17  marker.header.stamp = ros::Time();
18  marker.ns = w->name();
19  marker.id = 1;
20  marker.type = visualization_msgs::Marker::CUBE;
21  marker.action = visualization_msgs::Marker::ADD;
22  marker.pose.position.x = w->world_tf().getOrigin().getX();
23  marker.pose.position.y = w->world_tf().getOrigin().getY();
24  marker.pose.position.z = w->world_tf().getOrigin().getZ();
25  marker.pose.orientation.x = w->world_tf().getRotation().getX();
26  marker.pose.orientation.y = w->world_tf().getRotation().getY();
27  marker.pose.orientation.z = w->world_tf().getRotation().getZ();
28  marker.pose.orientation.w = w->world_tf().getRotation().getW();
29  marker.scale.x = w->size().getX();
30  marker.scale.y = w->size().getY();
31  marker.scale.z = w->size().getZ();
32  marker.color.r = 1.0;
33  marker.color.g = 1.0;
34  marker.color.b = 1.0;
35  marker.color.a = 1.0;
36  ma.markers.push_back(marker);
37  }
38  }
39  //pub_->publish(ma);
40 }
41 
42 void Mediator::set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp){
43 
44  for (int i =0; i < wbp.size(); i++){
45  std::vector<Abstract_robot_element*> v;
46  for (int j =0; j < wbp[i].first.size(); j++){
47  Abstract_robot_element* are = new Wing_rviz_decorator( new Wing(wbp[i].first[j].name_, wbp[i].first[j].pose_,wbp[i].first[j].size_));
48  v.push_back(are);
49  }
50  wings_.push_back(v);
51  }
52 
53 }
54 
55 bool Mediator::check_collision( const int& robot){
56  bool succ = true;
57  std::vector<int> count_v;
58  Robot* r = dynamic_cast<Robot*>(robots_[robot]);
59  count_v.resize(r->observers().size()+1);
60  std::string str;
61 
62  visualization_msgs::MarkerArray ma;
63  for(long unsigned int j = 0; j < objects_[robot].size(); j++){
64  visualization_msgs::Marker marker;
65  marker.header.frame_id = "map";
66  marker.header.stamp = ros::Time();
67  marker.ns = "objects ";
68  marker.id = j;
69  marker.type = visualization_msgs::Marker::CUBE;
70  marker.action = visualization_msgs::Marker::ADD;
71  marker.pose.position.x = objects_[robot][j].getOrigin().getX();
72  marker.pose.position.y = objects_[robot][j].getOrigin().getY();
73  marker.pose.position.z = objects_[robot][j].getOrigin().getZ();
74  marker.pose.orientation.x = objects_[robot][j].getRotation().getX();
75  marker.pose.orientation.y = objects_[robot][j].getRotation().getY();
76  marker.pose.orientation.z = objects_[robot][j].getRotation().getZ();
77  marker.pose.orientation.w = objects_[robot][j].getRotation().getW();
78  marker.scale.x = box_size.getX();
79  marker.scale.y = box_size.getY();
80  marker.scale.z = box_size.getZ();
81  if(robots_[robot]->check_single_object_collision(objects_[robot][j], str)){
82  marker.color.r = 0;
83  marker.color.g = 1.0;
84  marker.color.b = 0;
85  marker.color.a = 1;
86  } else {
87  marker.color.r = 1;
88  marker.color.g = 0;
89  marker.color.b = 0;
90  marker.color.a = 1.0;
91  succ = false;
92  }
93  ma.markers.push_back(marker);
94  robots_[robot]->workload_checker(count_v, objects_[robot][j]);
95  }
96  pub_->publish(ma);
97 
98  for (int& i : count_v){
99  if(i == 0) {succ = false; break;}
100  }
101  return succ;
102 }
103 
104 
106  ROS_INFO("assigne result to first robot...");
107  std::vector<pcl::PointXYZ> grCenter = Abstract_mediator::generate_Ground(tf2::Vector3(0,0,0), 3.0f, 0.1f);
108  std::vector<std::vector<tf2::Transform>> filter_per_robot;
109 
110  pcl::octree::OctreePointCloudSearch< pcl::PointXYZ >first_cloud(0.4f);
111  first_cloud.setInputCloud(Abstract_mediator::vector_to_cloud(result_vector_[0]));
112  first_cloud.addPointsFromInputCloud();
113  std::vector<tf2::Transform> ground_per_robot;
114  for(pcl::PointXYZ& p : grCenter){
115  double min_x, min_y, min_z, max_x, max_y, max_z;
116  first_cloud.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
117  bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
118  if (isInBox && first_cloud.isVoxelOccupiedAtPoint(p)) {
119  std::vector< int > pointIdxVec;
120  if (first_cloud.voxelSearch(p, pointIdxVec)) ground_per_robot.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(p.x, p.y, p.z)));
121  }
122  }
123 
124  for(int i = 0; i < objects_.size(); i++){
125  if (i+1 < objects_.size()){
126  for (long unsigned int j = objects_[i].size()-1; j > 0; j--){
127  if(objects_[i][j].getOrigin().distance(objects_[i+1].back().getOrigin()) == 0) objects_[i+1].pop_back();
128  }
129  }
130  }
131 
132  calculate(ground_per_robot);
133 
134  //swap
135  Abstract_robot* ar = robots_[1];
136  robots_[1] = robots_[0];
137  robots_[0] = ar;
138 
139  calculate(ground_per_robot);
140 
141 
142 }
143 
144 void Mediator::calculate(std::vector<tf2::Transform>& ground_per_robot){
145  visualization_msgs::MarkerArray ma;
146  int r1 = 0;
147  Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]);
148  for(int j = 0; j <= 7; j++){
149  std::bitset<3> wing_config(j);
151 
152  visualization_msgs::Marker m;
153  m.header.frame_id = "map";
154  m.header.stamp = ros::Time();
155  m.ns = ceti1->name();
156  m.id = 1;
157  m.type = visualization_msgs::Marker::CUBE;
158  m.action = visualization_msgs::Marker::ADD;
159  m.pose.position.x = ceti1->tf().getOrigin().getX();
160  m.pose.position.y = ceti1->tf().getOrigin().getY();
161  m.pose.position.z = ceti1->tf().getOrigin().getZ();
162  m.pose.orientation.x = ceti1->tf().getRotation().getX();
163  m.pose.orientation.y = ceti1->tf().getRotation().getY();
164  m.pose.orientation.z = ceti1->tf().getRotation().getZ();
165  m.pose.orientation.w = ceti1->tf().getRotation().getW();
166  m.scale.x = ceti1->size().getX();
167  m.scale.y = ceti1->size().getY();
168  m.scale.z = ceti1->tf().getOrigin().getZ()*2;
169  m.color.r = 1.0;
170  m.color.g = 1.0;
171  m.color.b = 1.0;
172  m.color.a = 1.0;
173  ma.markers.push_back(m);
174  pub_->publish(ma);
175  ma.markers.clear();
176 
177  ros::Duration timer(0.10);
178 
179  for (long unsigned int i = 0; i < ground_per_robot.size(); i++){
180  ceti1->set_tf(ground_per_robot[i]);
181  // vis. robot 1
182 
183  for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
184  ceti1->rotate(0.0872665f);
185  ceti1->notify();
186 
187  m.action = visualization_msgs::Marker::MODIFY;
188  m.pose.position.x = ceti1->tf().getOrigin().getX();
189  m.pose.position.y = ceti1->tf().getOrigin().getY();
190  m.pose.position.z = ceti1->tf().getOrigin().getZ();
191  m.pose.orientation.x = ceti1->tf().getRotation().getX();
192  m.pose.orientation.y = ceti1->tf().getRotation().getY();
193  m.pose.orientation.z = ceti1->tf().getRotation().getZ();
194  m.pose.orientation.w = ceti1->tf().getRotation().getW();
195  ma.markers.push_back(m);
196 
197  for (Abstract_robot_element* are : ceti1->observers()){
198  Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(are);
199  ma.markers.push_back(*wrd->markers());
200  }
201 
202  pub_->publish(ma);
203  ma.markers.clear();
204 
205  if (check_collision(r1)) {
206  approximation(ceti1);
207  } else {
208  continue;
209  }
210 
211  //timer.sleep();
212  }
213  }
214  ceti1->reset();
215  m.action = visualization_msgs::Marker::DELETEALL;
216  ma.markers.push_back(m);
217  pub_->publish(ma);
218  ma.markers.clear();
219  }
220 }
221 
223  Robot* ceti1 = dynamic_cast<Robot*>(robots_[1]);
224  int r1 = 1;
225 
226  visualization_msgs::MarkerArray ma;
227 
228  for (int i = 0; i <= 7; i++){
229  std::bitset<3> wing_config(i);
231 
232  visualization_msgs::Marker m;
233  m.header.frame_id = "map";
234  m.header.stamp = ros::Time();
235  m.ns = ceti1->name();
236  m.id = 1;
237  m.type = visualization_msgs::Marker::CUBE;
238  m.action = visualization_msgs::Marker::ADD;
239  m.pose.position.x = ceti1->tf().getOrigin().getX();
240  m.pose.position.y = ceti1->tf().getOrigin().getY();
241  m.pose.position.z = ceti1->tf().getOrigin().getZ();
242  m.pose.orientation.x = ceti1->tf().getRotation().getX();
243  m.pose.orientation.y = ceti1->tf().getRotation().getY();
244  m.pose.orientation.z = ceti1->tf().getRotation().getZ();
245  m.pose.orientation.w = ceti1->tf().getRotation().getW();
246  m.scale.x = ceti1->size().getX();
247  m.scale.y = ceti1->size().getY();
248  m.scale.z = ceti1->tf().getOrigin().getZ()*2;
249  m.color.r = 1.0;
250  m.color.g = 1.0;
251  m.color.b = 1.0;
252  m.color.a = 1.0;
253  ma.markers.push_back(m);
254  pub_->publish(ma);
255  ma.markers.clear();
256 
257  for (Abstract_robot_element* fields : robot->access_fields()) {
258  ceti1->set_tf(fields->world_tf());
259  //ROS_INFO("field size %i", robot->access_fields().size());
260  for ( float p = 0; p < 2*M_PI; p += M_PI/2){
261  ceti1->rotate(M_PI/2);
262  ceti1->notify();
263 
264  m.action = visualization_msgs::Marker::MODIFY;
265  m.pose.position.x = ceti1->tf().getOrigin().getX();
266  m.pose.position.y = ceti1->tf().getOrigin().getY();
267  m.pose.position.z = ceti1->tf().getOrigin().getZ();
268  m.pose.orientation.x = ceti1->tf().getRotation().getX();
269  m.pose.orientation.y = ceti1->tf().getRotation().getY();
270  m.pose.orientation.z = ceti1->tf().getRotation().getZ();
271  m.pose.orientation.w = ceti1->tf().getRotation().getW();
272  ma.markers.push_back(m);
273 
274  for (Abstract_robot_element* are : ceti1->observers()){
275  Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(are);
276  ma.markers.push_back(*wrd->markers());
277  }
278 
279  pub_->publish(ma);
280  ma.markers.clear();
281  if (robot->check_robot_collision(ceti1)) continue;
282 
283  if (check_collision(r1)) {
284  ROS_INFO("penis");
285  write_file(robot, ceti1);
286  } else {
287  continue;
288  }
289 
290  }
291 
292  //relative_ground.push_back(pcl::PointXYZ(fields->world_tf().getOrigin().getX(), fields->world_tf().getOrigin().getY(), fields->world_tf().getOrigin().getZ()));
293  }
294  ceti1->reset();
295  m.action = visualization_msgs::Marker::DELETEALL;
296  ma.markers.push_back(m);
297  pub_->publish(ma);
298  ma.markers.clear();
299  }
300 }
301 
302 
304  std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/" + dirname_ + "/" + std::to_string(static_cast<int>(ros::Time::now().toNSec())) + ".yaml");
305  double r,p,y;
306  tf2::Matrix3x3 m(A->tf().getRotation());
307  m.getRPY(r,p,y);
308 
309 
310  float size_x = A->size().getX();
311  float size_y = A->size().getY();
312  float size_z = A->size().getZ();
313 
314  float pos_x = A->tf().getOrigin().getX();
315  float pos_y = A->tf().getOrigin().getY();
316  float pos_z = A->tf().getOrigin().getZ() *2 ;
317  float rot_x = A->tf().getRotation().getX();
318  float rot_y = A->tf().getRotation().getY();
319  float rot_z = A->tf().getRotation().getZ();
320  float rot_w = A->tf().getRotation().getW();
321 
322 
323 
324 
325 
326  std::stringstream ss;
327  ss << "{ 'objects' : [ \n";
328  ss << "{ 'id' : 'table" << A->name().back() << "_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
329  ss << "{ 'id' : 'table" << A->name().back() << "_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},\n";
330  ss << "{ 'id' : 'table" << A->name().back() << "_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
331  ss << "{ 'id' : 'table" << A->name().back() << "_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
332  ss << "{ 'id' : 'table" << A->name().back() << "_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
333  ss << "{ 'id' : 'table" << A->name().back() << "_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
334  ss << "{ 'id' : 'table" << A->name().back() << "_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
335  ss << "{ 'id' : 'table" << A->name().back() << "_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
336  ss << "{ 'id' : 'table" << A->name().back() << "_table_top', 'pos': { 'x': " << std::to_string(pos_x) << " , 'y': "<< std::to_string(pos_y) << " , 'z': "<< std::to_string(pos_z) << " },'size': { 'length': "<< std::to_string(size_x) << " ,'width': "<< std::to_string(size_y) << " ,'height': "<< std::to_string(size_z) << " },'orientation': { 'x': " << std::to_string(rot_x) << " , 'y': " << std::to_string(rot_y) << " , 'z': " << std::to_string(rot_z) << " , 'w': " << std::to_string(rot_w) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) << " , 'p': " << std::to_string(p) << " , 'y': " << std::to_string(y) << " } },\n";
337 
338  m.setRotation(B->tf().getRotation());
339  m.getRPY(r,p,y);
340 
341  size_x = B->size().getX();
342  size_y = B->size().getY();
343  size_z = B->size().getZ();
344 
345  pos_x = B->tf().getOrigin().getX();
346  pos_y = B->tf().getOrigin().getY();
347  pos_z = B->tf().getOrigin().getZ() *2 ;
348  rot_x = B->tf().getRotation().getX();
349  rot_y = B->tf().getRotation().getY();
350  rot_z = B->tf().getRotation().getZ();
351  rot_w = B->tf().getRotation().getW();
352 
353  ss << "{ 'id' : 'table" << B->name().back() << "_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
354  ss << "{ 'id' : 'table" << B->name().back() << "_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},\n";
355  ss << "{ 'id' : 'table" << B->name().back() << "_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
356  ss << "{ 'id' : 'table" << B->name().back() << "_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
357  ss << "{ 'id' : 'table" << B->name().back() << "_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
358  ss << "{ 'id' : 'table" << B->name().back() << "_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
359  ss << "{ 'id' : 'table" << B->name().back() << "_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
360  ss << "{ 'id' : 'table" << B->name().back() << "_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n";
361  ss << "{ 'id' : 'table" << B->name().back() << "_table_top', 'pos': { 'x': " << std::to_string(pos_x) << ", 'y': "<< std::to_string(pos_y) << ", 'z': "<< std::to_string(pos_z) << " },'size': { 'length': "<< std::to_string(size_x) << " ,'width': "<< std::to_string(size_y) << " ,'height': "<< std::to_string(size_z) << " },'orientation': { 'x': " << std::to_string(rot_x) << " , 'y': " << std::to_string(rot_y) << " , 'z': " << std::to_string(rot_z) << " , 'w': " << std::to_string(rot_w) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': " << std::to_string(r) << ", 'p': " << std::to_string(p) << ", 'y': " << std::to_string(y) << "}},\n";
362 
363 
364  for (Abstract_robot_element* ae : A->observers()){
365  Wing_rviz_decorator* rd = dynamic_cast<Wing_rviz_decorator*>(ae);
366  float x = rd->wing()->world_tf().getOrigin().getX();
367  float y = rd->wing()->world_tf().getOrigin().getY();
368  float z = rd->wing()->world_tf().getOrigin().getZ();
369  float qx = rd->wing()->world_tf().getRotation().getX();
370  float qy = rd->wing()->world_tf().getRotation().getY();
371  float qz = rd->wing()->world_tf().getRotation().getZ();
372  float qw = rd->wing()->world_tf().getRotation().getW();
373  Wing* w = dynamic_cast<Wing*>(rd->wing());
374 
375  float length = w->size().getX();
376  float width = w->size().getY();
377  float height = w->size().getZ();
378 
379  ss << "{ 'id': '" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z - 0.25*height) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n";
380  }
381 
382 
383  for (Abstract_robot_element* ae : B->observers()){
384  Wing_rviz_decorator* rd = dynamic_cast<Wing_rviz_decorator*>(ae);
385  float x = rd->wing()->world_tf().getOrigin().getX();
386  float y = rd->wing()->world_tf().getOrigin().getY();
387  float z = rd->wing()->world_tf().getOrigin().getZ();
388  float qx = rd->wing()->world_tf().getRotation().getX();
389  float qy = rd->wing()->world_tf().getRotation().getY();
390  float qz = rd->wing()->world_tf().getRotation().getZ();
391  float qw = rd->wing()->world_tf().getRotation().getW();
392  Wing* w = dynamic_cast<Wing*>(rd->wing());
393 
394  float length = w->size().getX();
395  float width = w->size().getY();
396  float height = w->size().getZ();
397 
398  ss << "{ 'id': '" << w->name() << "', 'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z - 0.25*height) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n";
399  }
400  tf2::Transform tf_arm = A->tf() * A->root_tf();
401  float arm_x = tf_arm.getOrigin().getX();
402  float arm_y = tf_arm.getOrigin().getY();
403  float arm_z = tf_arm.getOrigin().getZ();
404  float arm_qx = tf_arm.getRotation().getX();
405  float arm_qy = tf_arm.getRotation().getY();
406  float arm_qz = tf_arm.getRotation().getZ();
407  float arm_qw = tf_arm.getRotation().getW();
408 
409  ss << "{ 'id': 'arm" << A->name().back() << "','type': 'ARM','pos': { 'x': " << std::to_string(arm_x) << ", 'y': " << std::to_string(arm_y) << ", 'z': 0.89 },'size': { },'orientation': { 'x': " << std::to_string(arm_qx) <<", 'y': " << std::to_string(arm_qy) << ", 'z': " << std::to_string(arm_qz) << ", 'w': " << std::to_string(arm_qw) << " },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, \n";
410 
411  tf_arm = B->tf() * B->root_tf();
412  arm_x = tf_arm.getOrigin().getX();
413  arm_y = tf_arm.getOrigin().getY();
414  arm_z = tf_arm.getOrigin().getZ();
415  arm_qx = tf_arm.getRotation().getX();
416  arm_qy = tf_arm.getRotation().getY();
417  arm_qz = tf_arm.getRotation().getZ();
418  arm_qw = tf_arm.getRotation().getW();
419 
420  ss << "{ 'id': 'arm" << B->name().back() << "','type': 'ARM','pos': { 'x': " << std::to_string(arm_x) << ", 'y': " << std::to_string(arm_y) << ", 'z': 0.89 },'size': { },'orientation': { 'x': " << std::to_string(arm_qx) <<", 'y': " << std::to_string(arm_qy) << ", 'z': " << std::to_string(arm_qz) << ", 'w': " << std::to_string(arm_qw) << " },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } \n";
421 
422  ss << "]}";
423 
424  o << ss.str();
425  o.close();
426 
427 }
428 
429 void Mediator::build_wings(std::bitset<3>& wing, int& robot){
430  std::bitset<3> result = robots_[robot]->observer_mask() & wing;
431  Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);
432 
433  for (std::size_t i = 0; i < result.size(); i++){
434  if (result[i]){
435  ceti->register_observers(wings_[robot][i]);
436  }
437  }
438 
439  visualization_msgs::MarkerArray ma;
440  for (Abstract_robot_element* are : ceti->observers()){
441  Wing_rviz_decorator* wad = dynamic_cast<Wing_rviz_decorator*>(are);
442  wad->markers()->action = visualization_msgs::Marker::ADD;
443  ma.markers.push_back(*wad->markers());
444  }
445  pub_->publish(ma);
446 }
447 
449  visualization_msgs::MarkerArray ma;
450  visualization_msgs::Marker marker;
451  marker.header.frame_id = "map";
452  marker.header.stamp = ros::Time();
453  marker.ns = ar->name();
454  marker.id = 1;
455  marker.type = visualization_msgs::Marker::CUBE;
456  marker.action = visualization_msgs::Marker::MODIFY;
457  marker.pose.position.x = ar->tf().getOrigin().getX();
458  marker.pose.position.y = ar->tf().getOrigin().getY();
459  marker.pose.position.z = ar->tf().getOrigin().getZ();
460  marker.pose.orientation.x = ar->tf().getRotation().getX();
461  marker.pose.orientation.y = ar->tf().getRotation().getY();
462  marker.pose.orientation.z = ar->tf().getRotation().getZ();
463  marker.pose.orientation.w = ar->tf().getRotation().getW();
464  marker.scale.x = ar->size().getX();
465  marker.scale.y = ar->size().getY();
466  marker.scale.z = ar->size().getZ();
467  marker.color.r = 1.0;
468  marker.color.g = 1.0;
469  marker.color.b = 1.0;
470  marker.color.a = 1.0;
471  for (auto w : ar->observers()){
472  Wing_rviz_decorator* wrd = dynamic_cast<Wing_rviz_decorator*>(w);
473  ma.markers.push_back(*wrd->markers());
474  }
475 
476  //pub_->publish(ma);
477 }
Mediator::connect_robots
void connect_robots(Abstract_robot *robot) override
Definition: mediator.cpp:6
Abstract_mediator::pub_
ros::Publisher * pub_
Definition: impl/abstract_mediator.h:38
Robot::observers
std::vector< Abstract_robot_element * > & observers()
Definition: robot.h:29
Wing::name
std::string & name()
Definition: wing.h:23
Mediator::approximation
void approximation(Robot *robot)
Definition: mediator.cpp:222
Abstract_robot::rotate
void rotate(float deg)
Definition: impl/abstract_robot.h:65
Wing::size
tf2::Vector3 & size()
Definition: wing.h:27
Robot::notify
void notify() override
Definition: robot.cpp:125
Mediator::build_wings
void build_wings(std::bitset< 3 > &wings, int &robot) override
Definition: mediator.cpp:429
Abstract_robot::root_tf
tf2::Transform & root_tf()
Definition: impl/abstract_robot.h:60
Robot::reset
void reset()
Definition: robot.cpp:118
mediator.h
Abstract_mediator::wings_
std::vector< std::vector< Abstract_robot_element * > > wings_
Definition: impl/abstract_mediator.h:40
Abstract_robot::set_tf
void set_tf(tf2::Transform &t)
Definition: impl/abstract_robot.h:63
Abstract_mediator::vector_to_cloud
pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector< pcl::PointXYZ > &vector)
Definition: impl/abstract_mediator.cpp:16
Wing_rviz_decorator
Definition: wing_rviz_decorator.h:9
box_size
#define box_size
Definition: impl/abstract_mediator.h:21
Abstract_mediator::result_vector_
std::vector< std::vector< pcl::PointXYZ > > result_vector_
Definition: impl/abstract_mediator.h:39
Abstract_mediator::dirname_
std::string dirname_
Dirname of the reference protobuff.
Definition: impl/abstract_mediator.h:41
Abstract_robot_element_decorator::wing
Abstract_robot_element * wing()
Definition: impl/abstract_robot_element_decorator.h:15
Abstract_robot::name
std::string & name()
Definition: impl/abstract_robot.h:57
Abstract_robot::size
tf2::Vector3 & size()
Definition: impl/abstract_robot.h:59
Mediator::mediate
void mediate() override
Definition: mediator.cpp:105
Robot
Definition: robot.h:19
Abstract_robot_element
Definition: impl/abstract_robot_element.h:9
Wing_rviz_decorator::markers
visualization_msgs::Marker * markers()
Definition: wing_rviz_decorator.h:17
Abstract_robot::tf
tf2::Transform & tf()
Definition: impl/abstract_robot.h:58
Abstract_mediator::generate_Ground
std::vector< pcl::PointXYZ > generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution)
Definition: impl/abstract_mediator.cpp:4
Abstract_mediator::objects_
std::vector< std::vector< tf2::Transform > > objects_
Definition: impl/abstract_mediator.h:36
Mediator::write_file
void write_file(Robot *A, Robot *B)
Definition: mediator.cpp:303
Robot::access_fields
std::vector< Abstract_robot_element * > & access_fields()
Definition: robot.h:30
Mediator::set_wings
void set_wings(std::vector< std::pair< std::vector< object_data >, int >> &wbp) override
Definition: mediator.cpp:42
wing_rviz_decorator.h
Mediator::calculate
void calculate(std::vector< tf2::Transform > &ground_per_robot)
Definition: mediator.cpp:144
Mediator::setup_rviz
void setup_rviz()
Definition: mediator.cpp:8
Mediator::check_collision
bool check_collision(const int &robot) override
Definition: mediator.cpp:55
Abstract_mediator::robots_
std::vector< Abstract_robot * > robots_
Definition: impl/abstract_mediator.h:35
Abstract_robot_element::world_tf
tf2::Transform & world_tf()
Definition: impl/abstract_robot_element.h:21
Wing
Definition: wing.h:7
Robot::register_observers
void register_observers(Abstract_robot_element *wd)
Definition: robot.cpp:182
wing_config
wing_config
Definition: impl/abstract_robot.h:14
Abstract_robot
Definition: impl/abstract_robot.h:25
Robot::check_robot_collision
bool check_robot_collision(Robot *R)
Definition: robot.cpp:130
Mediator::publish
void publish(Robot *r)
Definition: mediator.cpp:448


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