base_calculation_mediator.cpp
Go to the documentation of this file.
2 
3 BaseCalculationMediator::BaseCalculationMediator(std::shared_ptr<ros::NodeHandle> const& nh)
4  : AbstractMediator(nh)
5  , pub_(std::make_unique<ros::Publisher>(nh->advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1))){
6  nh_->getParam("/resource_name", filename_);
7 
8  // create new folder
9  if(std::filesystem::exists(ros::package::getPath("multi_cell_builder") + "/results/" + filename_)) std::filesystem::remove_all(ros::package::getPath("multi_cell_builder") + "/results/" + filename_);
10  std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_);
11  };
12 
13 void BaseCalculationMediator::generateGrounds(const tf2::Vector3 origin, const float diameter, float resolution){
14  for(const auto& s_r : robots_){
15  std::vector<pcl::PointXYZ> ground_plane;
16  for (float x = origin.getX() - diameter * 1.5; x <= origin.getX() + diameter * 1.5; x += resolution){
17  for (float y = origin.getY() - diameter * 1.5; y <= origin.getY() + diameter * 1.5; y += resolution){
18  ground_plane.push_back(pcl::PointXYZ(x,y, s_r.second->tf().getOrigin().getZ() - 0.0025f));
19  }
20  }
21  grounds_.insert(std::pair<const std::string, std::vector<pcl::PointXYZ>>(s_r.first, ground_plane));
22  }
23 }
24 
25 void BaseCalculationMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) {
26  ROS_INFO("connecting %s...", robot->name().c_str());
27  robots_.insert_or_assign(robot->name(), std::move(robot));
28 }
29 
31  auto wd = wing_reader_->wingData();
32 
33  for (const auto& s_r : robots_){
34  try{
35  auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
36  ceti_bot->setObserverMask(static_cast<wing_config>(wd.at(s_r.first).second));
37  } catch(const std::out_of_range& oor) {
38  ROS_INFO("No mask data for %s", s_r.first.c_str());
39  }
40  }
41 
42  for (const auto& s_r : robots_){
43  auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
44  std::vector<std::unique_ptr<AbstractRobotElementDecorator>> panels(3);
45 
46  for (std::size_t j = 0; j < ceti_bot->observerMask().size(); j++){
47  if (ceti_bot->observerMask()[j]){
48  try{
49  tf2::Transform relative_tf = ceti_bot->tf().inverse() * wd.at(ceti_bot->name()).first[j].pose_;
50 
51  std::unique_ptr<AbstractRobotElement> panel = std::make_unique<RvizPanel>(wd.at(s_r.first).first[j].name_, relative_tf, wd.at(s_r.first).first[j].size_);
52  std::unique_ptr<AbstractRobotElementDecorator> log = std::make_unique<LogDecorator>(std::move(panel));
53  panels[j] = std::move(log);
54  } catch (std::out_of_range &oor){
55  ROS_INFO("OOR in set_panel");
56  }
57  }
58  }
59  ceti_bot->setObservers(panels);
60  }
61 }
62 
63 bool BaseCalculationMediator::checkCollision(const std::string& robot, std::bitset<3>& panel_mask, bool& workload, std::vector<object_data>& ts){
64 
65  // Visualization workspace definition
66  std::string str;
67  std::stringstream ns;
68  ns << "Targets " << robot;
69 
70  // Get Robot reference by name
71  CetiRobot* ceti1;
72  try{
73  ceti1 = dynamic_cast<CetiRobot*>(robots_.at(robot)->next());
74  } catch (std::out_of_range& oor){}
75 
76  // prepare map for workload check
77  std::map<std::string, int> workload_map;
78 
79  bool succ = true;
80 
81  visualization_msgs::MarkerArray ma;
82  for(int i = 0; i < ts.size(); i++){
83  visualization_msgs::Marker marker;
84  marker.header.frame_id = "map";
85  marker.header.stamp = ros::Time();
86  marker.ns = ns.str();
87  marker.id = i;
88  marker.type = visualization_msgs::Marker::CUBE;
89  marker.action = visualization_msgs::Marker::ADD;
90  marker.pose.position.x = ts[i].pose_.getOrigin().getX();
91  marker.pose.position.y = ts[i].pose_.getOrigin().getY();
92  marker.pose.position.z = ts[i].pose_.getOrigin().getZ();
93  marker.pose.orientation.x = ts[i].pose_.getRotation().getX();
94  marker.pose.orientation.y = ts[i].pose_.getRotation().getY();
95  marker.pose.orientation.z = ts[i].pose_.getRotation().getZ();
96  marker.pose.orientation.w = ts[i].pose_.getRotation().getW();
97  marker.scale.x = ts[i].size_.getX();
98  marker.scale.y = ts[i].size_.getY();
99  marker.scale.z = 0.1f;
100 
101  // ROS_INFO("%f, %f, %f", ts.at(robot)[i].pose_.getOrigin().getX(), ts.at(robot)[i].pose_.getOrigin().getY(), ts.at(robot)[i].pose_.getOrigin().getZ());
102  if(ceti1->checkSingleObjectCollision(ts[i].pose_, str, panel_mask)){
103  try {
104  workload_map.at(str)++;
105  } catch (std::out_of_range& oor) {
106  ROS_INFO("Support surface not in map");
107  workload_map.insert(std::pair<std::string,int>(str,1));
108  }
109  marker.color.r = 0;
110  marker.color.g = 1.0;
111  marker.color.b = 0;
112  marker.color.a = 1;
113  } else {
114  marker.color.r = 1;
115  marker.color.g = 0;
116  marker.color.b = 0;
117  marker.color.a = 1.0;
118  succ = false;
119  return false;
120  }
121  ma.markers.push_back(marker);
122  }
123 
124  pub_->publish(ma);
125  workload = (workload_map.size() <= panel_mask.count()+1) && succ;
126  return succ;
127 }
128 
129 
131  generateGrounds(tf2::Vector3(0,0,0), 3.0f, 0.1f);
132 
133  // estimate workcells
134  auto ts = task_space_reader_->dropOffData();
135  std::vector<std::vector<std::string>> wcs;
136  /*
137  for (auto it = ts.begin(); it != ts.end(); it++){
138  std::vector<std::string> wc = {it->first};
139 
140  auto itt = std::next(it);
141  for(auto& v1: it->second){
142  ROS_INFO("penis");
143  for (; itt != ts.end(); itt++){
144  for(auto& v2: itt->second){
145  if(tf2::tf2Distance2(v1.pose_.getOrigin(), v2.pose_.getOrigin())){
146  wc.push_back(itt->first);
147  break;
148  }
149  }
150  }
151  }
152  if(wc.size()>1) wcs.push_back(wc);
153  }
154  */
155 
156  /* wcs
157  [[panda_arm1, panda_arm2], [] ...]
158  */
159  wcs.push_back(std::vector<std::string>{"panda_arm1", "panda_arm2"});
160 
161  /*
162  [{panda_arm1: [xyz, xyz], panda_arm2: [xyz, xyz]}, {} ...]
163  */
164  std::vector<std::map<const std::string, std::vector<tf2::Transform>>> workcells_with_bases;
165 
166 
167  for(auto& workcell: wcs){
168  std::map<const std::string, std::vector<tf2::Transform>> ground_projection;
169  for (auto& robot: workcell){
170  std::vector<tf2::Transform> ground_per_robot;
171  pcl::octree::OctreePointCloudSearch< pcl::PointXYZ >cloud(0.4f);
172  try{
173  cloud.setInputCloud(AbstractMediator::vector2cloud(result_vector_.at(robot)));
174  cloud.addPointsFromInputCloud();
175 
176  for(pcl::PointXYZ& p : grounds_.at(robot)){
177  double min_x, min_y, min_z, max_x, max_y, max_z;
178  cloud.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
179  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);
180  if (isInBox && cloud.isVoxelOccupiedAtPoint(p)) {
181  std::vector< int > pointIdxVec;
182  if (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)));
183  }
184  }
185  } catch (std::out_of_range& oor) {
186  ROS_INFO("No result for robot");
187  ros::shutdown();
188  }
189  ground_projection.insert(std::pair<const std::string, std::vector<tf2::Transform>>(robot,ground_per_robot));
190  }
191  workcells_with_bases.push_back(ground_projection);
192  }
193 
194  ROS_INFO("assigne result to first robot...");
195  for(auto& wc: workcells_with_bases){
196  /*
197  calculate pose per wc {panda_arm1: [,,,], panda_arm2: [,,,]}
198  */
199  calculate(wc);
200  }
201 }
202 
203 void BaseCalculationMediator::calculate(std::map<const std::string, std::vector<tf2::Transform>>& workcell){
204  /*
205  Iterate through map, safe all possible solutions in [[(name, pose, config), (name, pose, config)], [....]]
206  */
207  std::vector<std::vector<protobuf_entry>> all_solutions_per_workcell;
208 
209  visualization_msgs::MarkerArray ma;
210  for (auto& s_pos : workcell){
211  CetiRobot* ceti1;
212  try{
213  ceti1 = dynamic_cast<CetiRobot*>(robots_.at(s_pos.first)->next());
214  } catch(std::out_of_range& oor){}
215 
216  visualization_msgs::Marker m;
217  m.header.frame_id = "map";
218  m.header.stamp = ros::Time();
219  m.ns = ceti1->name();
220  m.id = 1;
221  m.type = visualization_msgs::Marker::CUBE;
222  m.action = visualization_msgs::Marker::ADD;
223  m.pose.position.x = ceti1->tf().getOrigin().getX();
224  m.pose.position.y = ceti1->tf().getOrigin().getY();
225  m.pose.position.z = ceti1->tf().getOrigin().getZ();
226  m.pose.orientation.x = ceti1->tf().getRotation().getX();
227  m.pose.orientation.y = ceti1->tf().getRotation().getY();
228  m.pose.orientation.z = ceti1->tf().getRotation().getZ();
229  m.pose.orientation.w = ceti1->tf().getRotation().getW();
230 
231  m.scale.x = ceti1->size().getX();
232  m.scale.y = ceti1->size().getY();
233  m.scale.z = ceti1->tf().getOrigin().getZ()*2;
234  m.color.r = 1.0;
235  m.color.g = 1.0;
236  m.color.b = 1.0;
237  m.color.a = 1.0;
238 
239  for (std::size_t k = 0; k < ceti1->observerMask().size(); k++){
240  if(ceti1->observerMask()[k]){
241  auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next());
242  ma.markers.push_back(wrd->marker());
243  }
244  }
245 
246 
247  ma.markers.push_back(m);
248  pub_->publish(ma);
249  ma.markers.clear();
250 
251  for(int j = 5; j <= 7; j++){
252  std::bitset<3> wing_config(j);
253  for (long unsigned int i = 0; i < s_pos.second.size(); i++){
254  ceti1->setTf(s_pos.second[i]);
255  for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
256  ceti1->rotate(0.0872665f);
257  ceti1->notify();
258  m.action = visualization_msgs::Marker::MODIFY;
259  m.pose.position.x = ceti1->tf().getOrigin().getX();
260  m.pose.position.y = ceti1->tf().getOrigin().getY();
261  m.pose.position.z = ceti1->tf().getOrigin().getZ();
262  m.pose.orientation.x = ceti1->tf().getRotation().getX();
263  m.pose.orientation.y = ceti1->tf().getRotation().getY();
264  m.pose.orientation.z = ceti1->tf().getRotation().getZ();
265  m.pose.orientation.w = ceti1->tf().getRotation().getW();
266  ma.markers.push_back(m);
267 
268  for (std::size_t k = 0; k < ceti1->observerMask().size(); k++){
269  if(ceti1->observerMask()[k] & wing_config[k]){
270  auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next());
271  wrd->marker().color.a = 1;
272  ma.markers.push_back(wrd->marker());
273  } else if (ceti1->observers()[k]){
274  auto wrd = dynamic_cast<RvizPanel*>(ceti1->observers()[k]->next());
275  wrd->marker().color.a = 0;
276  ma.markers.push_back(wrd->marker());
277  }
278  }
279  pub_->publish(ma);
280  ma.markers.clear();
281 
282  bool workload;
283  bool target_collisions = checkCollision(s_pos.first, wing_config, workload, task_space_reader_->dropOffData().at(ceti1->name()));
284 
285  if (workload && target_collisions){
286  std::vector<protobuf_entry> wc_solution;// std::vector<protobuff> is one workcell solution
287  wc_solution.push_back(protobuf_entry{ceti1->name(), ceti1->tf(), j});
288  approximation(workcell, wc_solution);
289  if (workcell.size() == wc_solution.size()){
290  all_solutions_per_workcell.push_back(wc_solution);
291  }
292  }
293  }
294  }
295  }
296  m.action = visualization_msgs::Marker::DELETEALL;
297  ma.markers.push_back(m);
298  pub_->publish(ma);
299  ma.markers.clear();
300  protobuf_.push_back(all_solutions_per_workcell);
301  }
302 
303 }
304 
305 void BaseCalculationMediator::approximation(std::map<const std::string, std::vector<tf2::Transform>>& workcell, std::vector<protobuf_entry>& wc_solution){
306  // build access fiels to existing wc_solution
307  std::vector<tf2::Transform> access_map;
308  float padding = 0.0025f;
309 
310  // Choose next robot
311  CetiRobot* next_ceti;
312  if (!(workcell.size() == wc_solution.size())){
313  for (auto& r: workcell){
314  for (auto& s: wc_solution){
315  if (!(r.first == s.name_)){
316  next_ceti = dynamic_cast<CetiRobot*>(robots_.at(r.first)->next());
317  break;
318  }
319  }
320  }
321  } else {
322  return;
323  }
324 
325  // make task_space unique
326  auto ts = task_space_reader_->dropOffData().at(next_ceti->name());
327  ROS_INFO("%i", ts.size());
328  for (auto& r: wc_solution){
329  for(auto& vec: task_space_reader_->dropOffData().at(r.name_)){
330  for (auto it = ts.begin(); it != ts.end();){
331  if(tf2::tf2Distance2(it->pose_.getOrigin(), vec.pose_.getOrigin()) == 0) {
332  ts.erase(it);
333  } else {
334  ++it;
335  }
336  }
337  }
338  }
339 
340  for (auto& r : wc_solution){
341  CetiRobot* ceti = dynamic_cast<CetiRobot*>(robots_.at(r.name_)->next());
342  ceti->setTf(r.tf_);
343  ceti->notify();
344 
345  for (int i = 0; i <= 2; i++){
346  for (int j = 0; j <= 2; j++){
347  if(i == 0 && j == 0) {continue;}
348  if(i == 2 && j == 2) {continue;}
349  if(i == 0) {
350  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,ceti->size().getY()*j + padding,0)));
351  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-ceti->size().getY()*j + padding,0)));
352  } else if (j == 0){
353  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(ceti->size().getX()*i + padding,0,0)));
354  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-ceti->size().getX()*i + padding,0,0)));
355  } else {
356  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(ceti->size().getX()*i+padding,ceti->size().getY()*j +padding,0)));
357  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-ceti->size().getX()*i+padding,ceti->size().getY()*j +padding,0)));
358  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(ceti->size().getX()*i+padding,-ceti->size().getY()*j +padding,0)));
359  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-ceti->size().getX()*i+padding,-ceti->size().getY()*j+padding,0)));
360  }
361 
362  };
363  };
364 
365  std::bitset<3> actual_config(r.wing_config_);
366  for(std::size_t i = 0; i < ceti->observers().size(); i++){
367  if(actual_config[i]){
368  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.5f * ceti->size().getX() + ceti->observers()[i]->size().getX() + 2*padding + next_ceti->size().getX()*0.5f,0,0)));
369  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.5f * ceti->size().getX() - ceti->observers()[i]->size().getX() - 2*padding - next_ceti->size().getX()*0.5f,0,0)));
370  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.5f*ceti->size().getY() + ceti->observers()[i]->size().getY() + 2*padding + next_ceti->size().getY()*0.5f ,0)));
371  access_map.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.5f*ceti->size().getY() - ceti->observers()[i]->size().getY() - 2*padding - next_ceti->size().getY()*0.5f,0)));
372  }
373  }
374 
375  for(auto& am: access_map){
376  am = r.tf_ * am;
377  }
378 
379  }
380 
381  visualization_msgs::MarkerArray ma;
382  visualization_msgs::Marker m;
383  m.header.frame_id = "map";
384  m.header.stamp = ros::Time();
385  m.ns = next_ceti->name();
386  m.id = 1;
387  m.type = visualization_msgs::Marker::CUBE;
388  m.action = visualization_msgs::Marker::ADD;
389  m.pose.position.x = next_ceti->tf().getOrigin().getX();
390  m.pose.position.y = next_ceti->tf().getOrigin().getY();
391  m.pose.position.z = next_ceti->tf().getOrigin().getZ();
392  m.pose.orientation.x = next_ceti->tf().getRotation().getX();
393  m.pose.orientation.y = next_ceti->tf().getRotation().getY();
394  m.pose.orientation.z = next_ceti->tf().getRotation().getZ();
395  m.pose.orientation.w = next_ceti->tf().getRotation().getW();
396 
397  m.scale.x = next_ceti->size().getX();
398  m.scale.y = next_ceti->size().getY();
399  m.scale.z = next_ceti->tf().getOrigin().getZ()*2;
400  m.color.r = 1.0;
401  m.color.g = 1.0;
402  m.color.b = 1.0;
403  m.color.a = 1.0;
404 
405  for (std::size_t k = 0; k < next_ceti->observerMask().size(); k++){
406  if(next_ceti->observerMask()[k]){
407  auto wrd = dynamic_cast<RvizPanel*>(next_ceti->observers()[k]->next());
408  ma.markers.push_back(wrd->marker());
409  }
410  }
411 
412  ma.markers.push_back(m);
413  pub_->publish(ma);
414  ma.markers.clear();
415 
416  for(int j = 0; j <= 7; j++){
417  std::bitset<3> wing_config(j);
418  for (long unsigned int i = 0; i < access_map.size(); i++){
419  next_ceti->setTf(access_map[i]);
420  for ( float p = 0; p < 2*M_PI; p += M_PI/2){
421  next_ceti->rotate(M_PI/2);
422  next_ceti->notify();
423  m.action = visualization_msgs::Marker::MODIFY;
424  m.pose.position.x = next_ceti->tf().getOrigin().getX();
425  m.pose.position.y = next_ceti->tf().getOrigin().getY();
426  m.pose.position.z = next_ceti->tf().getOrigin().getZ();
427  m.pose.orientation.x = next_ceti->tf().getRotation().getX();
428  m.pose.orientation.y = next_ceti->tf().getRotation().getY();
429  m.pose.orientation.z = next_ceti->tf().getRotation().getZ();
430  m.pose.orientation.w = next_ceti->tf().getRotation().getW();
431  ma.markers.push_back(m);
432 
433  for (std::size_t k = 0; k < next_ceti->observerMask().size(); k++){
434  if(next_ceti->observerMask()[k] & wing_config[k]){
435  auto wrd = dynamic_cast<RvizPanel*>(next_ceti->observers()[k]->next());
436  wrd->marker().color.a = 1;
437  ma.markers.push_back(wrd->marker());
438  } else if (next_ceti->observers()[k]){
439  auto wrd = dynamic_cast<RvizPanel*>(next_ceti->observers()[k]->next());
440  wrd->marker().color.a = 0;
441  ma.markers.push_back(wrd->marker());
442  }
443  }
444  pub_->publish(ma);
445  ma.markers.clear();
446 
447  bool workload;
448  bool target_collisions = checkCollision(next_ceti->name(), wing_config, workload, ts);
449  if(workload && target_collisions) {
450  wc_solution.push_back({next_ceti->name(), next_ceti->tf(), j});
451  if (sceneCollision(wc_solution)) {
452  wc_solution.pop_back();
453  } else {
454  if ( workcell.size() == wc_solution.size()) {
455  writeFile(wc_solution);
456  //ros::shutdown();
457  }
458  }
459  }
460  }
461  }
462  }
463  m.action = visualization_msgs::Marker::DELETEALL;
464  ma.markers.push_back(m);
465  pub_->publish(ma);
466  ma.markers.clear();
467 }
468 
469 bool BaseCalculationMediator::sceneCollision(std::vector<protobuf_entry>& wc_solution){
470  for(auto& r1 : wc_solution){
471  std::vector<tf2::Transform> bounds;
472  for(auto& r2 : wc_solution){
473  if (r1.name_ == r2.name_) continue;
474  auto robot2 = dynamic_cast<CetiRobot*>(robots_.at(r2.name_)->next());
475  bounds.push_back(r2.tf_);
476 
477  robot2->setTf(r2.tf_);
478  robot2->notify();
479 
480  bounds.push_back(r2.tf_ * robot2->rootTf() * robot2->robotRootBounds()[0]);
481  bounds.push_back(r2.tf_ * robot2->rootTf() * robot2->robotRootBounds()[1]);
482  bounds.push_back(r2.tf_ * robot2->rootTf() * robot2->robotRootBounds()[2]);
483  bounds.push_back(r2.tf_ * robot2->rootTf() * robot2->robotRootBounds()[3]);
484 
485  bounds.push_back(r2.tf_ * robot2->bounds()[0]);
486  bounds.push_back(r2.tf_ * robot2->bounds()[1]);
487  bounds.push_back(r2.tf_ * robot2->bounds()[2]);
488  bounds.push_back(r2.tf_ * robot2->bounds()[3]);
489 
490  std::bitset<3> panel_mask(r2.wing_config_);
491  for(std::size_t i = 0; i < robot2->observers().size(); i++){
492  if(robot2->observerMask()[i] & panel_mask[i]){
493  bounds.push_back(robot2->observers()[i]->worldTf());
494  bounds.push_back(robot2->observers()[i]->worldTf() * robot2->observers()[i]->bounds()[0]);
495  bounds.push_back(robot2->observers()[i]->worldTf() * robot2->observers()[i]->bounds()[1]);
496  bounds.push_back(robot2->observers()[i]->worldTf() * robot2->observers()[i]->bounds()[2]);
497  bounds.push_back(robot2->observers()[i]->worldTf() * robot2->observers()[i]->bounds()[3]);
498  }
499  }
500  }
501 
502  auto robot1 = dynamic_cast<CetiRobot*>(robots_.at(r1.name_)->next());
503  robot1->setTf(r1.tf_);
504  robot1->notify();
505 
506  std::bitset<3> panel_mask(r1.wing_config_);
507  std::string str;
508  for(int i = 0; i < bounds.size(); i++){
509  if (robot1->checkSingleObjectCollision(bounds[i], str, panel_mask)) {
510  /*
511  ROS_INFO("%s, %s, %f, %f, %f", str.c_str(),robot1->name().c_str(), bounds[i].getOrigin().getX(), bounds[i].getOrigin().getY(), bounds[i].getOrigin().getZ());
512  visualization_msgs::MarkerArray ma;
513  visualization_msgs::Marker m;
514  m.header.frame_id = "map";
515  m.header.stamp = ros::Time();
516  m.ns = "point" +i;
517  m.id = 1;
518  m.type = visualization_msgs::Marker::CUBE;
519  m.action = visualization_msgs::Marker::ADD;
520  m.pose.position.x = bounds[i].getOrigin().getX();
521  m.pose.position.y = bounds[i].getOrigin().getY();
522  m.pose.position.z = bounds[i].getOrigin().getZ();
523  m.pose.orientation.x = 0;
524  m.pose.orientation.y = 0;
525  m.pose.orientation.z = 0;
526  m.pose.orientation.w = 1;
527 
528  m.scale.x = 0.01f;
529  m.scale.y = 0.01f;
530  m.scale.z = 0.01f;
531  m.color.r = 1.0;
532  m.color.g = 0;
533  m.color.b = 0;
534  m.color.a = 1.0;
535  ma.markers.push_back(m);
536  pub_->publish(ma);
537  */
538  return true;
539  }
540  }
541  }
542  return false;
543 }
544 /*
545 
546 void Base_calculation_mediator::write_file(){
547  for(auto& workcell: protobuf_){
548  for(auto& single_solution: workcell){
549 
550  }
551  }
552 }
553 */
554 
555 void BaseCalculationMediator::writeFile(std::vector<protobuf_entry>& wc_solution){
556 
557  std::string resultFile = std::to_string(static_cast<int>(ros::Time::now().toNSec()));
558  std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_ + "/" + resultFile);
559  std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_ + "/" + resultFile + "/configs");
560  std::filesystem::create_directory(ros::package::getPath("multi_cell_builder") + "/results/" + filename_ + "/" + resultFile + "/launch");
561 
562  std::stringstream ss;
563  std::stringstream panel_ss;
564  std::stringstream root_ss;
565  std::stringstream box_ss;
566 
567 
568  for (auto& box : cuboid_reader_->cuboidBox()){
569  box_ss << "{ 'id': '" << box.Name << "', 'type': 'BOX', 'pos': { 'x': "<<box.Pose.position.x<<", 'y': "<<box.Pose.position.y<<", 'z': "<< box.Pose.position.z<<" },'size': { 'length': "<< box.x_depth<<", 'width': "<<box.y_width<<", 'height': "<< box.z_heigth<<" },'orientation': { 'x':"<< box.Pose.orientation.x <<", 'y':" << box.Pose.orientation.y<< ", 'z':" << box.Pose.orientation.z << ", 'w':" << box.Pose.orientation.w<< "},'color': { 'b': 1 } }, \n";
570  }
571 
572  for (auto& box : cuboid_reader_->cuboidObstacle()){
573  box_ss << "{ 'id': '" << box.Name << "', 'type': 'BIN', 'pos': { 'x': "<<box.Pose.position.x<<", 'y': "<<box.Pose.position.y<<", 'z': "<< box.Pose.position.z<<" },'size': { 'length': "<< box.x_depth<<", 'width': "<<box.y_width<<", 'height': "<< box.z_heigth<<" },'orientation': { 'x':"<< box.Pose.orientation.x <<", 'y':" << box.Pose.orientation.y<< ", 'z':" << box.Pose.orientation.z << ", 'w':" << box.Pose.orientation.w<< "},'color': { 'b': 1 } }, \n";
574  }
575 
576  ss << "{ 'objects' : [ \n";
577  for (int c = 0; c < wc_solution.size(); c++){
578  try{
579  std::regex rx("panda_arm([0-9]+)");
580  std::smatch match;
581  std::regex_match(wc_solution[c].name_, match, rx);
582 
583  auto ceti = dynamic_cast<CetiRobot*>(robots_.at(wc_solution[c].name_)->next());
584  ceti->setTf(wc_solution[c].tf_);
585  ceti->notify();
586 
587  double r,p,y;
588  tf2::Matrix3x3 m(wc_solution[c].tf_.getRotation());
589  m.getRPY(r,p,y);
590 
591  float size_x = ceti->size().getX();
592  float size_y = ceti->size().getY();
593  float size_z = ceti->size().getZ();
594 
595  float pos_x = ceti->tf().getOrigin().getX();
596  float pos_y = ceti->tf().getOrigin().getY();
597  float pos_z = ceti->tf().getOrigin().getZ() *2 ;
598  float rot_x = ceti->tf().getRotation().getX();
599  float rot_y = ceti->tf().getRotation().getY();
600  float rot_z = ceti->tf().getRotation().getZ();
601  float rot_w = ceti->tf().getRotation().getW();
602 
603  // initial stardconfig
604  ss << "{ 'id' : 'table" << match[1] << "_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";
605  ss << "{ 'id' : 'table" << match[1] << "_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";
606  ss << "{ 'id' : 'table" << match[1] << "_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";
607  ss << "{ 'id' : 'table" << match[1] << "_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";
608  ss << "{ 'id' : 'table" << match[1] << "_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";
609  ss << "{ 'id' : 'table" << match[1] << "_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";
610  ss << "{ 'id' : 'table" << match[1] << "_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";
611  ss << "{ 'id' : 'table" << match[1] << "_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";
612  ss << "{ 'id' : 'table" << match[1] << "_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";
613 
614  // config
615  std::stringstream config;
616  config << "mapspace: {'dim': [";
617  config << size_x/2 << ", ";
618  config << size_y/2 << ", 0.04], ";
619  config << "'pos': [";
620  config << pos_x << ", ";
621  config << pos_y << ", ";
622  config << (pos_z + size_z/2.0f + 0.04f/2.0f) << "], ";
623  config << "'rot': [";
624  config << r << ", " << p << ", " << y <<"], ";
625  config << "'verbose': false }\n";
626  config << "voxel_size: 0.02 \n";
627  config << "voxel_space: {'dim': [";
628  config << (size_x + 0.1f)/2 << ", ";
629  config << (size_y + 0.1f)/2 << ", 0.2], ";
630  config << "'pos': [";
631  config << pos_x << ", ";
632  config << pos_y << ", ";
633  config << (pos_z + size_z/2.0f + 0.2f/2.0f) << "], ";
634  config << "'rot': [";
635  config << r << ", " << p << ", " << y <<"]}\n";
636  config << "voxel_verbose_level: 0\n";
637  config << "translation_rate: 0.03\n";
638  config << "rotation_rate: 45.0\n";
639  config << "rotation_max: 90.0\n";
640  config << "clearance: 0.01\n";
641  config << "min_quality: 0.1\n";
642  config << "max_candidate_count: 10\n";
643  config << "allow_dependencies: true\n";
644  config << "top_grasps_only: true\n";
645 
646  std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/configs/" + resultFile + "_"+ wc_solution[c].name_+".yaml");
647  o << config.str();
648  o.close();
649 
650 
651  std::bitset<3> panel_mask(wc_solution[c].wing_config_);
652  for(std::size_t i = 0; i < ceti->observers().size(); i++){
653  if(ceti->observerMask()[i] & panel_mask[i]){
654  float x = ceti->observers()[i]->worldTf().getOrigin().getX();
655  float y = ceti->observers()[i]->worldTf().getOrigin().getY();
656  float z = ceti->observers()[i]->worldTf().getOrigin().getZ();
657  float qx = ceti->observers()[i]->worldTf().getRotation().getX();
658  float qy = ceti->observers()[i]->worldTf().getRotation().getY();
659  float qz = ceti->observers()[i]->worldTf().getRotation().getZ();
660  float qw = ceti->observers()[i]->worldTf().getRotation().getW();
661 
662  float length = ceti->observers()[i]->size().getX();
663  float width = ceti->observers()[i]->size().getY();
664  float height = ceti->observers()[i]->size().getZ();
665  panel_ss << "{ 'id': '" << ceti->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";
666  }
667  }
668 
669  tf2::Transform tf_arm = wc_solution[c].tf_ * ceti->rootTf();
670  float arm_x = tf_arm.getOrigin().getX();
671  float arm_y = tf_arm.getOrigin().getY();
672  float arm_z = tf_arm.getOrigin().getZ();
673  float arm_qx = tf_arm.getRotation().getX();
674  float arm_qy = tf_arm.getRotation().getY();
675  float arm_qz = tf_arm.getRotation().getZ();
676  float arm_qw = tf_arm.getRotation().getW();
677 
678  if (!(c == wc_solution.size())){
679  // begin
680  root_ss << "{ 'id': 'arm" << match[1] << "','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";
681  } else {
682  root_ss << "{ 'id': 'arm" << match[1] << "','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 } }";
683  }
684 
685  //launch
686  std::stringstream launch;
687  launch << "<launch>\n";
688  launch << "<arg name=\"referenceRobot\" default=\""<< wc_solution[c].name_ << "\" />\n";
689  launch << "<arg name=\"referenceXYZ\" default=\""<< arm_x <<" "<< arm_y << " " << arm_z <<"\"/>\n";
690  launch << "<arg name=\"referenceRPY\" default=\"" << r << " " << p << " " << y << "\"/>\n";
691  launch << "<arg name=\"result\" default=\"" << filename_ <<"/" << resultFile << "/" << resultFile <<".yaml\" />\n";
692  launch << "<rosparam command=\"load\" file=\"$(find multi_cell_builder)/results/$(arg result)\"/>\n";
693  launch << "<rosparam param=\"referenceRobot\" subst_value=\"True\"> $(arg referenceRobot)</rosparam>\n";
694  launch << "<rosparam param=\"resultPath\" subst_value=\"True\"> $(arg result)</rosparam>\n";
695  launch << "<arg name=\"pipeline\" default=\"ompl\"/>\n";
696  launch << "<arg name=\"db\" default=\"false\"/>\n";
697  launch << "<arg name=\"db_path\" default=\"$(find panda_moveit_config)/default_warehouse_mongo_db\"/>\n";
698  launch << "<arg name=\"debug\" default=\"false\" />\n";
699  launch << "<arg name=\"load_robot_description\" default=\"true\"/>\n";
700  launch << "<arg name=\"moveit_controller_manager\" default=\"fake\"/>\n";
701  launch << "<arg name=\"fake_execution_type\" default=\"interpolate\"/>\n";
702  launch << "<arg name=\"use_gui\" default=\"false\"/>\n";
703  launch << "<arg name=\"use_rviz\" default=\"true\"/>\n";
704  launch << "<node pkg=\"tf2_ros\" type=\"static_transform_publisher\" name=\"virtual_joint_broadcaster_0\" args=\"0 0 0 0 0 0 world panda_link0\"/>\n";
705  launch << "<group if=\"$(eval arg('moveit_controller_manager') == 'fake')\">\n";
706  launch << "<node name=\"joint_state_publisher\" pkg=\"joint_state_publisher\" type=\"joint_state_publisher\" unless=\"$(arg use_gui)\">\n";
707  launch << "<rosparam param=\"source_list\">[move_group/fake_controller_joint_states]</rosparam>\n";
708  launch << "</node>\n";
709  launch << "<node name=\"joint_state_publisher\" pkg=\"joint_state_publisher_gui\" type=\"joint_state_publisher_gui\" if=\"$(arg use_gui)\">\n";
710  launch << "<rosparam param=\"source_list\">[move_group/fake_controller_joint_states]</rosparam>\n";
711  launch << "</node>\n";
712  launch << "<node name=\"robot_state_publisher\" pkg=\"robot_state_publisher\" type=\"robot_state_publisher\" respawn=\"true\" output=\"screen\"/>\n";
713  launch << "</group>\n";
714  launch << "<include file=\"$(find panda_moveit_config)/launch/move_group.launch\">\n";
715  launch << "<arg name=\"allow_trajectory_execution\" value=\"true\"/>\n";
716  launch << "<arg name=\"moveit_controller_manager\" value=\"$(arg moveit_controller_manager)\"/>\n";
717  launch << "<arg name=\"fake_execution_type\" value=\"$(arg fake_execution_type)\"/>\n";
718  launch << "<arg name=\"info\" value=\"true\"/>\n";
719  launch << "<arg name=\"debug\" value=\"$(arg debug)\"/>\n";
720  launch << "<arg name=\"pipeline\" value=\"$(arg pipeline)\"/>\n";
721  launch << "<arg name=\"load_robot_description\" value=\"$(arg load_robot_description)\"/>\n";
722  launch << "<arg name=\"referenceXYZ\" value=\"$(arg referenceXYZ)\"/>\n";
723  launch << "<arg name=\"referenceRPY\" value=\"$(arg referenceRPY)\"/>\n";
724  launch << "</include>\n";
725 
726  launch << "<include file=\"$(find panda_moveit_config)/launch/default_warehouse_db.launch\" if=\"$(arg db)\">\n";
727  launch << "<arg name=\"moveit_warehouse_database_path\" value=\"$(arg db_path)\"/>\n";
728  launch << "</include>\n";
729 
730  launch << "<node name=\"$(anon rviz)\" pkg=\"rviz\" type=\"rviz\" respawn=\"false\" args=\"-d $(find moveit_grasps)/launch/rviz/grasps.rviz\" output=\"screen\">\n";
731  launch << "<rosparam command=\"load\" file=\"$(find panda_moveit_config)/config/kinematics.yaml\"/>\n";
732  launch << "</node>\n";
733 
734  launch << "<node name=\"config_routine\" pkg=\"multi_cell_builder\" type=\"config_routine\" output=\"screen\">\n";
735  launch << "<param name=\"gripper\" value=\"two_finger\"/>\n";
736  launch << "<param name=\"ee_group_name\" value=\"hand\"/>\n";
737  launch << "<param name=\"planning_group_name\" value=\"panda_arm_hand\" />\n";
738  launch << "<rosparam command=\"load\" file=\"$(find gb_grasp)/config_robot/panda_grasp_data.yaml\"/>\n";
739  launch << "<rosparam command=\"load\" file=\"$(find gb_grasp)/config/moveit_grasps_config.yaml\"/>\n";
740  launch << "<rosparam command=\"load\" file=\"$(find multi_cell_builder)/results/" << filename_ << "/" << resultFile << "/configs/" << resultFile << "_" << wc_solution[c].name_ << ".yaml\"/>\n";
741  launch << "</node>\n";
742 
743  launch << "<arg name=\"planner\" default=\"ompl\" />\n";
744  launch << "<include ns=\"moveit_grasps_demo\" file=\"$(find panda_moveit_config)/launch/planning_pipeline.launch.xml\">\n";
745  launch << "<arg name=\"pipeline\" value=\"$(arg planner)\" />\n";
746  launch << "</include>\n";
747  launch << "</launch>\n";
748 
749  std::ofstream l(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/launch/" + wc_solution[c].name_+ "_" + resultFile + ".launch");
750  l << launch.str();
751  l.close();
752 
753 
754  } catch (std::out_of_range& oor) {
755  }
756  }
757 
758 
759 
760 
761  std::ofstream o(ros::package::getPath("multi_cell_builder") + "/results/"+ filename_ + "/" + resultFile + "/" + resultFile + ".yaml");
762  ss << panel_ss.str();
763  ss << box_ss.str();
764  ss << root_ss.str();
765  ss << "]}";
766  o << ss.str();
767  o.close();
768 }
769 
771  visualization_msgs::MarkerArray ma;
772  visualization_msgs::Marker marker;
773  marker.header.frame_id = "map";
774  marker.header.stamp = ros::Time();
775  marker.ns = ar->name();
776  marker.id = 1;
777  marker.type = visualization_msgs::Marker::CUBE;
778  marker.action = visualization_msgs::Marker::MODIFY;
779  marker.pose.position.x = ar->tf().getOrigin().getX();
780  marker.pose.position.y = ar->tf().getOrigin().getY();
781  marker.pose.position.z = ar->tf().getOrigin().getZ();
782  marker.pose.orientation.x = ar->tf().getRotation().getX();
783  marker.pose.orientation.y = ar->tf().getRotation().getY();
784  marker.pose.orientation.z = ar->tf().getRotation().getZ();
785  marker.pose.orientation.w = ar->tf().getRotation().getW();
786  marker.scale.x = ar->size().getX();
787  marker.scale.y = ar->size().getY();
788  marker.scale.z = ar->size().getZ();
789  marker.color.r = 1.0;
790  marker.color.g = 1.0;
791  marker.color.b = 1.0;
792  marker.color.a = 1.0;
793  for (int i = 0; i < ar->observers().size(); i++){
794  auto wrd = dynamic_cast<RvizPanel*>(ar->observers()[i].get());
795  ma.markers.push_back(wrd->marker());
796  }
797 
798  //pub_->publish(ma);
799 }
BaseCalculationMediator::calculate
void calculate(std::map< const std::string, std::vector< tf2::Transform >> &workcell)
Workspace calculator.
Definition: base_calculation_mediator.cpp:203
AbstractMediator::result_vector_
std::map< const std::string, std::vector< pcl::PointXYZ > > result_vector_
Result_vector of base positions linked to robot.
Definition: abstract_mediator.h:50
BaseCalculationMediator::checkCollision
bool checkCollision(const std::string &robot, std::bitset< 3 > &panel_mask, bool &workload, std::vector< object_data > &ts)
check_collision
Definition: base_calculation_mediator.cpp:63
BaseCalculationMediator::BaseCalculationMediator
BaseCalculationMediator(std::shared_ptr< ros::NodeHandle > const &d)
BaseCalculationMediator constructor.
Definition: base_calculation_mediator.cpp:3
CetiRobot::observers
std::vector< std::unique_ptr< AbstractRobotElementDecorator > > & observers()
Get observers.
Definition: ceti_robot.h:53
CetiRobot::setObserverMask
void setObserverMask(int i)
Definition: ceti_robot.h:83
AbstractRobot::rotate
void rotate(float deg)
Definition: abstract_robot.h:49
BaseCalculationMediator::protobuf_
std::vector< std::vector< std::vector< protobuf_entry > > > protobuf_
Datastructure as product of all baseposition -> robot combinations.
Definition: base_calculation_mediator.h:28
wing_config
wing_config
Definition: abstract_robot.h:10
AbstractRobot::setTf
void setTf(tf2::Transform &t)
Definition: abstract_robot.h:48
BaseCalculationMediator::grounds_
std::map< const std::string, std::vector< pcl::PointXYZ > > grounds_
Possible ground positions per robots.
Definition: base_calculation_mediator.h:26
CetiRobot::notify
void notify() override
Observer pattern.
Definition: ceti_robot.cpp:68
BaseCalculationMediator::filename_
std::string filename_
Definition: base_calculation_mediator.h:29
base_calculation_mediator.h
BaseCalculationMediator::connectRobots
void connectRobots(std::unique_ptr< AbstractRobotDecorator > robot) override
Robot Connector implementation.
Definition: base_calculation_mediator.cpp:25
AbstractMediator::wing_reader_
std::unique_ptr< WingReader > wing_reader_
Wing_reader which collects panel information of robots.
Definition: abstract_mediator.h:54
BaseCalculationMediator::generateGrounds
void generateGrounds(const tf2::Vector3 origin, const float diameter, float resolution)
Ground generator.
Definition: base_calculation_mediator.cpp:13
AbstractMediator::nh_
std::shared_ptr< ros::NodeHandle > nh_
Nodehandle for access to the Rosparam server.
Definition: abstract_mediator.h:45
CetiRobot::observerMask
std::bitset< 3 > observerMask()
Definition: ceti_robot.h:82
protobuf_entry
Protobuf entry.
Definition: abstract_mediator.h:33
CetiRobot::size
tf2::Vector3 & size() override
Definition: ceti_robot.h:34
AbstractMediator
AbstractMediator.
Definition: abstract_mediator.h:43
CetiRobot
Concrete Ceti-Robot.
Definition: ceti_robot.h:14
BaseCalculationMediator::setPanel
void setPanel() override
pure virtual Sets panels for robots
Definition: base_calculation_mediator.cpp:30
RvizPanel::marker
visualization_msgs::Marker & marker()
Definition: rviz_panel.h:18
AbstractMediator::vector2cloud
pcl::PointCloud< pcl::PointXYZ >::Ptr vector2cloud(std::vector< pcl::PointXYZ > &vector)
Cloud converter.
Definition: abstract_mediator.cpp:11
BaseCalculationMediator::publish
void publish(CetiRobot *r)
Marker publishing methode.
Definition: base_calculation_mediator.cpp:770
CetiRobot::checkSingleObjectCollision
bool checkSingleObjectCollision(tf2::Transform &obj, std::string &robot_element, std::bitset< 3 > &panel_mask) override
Collsion calculation for single objects.
Definition: ceti_robot.cpp:10
RvizPanel
Definition: rviz_panel.h:10
AbstractMediator::robots_
std::map< std::string, std::unique_ptr< AbstractRobotDecorator > > robots_
Robots agents.
Definition: abstract_mediator.h:47
BaseCalculationMediator::pub_
std::unique_ptr< ros::Publisher > pub_
Publisher sharing visualization messages of the scene.
Definition: base_calculation_mediator.h:25
BaseCalculationMediator::mediate
void mediate() override
Mediator implementation.
Definition: base_calculation_mediator.cpp:130
BaseCalculationMediator::writeFile
void writeFile(std::vector< protobuf_entry > &wc_solution)
Writes result file.
Definition: base_calculation_mediator.cpp:555
BaseCalculationMediator::sceneCollision
bool sceneCollision(std::vector< protobuf_entry > &wc_solution)
Definition: base_calculation_mediator.cpp:469
AbstractMediator::cuboid_reader_
std::unique_ptr< CuboidReader > cuboid_reader_
coboidReader instance that distinguishes between scene objects of type bin and box
Definition: abstract_mediator.h:56
CetiRobot::name
std::string & name() override
Definition: ceti_robot.h:32
BaseCalculationMediator::approximation
void approximation(std::map< const std::string, std::vector< tf2::Transform >> &workcell, std::vector< protobuf_entry > &wc_solution)
Approximates other robots to fit in the workspace.
Definition: base_calculation_mediator.cpp:305
CetiRobot::tf
tf2::Transform & tf() override
Definition: ceti_robot.h:33
AbstractMediator::task_space_reader_
std::unique_ptr< TSReader > task_space_reader_
Task_space reader which provides drop off positions.
Definition: abstract_mediator.h:46


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51