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


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