moveit_mediator.cpp
Go to the documentation of this file.
1 #include "impl/moveit_mediator.h"
2 #include <thread>
3 #include <mutex>
4 #include <algorithm>
5 #include <gb_grasp/MapGenerator.h>
6 #include <regex>
7 
8 thread_local moveit::task_constructor::Task task__;
9 thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_;
10 
11 std::mutex task_writing;
12 //std::mutex publish;
13 
15  robots_.push_back(robot);
16  ROS_INFO("%s connected...", robot->name().c_str());
17 
18  Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot);
19  acm_.insert(std::pair<std::string, std::vector<uint8_t>>(mr->map().at("base").c_str(), std::vector<uint8_t>()));
20  for(auto name: mr->mgi()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
21  for(auto name: mr->mgi_hand()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
22 }
23 
25  ros::WallDuration sleep_time(1.0);
26  for(long unsigned int i = 0; i < robots_.size();i++){
27  Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);
28 
29  /*
30  moveit_msgs::CollisionObject table;
31  table.id = ceti->map().at("base").c_str();
32  table.header.frame_id = "world";
33  table.header.stamp = ros::Time::now();
34  table.primitives.resize(1);
35  table.primitives[0].type = table.primitives[0].BOX;
36  table.primitives[0].dimensions.resize(3);
37  table.primitives[0].dimensions[0] = ceti->size().getX();
38  table.primitives[0].dimensions[1] = ceti->size().getY();
39  table.primitives[0].dimensions[2] = ceti->tf().getOrigin().getZ() * 2;
40  table.primitive_poses.resize(1);
41  table.primitive_poses[0].position.x = ceti->tf().getOrigin().getX();
42  table.primitive_poses[0].position.y = ceti->tf().getOrigin().getY();
43  table.primitive_poses[0].position.z = ceti->tf().getOrigin().getZ();
44  table.primitive_poses[0].orientation.x = ceti->tf().getRotation().getX();
45  table.primitive_poses[0].orientation.y = ceti->tf().getRotation().getY();
46  table.primitive_poses[0].orientation.z = ceti->tf().getRotation().getZ();
47  table.primitive_poses[0].orientation.w = ceti->tf().getRotation().getW();
48  table.operation = table.ADD;
49  */
50  //psi_->applyCollisionObject(table);
51 
52  for (Abstract_robot_element* are : ceti->observers()) {
53  Wing_moveit_decorator* wmd = dynamic_cast<Wing_moveit_decorator*>(are);
54  psi_->applyCollisionObject(*wmd->markers());
55  acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->markers()->id.c_str(), std::vector<uint8_t>()));
56  sleep_time.sleep();
57  }
58  }
59 }
60 
61 bool Moveit_mediator::check_collision(const int& robot){
62  auto env = ps_->getCollisionEnvNonConst();
63  Collision_helper* ch = static_cast<Collision_helper*>(env.get());
64  return ch->collision_detector("object_A", "object_B"); //
65 }
66 
69  task_planner();
70  setup_task();
71  ros::waitForShutdown();
72 
73 
74 };
75 
76 void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){
77  std::bitset<3> result = robots_[robot]->observer_mask() & wing;
78  Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);
79 
80  for (std::size_t i = 0; i < result.size(); i++){
81  if (result[i]){
82  ceti->register_observers(wings_[robot][i]);
83  }
84  }
85 }
86 
87 void Moveit_mediator::set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp){
88  for (int i =0; i < wbp.size(); i++){
89  std::vector<Abstract_robot_element*> v;
90  for (int j =0; j < wbp[i].first.size(); j++){
91  Abstract_robot_element* are = new Wing_moveit_decorator( new Wing(wbp[i].first[j].name_, wbp[i].first[j].pose_,wbp[i].first[j].size_));
92  v.push_back(are);
93  }
94  wings_.push_back(v);
95  }
96 };
97 
99  ROS_INFO("=> write Task Constructor Objects");
100  for(auto& a: acm_) a.second.resize(acm_.size(), 0);
101  /*
102 
103  bool coop = false;
104 
105  for(int i = 0; i < objects_.size(); i++){
106  if (i+1 < objects_.size()){
107  for (long unsigned int j = objects_[i].size()-1; j > 0; j--){
108  if(objects_[i][j].getOrigin().distance(objects_[i+1].back().getOrigin()) == 0) {
109  objects_[i+1].pop_back();
110  coop= true;
111  }
112  }
113  }
114  }
115 
116  if(coop){
117  moveit_msgs::CollisionObject bottle;
118  bottle.id = "bottle";
119  bottle.header.frame_id = "world";
120  bottle.header.stamp = ros::Time::now();
121  bottle.primitives.resize(1);
122  bottle.primitives[0].type = bottle.primitives[0].BOX;
123  bottle.primitives[0].dimensions.resize(3);
124  bottle.primitives[0].dimensions[0] = box_size.getX();
125  bottle.primitives[0].dimensions[1] = box_size.getY();
126  bottle.primitives[0].dimensions[2] = box_size.getZ();
127 
128  bottle.primitive_poses.resize(1);
129  bottle.primitive_poses[0].position.x = -0.3f;
130  bottle.primitive_poses[0].position.y = -0.6f;
131  bottle.primitive_poses[0].position.z = 0.9355f;
132  bottle.primitive_poses[0].orientation.x = 0;
133  bottle.primitive_poses[0].orientation.y = 0;
134  bottle.primitive_poses[0].orientation.z = 0;
135  bottle.primitive_poses[0].orientation.w = 1;
136  bottle.operation = bottle.ADD;
137 
138  psi_->applyCollisionObject(bottle);
139 
140 
141  for( int i = 0; i < objects_.size();i++){
142  for(int j =0; j < objects_[i].size();j++){
143  Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]);
144  moveit::task_constructor::Task task = create_Task(mr, psi_->getObjects().at("bottle"), objects_[i][j]);
145  if (task.plan(1)){
146  task.execute(*task.solutions().front());
147  }
148 
149  }
150  }
151  } else {
152  acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle0", std::vector<uint8_t>()));
153  acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle1", std::vector<uint8_t>()));
154 
155  moveit_msgs::CollisionObject bottle1;
156  bottle1.id = "bottle0";
157  bottle1.header.frame_id = "world";
158  bottle1.header.stamp = ros::Time::now();
159  bottle1.primitives.resize(1);
160  bottle1.primitives[0].type = bottle1.primitives[0].BOX;
161  bottle1.primitives[0].dimensions.resize(3);
162  bottle1.primitives[0].dimensions[0] = box_size.getX();
163  bottle1.primitives[0].dimensions[1] = box_size.getY();
164  bottle1.primitives[0].dimensions[2] = box_size.getZ();
165 
166  bottle1.primitive_poses.resize(1);
167  bottle1.primitive_poses[0].position.x = objects_[0][0].getOrigin().getX();
168  bottle1.primitive_poses[0].position.y = objects_[0][0].getOrigin().getY();
169  bottle1.primitive_poses[0].position.z = 0.9355f;
170  bottle1.primitive_poses[0].orientation.x = 0;
171  bottle1.primitive_poses[0].orientation.y = 0;
172  bottle1.primitive_poses[0].orientation.z = 0;
173  bottle1.primitive_poses[0].orientation.w = 1;
174  bottle1.operation = bottle1.ADD;
175 
176  moveit_msgs::CollisionObject bottle2;
177  bottle2.id = "bottle1";
178  bottle2.header.frame_id = "world";
179  bottle2.header.stamp = ros::Time::now();
180  bottle2.primitives.resize(1);
181  bottle2.primitives[0].type = bottle2.primitives[0].BOX;
182  bottle2.primitives[0].dimensions.resize(3);
183  bottle2.primitives[0].dimensions[0] = box_size.getX();
184  bottle2.primitives[0].dimensions[1] = box_size.getY();
185  bottle2.primitives[0].dimensions[2] = box_size.getZ();
186 
187  bottle2.primitive_poses.resize(1);
188  bottle2.primitive_poses[0].position.x = objects_[1][0].getOrigin().getX();
189  bottle2.primitive_poses[0].position.y = objects_[1][0].getOrigin().getY();
190  bottle2.primitive_poses[0].position.z = 0.9355f;
191  bottle2.primitive_poses[0].orientation.x = 0;
192  bottle2.primitive_poses[0].orientation.y = 0;
193  bottle2.primitive_poses[0].orientation.z = 0;
194  bottle2.primitive_poses[0].orientation.w = 1;
195  bottle2.operation = bottle2.ADD;
196 
197  psi_->applyCollisionObject(bottle1);
198  psi_->applyCollisionObject(bottle2);
199 
200  std::vector<std::thread> ths;
201  std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> tasks;
202 
203  for (int i = 0 ; i < objects_.size(); i++){
204  std::stringstream ss;
205  ss << "bottle" << std::to_string(i);
206  Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]);
207  std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot;
208  for (int j = 1; j < objects_[i].size(); j++){
209  moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(ss.str()), objects_[i][j]);
210  if(mgt.plan(1)) {
211  moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
212  mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
213  tasks_per_robot.push(e);
214 
215  moveit_msgs::CollisionObject temp = psi_->getObjects().at(ss.str());
216  temp.operation = temp.MOVE;
217  temp.pose.position.x = objects_[i][j].getOrigin().getX();
218  temp.pose.position.y = objects_[i][j].getOrigin().getY();
219  psi_->applyCollisionObject(temp);
220  }
221  }
222 
223  moveit_msgs::CollisionObject temp = psi_->getObjects().at(ss.str());
224  temp.operation = temp.MOVE;
225  temp.pose.position.x = objects_[i][0].getOrigin().getX();
226  temp.pose.position.y = objects_[i][0].getOrigin().getY();
227  psi_->applyCollisionObject(temp);
228  tasks.push_back(tasks_per_robot);
229  //ths.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), psi_->getObjects().at(ss.str()), objects_[i]));
230  }
231 
232  for(auto& a: acm_) a.second.resize(acm_.size(), 0);
233 
234  while (!tasks[0].empty() && !tasks[1].empty()){
235  for (int i =0; i < std::max(tasks[0].front().solution.sub_trajectory.size(), tasks[1].front().solution.sub_trajectory.size()); i++){
236  moveit_msgs::RobotTrajectory* t1 = nullptr;
237  moveit_msgs::RobotTrajectory* t2 = nullptr;
238  moveit_msgs::PlanningScene* ps1 = nullptr;
239  moveit_msgs::PlanningScene* ps2 = nullptr;
240  moveit_msgs::PlanningScene ps_m;
241  std::vector<moveit_msgs::PlanningScene::_allowed_collision_matrix_type> acm_m;
242 
243  //for(auto entry: acm_) ROS_INFO("%s", entry.first.c_str());
244 
245  if (i < tasks[0].front().solution.sub_trajectory.size()){
246  t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
247  ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff;
248  }
249 
250  if (i < tasks[1].front().solution.sub_trajectory.size()){
251  t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory;
252  ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
253  }
254 
255  std::vector<std::thread> th;
256 
257  if (t1){
258  Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
259  th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));
260 
261  // First find ID from panda to start with
262  std::regex rx_panda("panda_arm([0-9]+)");
263  std::smatch match;
264  std::regex_match(mr->name(), match, rx_panda);
265 
266  // build panda link regex
267  std::stringstream ss;
268  ss << "panda_" << match[1] << "_.*";
269  std::regex rx_panda_links(ss.str());
270  std::regex rx_box("bottle.*");
271  std::regex rx_table("table.*");
272 
273  // Iterate task collsion matrix
274  for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){
275  if( mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){
276  In case an entry matches an robot spezific link
277  ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
278  for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
279  For that specific entry, loop over values
280  int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
281 
282  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
283  ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
284  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
285  }
286 
287  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
288  ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
289  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
290  }
291 
292  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
293  ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
294  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
295  }
296 
297  if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
298  ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
299  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
300  }
301  }
302  }
303 
304  if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
305  In case an entry matches an robot spezific link
306  ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
307  for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
308  For that specific entry, loop over values
309  int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
310 
311  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
312  ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
313  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
314  }
315 
316  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
317  ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
318  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
319  }
320 
321  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
322  ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
323  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
324  }
325 
326  if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
327  ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
328  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
329  }
330  }
331  }
332 
333  if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box)){
334  In case an entry matches an robot spezific link
335  ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
336  for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
337  For that specific entry, loop over values
338  int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
339 
340  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
341  ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
342  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
343  }
344 
345  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
346  ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
347  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
348  }
349 
350  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
351  ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
352  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
353  }
354 
355  if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
356  ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
357  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
358  }
359  }
360  }
361 
362  if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table)){
363  In case an entry matches an robot spezific link
364  ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j], j);
365  for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
366  For that specific entry, loop over values
367  int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));
368 
369  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
370  ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
371  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
372  }
373 
374  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
375  ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
376  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
377  }
378 
379  if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
380  ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
381  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
382  }
383 
384  if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
385  ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
386  acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
387  }
388  }
389  }
390  }
391  merge_ps(ps_m, ps1, mr);
392  }
393 
394  if (t2){
395  Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]);
396  th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2));
397 
398  std::regex rx_panda("panda_arm([0-9]+)");
399  std::smatch match;
400  std::regex_match(mr->name(), match, rx_panda);
401 
402  // build panda link regex
403  std::stringstream ss;
404  ss << "panda_" << match[1] << "_.*";
405  std::regex rx_panda_links(ss.str());
406  std::regex rx_box("bottle.*");
407  std::regex rx_table("table.*");
408 
409  // Iterate task collsion matrix
410  for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){
411  if( mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){
412  In case an entry matches an robot spezific link
413  ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
414  for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
415  For that specific entry, loop over values
416  int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
417 
418  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
419  ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
420  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
421  }
422 
423  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
424  ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
425  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
426  }
427 
428  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
429  ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
430  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
431  }
432 
433  if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
434  ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
435  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
436  }
437  }
438  }
439 
440  if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
441  In case an entry matches an robot spezific link
442  ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
443  for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
444  For that specific entry, loop over values
445  int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
446 
447  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
448  ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
449  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
450  }
451 
452  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
453  ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
454  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
455  }
456 
457  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
458  ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
459  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
460  }
461 
462  if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
463  ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
464  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
465  }
466  }
467  }
468 
469  if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box)){
470  In case an entry matches an robot spezific link
471  ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
472  for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
473  For that specific entry, loop over values
474  int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
475 
476  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
477  ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
478  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
479  }
480 
481  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
482  ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
483  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
484  }
485 
486  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
487  ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
488  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
489  }
490 
491  if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
492  ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
493  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
494  }
495  }
496  }
497 
498  if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table)){
499  In case an entry matches an robot spezific link
500  ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j], j);
501  for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
502  For that specific entry, loop over values
503  int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));
504 
505  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
506  ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
507  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
508  }
509 
510  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
511  ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
512  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
513  }
514 
515  if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
516  ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
517  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
518  }
519 
520  if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
521  ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
522  acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
523  }
524  }
525  }
526  }
527  merge_ps(ps_m, ps2, mr);
528  }
529 
530 
531  for(int i = 0; i < th.size(); i++){
532  th[i].join();
533  }
534  merge_acm(ps_m);
535  planning_scene_diff_publisher_->publish(ps_m);
536 
537  }
538  tasks[0].pop();
539  tasks[1].pop();
540  }
541  }
542  */
543 
544 
545  std::regex item("box_([0-9]+)");
546  std::smatch match;
547  ROS_INFO("tasks %li", tasks_.size());
548 
549  while(!tasks_.empty()){
550  ROS_INFO("in tasks");
551  std::vector<std::pair<std::string, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> q_ets;
552  for (auto*r:robots_){
553  ROS_INFO("robot iteration");
554  auto itlow = tasks_.lower_bound (r->name()); // itlow points to b
555  auto itup = tasks_.upper_bound (r->name());
556  for (auto it=itlow; it!=itup; ++it){
557  tf2::Vector3 b_start_position = (*it).second.first;
558  for(auto& s_co : psi_->getObjects()){
559  if(!std::regex_match(s_co.first, match, item)) continue;
560  if(tf2::tf2Distance2(b_start_position, tf2::Vector3(s_co.second.pose.position.x, s_co.second.pose.position.y,s_co.second.pose.position.z)) == 0) {
561  q_ets.push_back(std::pair<std::string, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>(r->name(), (*it).second.second));
562  tasks_.erase(it);
563  break;
564  }
565  }
566  }
567  }
568 
569  ROS_INFO("tasks %li", tasks_.size());
570  ROS_INFO("jobs %li", q_ets.size());
571 
572  // now execution should be possible, but with bocking
573  std::vector<std::thread> th;
574 
575  while(!q_ets.empty()){
576  moveit_msgs::PlanningScene ps_m;
577  ps_m.is_diff = 0;
578  for (auto*r:robots_){
579  for (auto& s_qets : q_ets){
580  if(s_qets.first == r->name()){
581  if(!s_qets.second.front().solution.sub_trajectory.empty()){
582  moveit_msgs::RobotTrajectory* rt = (!s_qets.second.front().solution.sub_trajectory.empty()) ? &s_qets.second.front().solution.sub_trajectory.front().trajectory : nullptr;
583  moveit_msgs::PlanningScene* ps = (!s_qets.second.front().solution.sub_trajectory.empty()) ? &s_qets.second.front().solution.sub_trajectory.front().scene_diff : nullptr;
584 
585  if (rt){
586  Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r);
587  // mr->mgi()->execute(*rt);
588  th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *rt, *ps));
589 
590  // First find ID from panda to start with
591  std::regex rx_panda("panda_arm([0-9]+)");
592  std::smatch match;
593  std::regex_match(mr->name(), match, rx_panda);
594 
595  // build panda link regex
596  std::stringstream ss;
597  ss << "panda_" << match[1] << "_.*";
598  std::regex rx_panda_links(ss.str());
599  std::regex rx_box("box.*");
600  std::regex rx_table("table.*");
601 
602  if(ps){
603  for(int j = 0; j < ps->allowed_collision_matrix.entry_names.size(); j++ ){
604  if( mr->map().at("base") == ps->allowed_collision_matrix.entry_names[j]){
605  for(int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
606  int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k]));
607  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
608  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
609  }
610  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
611  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
612  }
613  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
614  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
615  }
616  if (mr->map().at("base")== ps->allowed_collision_matrix.entry_names[k]){
617  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
618  }
619  }
620  }
621 
622  if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
623  for(int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
624  int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k]));
625  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
626  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
627  }
628  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
629  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
630  }
631  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
632  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
633  }
634  if (mr->map().at("base")== ps->allowed_collision_matrix.entry_names[k]){
635  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
636  }
637  }
638  }
639 
640  if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_box)){
641  for(int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
642  int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k]));
643  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
644  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
645  }
646  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
647  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
648  }
649  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
650  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
651  }
652  if (mr->map().at("base")== ps->allowed_collision_matrix.entry_names[k]){
653  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
654  }
655  }
656  }
657 
658  if(std::regex_match(ps->allowed_collision_matrix.entry_names[j], match, rx_table)){
659  for(int k = 0; k < ps->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
660  int distance = std::distance(acm_.begin(),acm_.find(ps->allowed_collision_matrix.entry_names[k]));
661  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
662  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
663  }
664  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_box)){
665  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
666  }
667  if (std::regex_match(ps->allowed_collision_matrix.entry_names[k], match, rx_table)){
668  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
669  }
670  if (mr->map().at("base")== ps->allowed_collision_matrix.entry_names[k]){
671  acm_.at(ps->allowed_collision_matrix.entry_names[k])[distance] = ps->allowed_collision_matrix.entry_values[j].enabled[k];
672  }
673  }
674  }
675  }
676  merge_ps(ps_m, ps, mr);
677  }
678  s_qets.second.front().solution.sub_trajectory.erase(s_qets.second.front().solution.sub_trajectory.begin());
679  }
680  } else {s_qets.second.pop();};
681  }
682  }
683  }
684  for(int i = 0; i < th.size(); i++){
685  if(th[i].joinable()) th[i].join();
686  }
687  merge_acm(ps_m);
688  if(ps_m.is_diff) planning_scene_diff_publisher_->publish(ps_m);
689  }
690 
691  }
692 
693 }
694 
695 
697  /* There are 2 ways to interprete a Task
698  1. A box position in acm is also the first entry in task,
699  2. A box position in acm is not the first entry in task, in that case we can might try for each position
700  */
701  auto jq = job_reader_->robot_data();
702  auto cd = cuboid_reader_->cuboid_bin();
703 
704  //std::vector<std::string> objs = {"bottle1", "bottle2"};
705 
706  for (int i = 0; i < cd.size(); i ++){
707  std::stringstream ss;
708  ss << "box_" << i;
709 
710  moveit_msgs::CollisionObject item;
711  item.id = ss.str();
712  item.header.frame_id = "world";
713  item.header.stamp = ros::Time::now();
714  item.primitives.resize(1);
715  item.primitives[0].type = item.primitives[0].BOX;
716  item.primitives[0].dimensions.resize(3);
717  item.primitives[0].dimensions[0] = cd[i].x_depth;
718  item.primitives[0].dimensions[1] = cd[i].y_width;
719  item.primitives[0].dimensions[2] = cd[i].z_heigth;
720 
721  item.primitive_poses.resize(1);
722  item.primitive_poses[0].position.x = cd[i].Pose.position.x;
723  item.primitive_poses[0].position.y = cd[i].Pose.position.y;
724  item.primitive_poses[0].position.z = cd[i].Pose.position.z;
725  item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
726  item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
727  item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
728  item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
729  item.operation = item.ADD;
730 
731  psi_->applyCollisionObject(item);
732  acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
733 
734  // Could also safe id's somewhere
735  }
736 
737  std::regex item("box_([0-9]+)");
738  std::smatch match;
739 
740  // circular buffer is job-q
741  while(!jq.empty()){
742  for(auto& s_co : psi_->getObjects()){
743  if(!std::regex_match(s_co.first, match, item)) continue;
744  std::pair<std::string, job_data> temp = jq.front();
745  ROS_INFO("1. job entry %f %f %f", temp.second.jobs_.front().getOrigin().getX(), temp.second.jobs_.front().getOrigin().getY(), temp.second.jobs_.front().getOrigin().getZ());
746  ROS_INFO("object position %f %f %f", s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z);
747  if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), tf2::Vector3(s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z )) == 0) {
748  std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot;
749  // get rob by name
750  Moveit_robot* mr;
751  for(auto* r: robots_) if (r->name() == temp.first) mr = dynamic_cast<Moveit_robot*>(r);
752 
753  // loop jobs
754  for (int k = 1; k < temp.second.jobs_.size(); k++){
755  moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(s_co.first), temp.second.jobs_[k]);
756  if(mgt.plan(1)) {
757  moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
758  mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
759  tasks_per_robot.push(e);
760 
761  moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first);
762  temp_co.operation = temp_co.MOVE;
763  temp_co.pose.position.x = temp.second.jobs_[k].getOrigin().getX();
764  temp_co.pose.position.y = temp.second.jobs_[k].getOrigin().getY();
765  temp_co.pose.position.z = temp.second.jobs_[k].getOrigin().getZ();
766  temp_co.pose.orientation.x = temp.second.jobs_[k].getRotation().getX();
767  temp_co.pose.orientation.y = temp.second.jobs_[k].getRotation().getY();
768  temp_co.pose.orientation.z = temp.second.jobs_[k].getRotation().getZ();
769  temp_co.pose.orientation.w = temp.second.jobs_[k].getRotation().getW();
770  psi_->applyCollisionObject(temp_co);
771  }
772  }
773  std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> pair(temp.second.jobs_.front().getOrigin(), tasks_per_robot);
774  tasks_.insert(std::pair<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>(mr->name(), pair));
775  jq.pop_front();
776  } else {jq.push_back(temp);}
777  }
778  }
779 
780  // clean up
781  for (int i = 0; i < cd.size(); i ++){
782  std::stringstream ss;
783  ss << "box_" << i;
784 
785  moveit_msgs::CollisionObject item;
786  item.id = ss.str();
787  item.header.frame_id = "world";
788  item.header.stamp = ros::Time::now();
789  item.primitives.resize(1);
790  item.primitives[0].type = item.primitives[0].BOX;
791  item.primitives[0].dimensions.resize(3);
792  item.primitives[0].dimensions[0] = cd[i].x_depth;
793  item.primitives[0].dimensions[1] = cd[i].y_width;
794  item.primitives[0].dimensions[2] = cd[i].z_heigth;
795 
796  item.pose.position.x = cd[i].Pose.position.x;
797  item.pose.position.y = cd[i].Pose.position.y;
798  item.pose.position.z = cd[i].Pose.position.z;
799  item.pose.orientation.x = cd[i].Pose.orientation.x;
800  item.pose.orientation.y = cd[i].Pose.orientation.y;
801  item.pose.orientation.z = cd[i].Pose.orientation.z;
802  item.pose.orientation.w = cd[i].Pose.orientation.w;
803  item.operation = item.MOVE;
804 
805  psi_->applyCollisionObject(item);
806  // Could also safe id's somewhere
807  }
808 
809 }
810 
811 
812 void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
813  moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
814  acmt.entry_values.resize(acm_.size());
815 
816  int i = 0;
817  for (auto& a : acm_){
818  acmt.entry_names.push_back(a.first);
819  acmt.entry_values[i].enabled = a.second;
820  i++;
821  }
822 
823  ps_m.allowed_collision_matrix = acmt;
824  ROS_INFO("broken after merge");
825 
826 }
827 
828 void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr){
829  // get full mr link list
830  std::vector<std::string> links;
831  for (auto link : mr->mgi()->getLinkNames())links.push_back(link);
832  links.push_back(mr->map()["left_finger"]);
833  links.push_back(mr->map()["right_finger"]);
834  links.push_back(mr->map()["hand_link"]);
835  links.push_back(mr->map()["base"]);
836 
837 
838  for (auto ao : in->robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
839  if (in->robot_state.is_diff) out.robot_state.is_diff = in->robot_state.is_diff;
840  if (in->is_diff) out.is_diff = in->is_diff;
841  out.robot_state.joint_state.header = in->robot_state.joint_state.header;
842  out.robot_model_name = "panda";
843 
844 
845 
846  for (auto link : links) {
847  for(int i = 0; i < in->robot_state.joint_state.name.size(); i++){
848  if(link.c_str() == in->robot_state.joint_state.name[i].c_str()){
849  out.robot_state.joint_state.effort.push_back(in->robot_state.joint_state.effort[i]);
850  out.robot_state.joint_state.position.push_back(in->robot_state.joint_state.position[i]);
851  out.robot_state.joint_state.velocity.push_back(in->robot_state.joint_state.velocity[i]);
852  ROS_INFO("check");
853  }
854  }
855 
856  for(int i = 0; i < in->link_padding.size(); i++){
857  if(link.c_str() == in->link_padding[i].link_name.c_str()){
858  out.link_padding.push_back(in->link_padding[i]);
859  }
860  }
861 
862  for(int i = 0; i < in->link_scale.size(); i++){
863  if(link.c_str() == in->link_scale[i].link_name.c_str()){
864  out.link_scale.push_back(in->link_scale[i]);
865  }
866  }
867  }
868 
869 }
870 
871 void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
872  mr.mgi()->execute(rt);
873 }
874 
875 
876 
877 moveit::task_constructor::Task Moveit_mediator::create_Task(Moveit_robot* mr, moveit_msgs::CollisionObject& source, tf2::Transform& target){
878  tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));
879  std::string support_surface1 = "nichts";
880  std::string support_surface2 = "nichts";
881 
882 
883  for (Abstract_robot* ar : robots_){
884  std::string str;
885  if(ar->check_single_object_collision(t, str)) support_surface1 = str;
886  if(ar->check_single_object_collision(target, str)) support_surface2= str;
887  }
888 
889  ROS_INFO("ss1 %s", support_surface1.c_str());
890  ROS_INFO("ss2 %s", support_surface2.c_str());
891 
892 
893  const std::string object = source.id;
894  moveit::task_constructor::Task task_;
895 
896  std::string name = "Pick&Place";
897  task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
898  task_.loadRobotModel();
899  task_.setRobotModel(mr->mgi()->getRobotModel());
900 
901  // Set task properties
902  task_.setProperty("group", mr->name());
903  task_.setProperty("eef", mr->map()["eef_name"]);
904  task_.setProperty("hand", mr->map()["hand_group_name"]);
905  task_.setProperty("hand_grasping_frame", mr->map()["hand_frame"]);
906  task_.setProperty("ik_frame", mr->map()["hand_frame"]);
907 
908  moveit::task_constructor::Stage* current_state_ptr = nullptr;
909  {
910  auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
911  auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
912  applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
913  if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
914  comment = "object with id '" + object + "' is already attached and cannot be picked";
915  return false;
916  }
917  return true;
918  });
919 
920  current_state_ptr = applicability_filter.get();
921  task_.add(std::move(applicability_filter));
922  }
923 
924  { // Open Hand
925  auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
926  stage->setGroup(mr->map()["eef_name"]);
927  stage->setGoal("open");
928  task_.add(std::move(stage));
929  }
930 
931  { // Move-to pre-grasp
932  auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
933  "move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_} });
934  stage->setTimeout(5.0);
935  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
936  task_.add(std::move(stage));
937  }
938 
939  moveit::task_constructor::Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
940  {
941  auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object");
942  task_.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
943  grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
944 
945 
946  { // Approach Obj
947  auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner_);
948  stage->properties().set("marker_ns", "approach_object");
949  stage->properties().set("link", mr->map()["hand_frame"]);
950  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
951  stage->setMinMaxDistance(0.07, 0.2);
952 
953  // Set hand forward direction
954  geometry_msgs::Vector3Stamped vec;
955  vec.header.frame_id = mr->map()["hand_frame"];
956  vec.vector.z = 1.0;
957  stage->setDirection(vec);
958  grasp->insert(std::move(stage));
959  }
960 
961  {
962  // Sample grasp pose
963  auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose");
964  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
965  stage->properties().set("marker_ns", "grasp_pose");
966  stage->setPreGraspPose("open");
967  stage->setObject(object);
968  stage->setAngleDelta(M_PI / 12);
969  stage->setMonitoredStage(current_state_ptr); // Hook into current state
970 
971  // Compute IK
972  Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
973  Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
974  Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
975  Eigen::Translation3d trans(0.1f, 0, 0);
976  Eigen::Isometry3d ik = eigen * trans;
977 
978  auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage));
979  wrapper->setMaxIKSolutions(8);
980  wrapper->setMinSolutionDistance(1.0);
981  wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
982  wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
983  wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
984  grasp->insert(std::move(wrapper));
985  }
986 
987  {
988  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
989  stage->allowCollisions(
990  object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
991  true);
992  grasp->insert(std::move(stage));
993  }
994 
995  {
996  auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
997  stage->setGroup(mr->map()["eef_name"]);
998  stage->setGoal("close");
999  grasp->insert(std::move(stage));
1000  }
1001 
1002  { // Attach obj
1003  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
1004  stage->attachObject(object, mr->map()["hand_frame"]);
1005  attach_object_stage = stage.get();
1006  grasp->insert(std::move(stage));
1007  }
1008 
1009  { // Allow Collision obj table
1010  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)");
1011  stage->allowCollisions({ object }, support_surface1, true);
1012  grasp->insert(std::move(stage));
1013  }
1014 
1015  {
1016  auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner_);
1017  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
1018  stage->setMinMaxDistance(0.1, 0.2);
1019  stage->setIKFrame(mr->map()["hand_frame"]);
1020  stage->properties().set("marker_ns", "lift_object");
1021 
1022  // Set upward direction
1023  geometry_msgs::Vector3Stamped vec;
1024  vec.header.frame_id = "world";
1025  vec.vector.z = 1.0;
1026  stage->setDirection(vec);
1027  grasp->insert(std::move(stage));
1028  }
1029 
1030  { // forbid collision
1031  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
1032  stage->allowCollisions({ object }, support_surface1, false);
1033  grasp->insert(std::move(stage));
1034  }
1035 
1036  // Add grasp container to task
1037  task_.add(std::move(grasp));
1038  }
1039 
1040  {
1041  auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
1042  "move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_ } });
1043  stage->setTimeout(5.0);
1044  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
1045  task_.add(std::move(stage));
1046  }
1047 
1048  {
1049  auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object");
1050  task_.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
1051  place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" });
1052 
1053  {
1054  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow cokbkmomsurface)");
1055  stage->allowCollisions( {object} , support_surface2, true);
1056  place->insert(std::move(stage));
1057  }
1058 
1059 
1060  {
1061  auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner_);
1062  stage->properties().set("marker_ns", "lower_object");
1063  stage->properties().set("link", mr->map()["hand_frame"]);
1064  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
1065  stage->setMinMaxDistance(.03, .13);
1066 
1067  // Set downward direction
1068  geometry_msgs::Vector3Stamped vec;
1069  vec.header.frame_id = "world";
1070  vec.vector.z = -1.0;
1071  stage->setDirection(vec);
1072  place->insert(std::move(stage));
1073  }
1074 
1075  {
1076  // Generate Place Pose
1077  auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose");
1078  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "ik_frame" });
1079  stage->properties().set("marker_ns", "place_pose");
1080  stage->setObject(object);
1081 
1082  // Set target pose
1083  geometry_msgs::PoseStamped p;
1084  p.header.frame_id = "world";
1085  p.pose.position.x = target.getOrigin().getX();
1086  p.pose.position.y = target.getOrigin().getY();
1087  p.pose.position.z = target.getOrigin().getZ();
1088  p.pose.orientation.x = target.getRotation().getX();
1089  p.pose.orientation.y = target.getRotation().getY();
1090  p.pose.orientation.z = target.getRotation().getZ();
1091  p.pose.orientation.w = target.getRotation().getW();
1092 
1093 
1094 
1095  stage->setPose(p);
1096  stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage
1097 
1098  // Compute IK
1099  Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
1100  Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
1101  Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
1102  Eigen::Translation3d trans(0.1f, 0, 0);
1103  Eigen::Isometry3d ik = eigen * trans;
1104  auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage));
1105  wrapper->setMaxIKSolutions(2);
1106  wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
1107  wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
1108  wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
1109  place->insert(std::move(wrapper));
1110  }
1111 
1112  {
1113  auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
1114  stage->setGroup(mr->map()["eef_name"]);
1115  stage->setGoal("open");
1116  place->insert(std::move(stage));
1117  }
1118 
1119  {
1120  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
1121  stage->allowCollisions(
1122  object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
1123  false);
1124  place->insert(std::move(stage));
1125  }
1126 
1127  {
1128  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object");
1129  stage->detachObject(object, mr->map()["hand_frame"]);
1130  place->insert(std::move(stage));
1131  }
1132 
1133  {
1134  auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner_);
1135  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
1136  stage->setMinMaxDistance(.1, .2);
1137  stage->setIKFrame(mr->map()["hand_frame"]);
1138  stage->properties().set("marker_ns", "retreat");
1139  geometry_msgs::Vector3Stamped vec;
1140  vec.header.frame_id = mr->map()["hand_frame"];
1141  vec.vector.z = -1.0;
1142  stage->setDirection(vec);
1143  place->insert(std::move(stage));
1144  }
1145 
1146  // Add place container to task
1147  task_.add(std::move(place));
1148  }
1149 
1150  {
1151  auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner_);
1152  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
1153  stage->setGoal("ready");
1154  stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
1155  task_.add(std::move(stage));
1156  }
1157 
1158  return task_;
1159 }
1160 
1161 
1162 void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){
1163  tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));
1164  // unique ids cartesian, samplimg, interpolation, ready, hand_open, pick_up
1165 
1166  std::string support_surface1 = "nichts";
1167  std::string support_surface2 = "nichts";
1168 
1169  XmlRpc::XmlRpcValue task;
1170  nh_->getParam("task/stages", task);
1171 
1172 
1173 
1174  for (Abstract_robot* ar : robots_){
1175  std::string str;
1176  if(ar->check_single_object_collision(t, str)) support_surface1 = str;
1177  if(ar->check_single_object_collision(target, str)) support_surface2= str;
1178  }
1179 
1180  Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r);
1181 
1182  ROS_INFO("%f %f %f source", t.getOrigin().getX(), t.getOrigin().getY(), t.getOrigin().getZ());
1183 
1184  ROS_INFO("surface1 %s", support_surface1.c_str());
1185  ROS_INFO("surface2 %s", support_surface2.c_str());
1186 
1187 
1188  nh_->setParam("/task/properties/group", r->name());
1189  nh_->setParam("/task/properties/eef", mr->map()["eef"]);
1190  nh_->setParam("/task/properties/hand_grasping_frame", mr->map()["hand_grasping_frame"]);
1191  nh_->setParam("/task/properties/ik_frame", mr->map()["ik_frame"]);
1192  nh_->setParam("/task/properties/hand", mr->map()["hand"]);
1193 
1194  XmlRpc::XmlRpcValue a, b, c, d, e, h;
1195  a["group"] = mr->map()["hand"];
1196  e["joint_model_group_name"] = mr->map()["hand"];
1197 
1198  b = task[4]["stages"];
1199  b[1]["stage"]["properties"]["object"] = source.id;
1200  b[3]["properties"] = a;
1201  b[2]["set"]["allow_collisions"]["first"] = source.id;
1202  b[2]["set"]["allow_collisions"]["second"] = e;
1203 
1204  c = task[6]["stages"];
1205 
1206  c[0]["set"]["allow_collisions"]["first"] = source.id;
1207  c[0]["set"]["allow_collisions"]["second"] = support_surface2;
1208  c[3]["properties"] = a;
1209  c[6]["properties"] = a;
1210  c[4]["set"]["allow_collisions"]["first"] = source.id;
1211  c[4]["set"]["allow_collisions"]["second"] = e;
1212 
1213  c[2]["stage"]["set"]["pose"]["point"]["x"]= static_cast<double>(target.getOrigin().getX());
1214  c[2]["stage"]["set"]["pose"]["point"]["y"]= static_cast<double>(target.getOrigin().getY());
1215  c[2]["stage"]["set"]["pose"]["point"]["z"]= static_cast<double>(target.getOrigin().getZ());
1216 
1217 
1218  task[2]["properties"] = a;
1219 
1220  XmlRpc::XmlRpcValue connect, f, g;
1221  f[r->name()] = "sampling";
1222  g["source"] = "PARENT";
1223  connect["type"] = "Connect";
1224  connect["group_planner_vector"] = f;
1225  connect["propertiesConfigureInitFrom"] = g;
1226  task[5] = connect;
1227  task[3] = connect;
1228 
1229  a.clear();
1230  e.clear();
1231  a["link"] = mr->map()["ik_frame"];
1232  a["min_distance"] = 0.07;
1233  a["max_distance"] = 0.2;
1234  c[5]["properties"] = a;
1235  b[0]["properties"] = a;
1236  a["min_distance"] = 0.1;
1237  a["max_distance"] = 0.2;
1238  c[1]["properties"] = a;
1239 
1240  e["object"] = "bottle";
1241  e["link"] = mr->map()["ik_frame"];
1242  b[4]["set"]["attach_object"] = e;
1243  c[4]["set"]["detach_object"] = e;
1244  c[2]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"];
1245  c[5]["set"]["direction"]["vector"]["header"]["frame_id"] = mr->map()["ik_frame"];
1246  b[1]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"];
1247  b[6]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"];
1248  b[0]["set"]["direction"]["vector"]["header"]["frame_id"] = mr->map()["ik_frame"];
1249 
1250  b[5]["set"]["allow_collisions"]["first"] = source.id;
1251  b[5]["set"]["allow_collisions"]["second"] = support_surface1;
1252  b[7]["set"]["allow_collisions"]["first"] = source.id;
1253  b[7]["set"]["allow_collisions"]["second"] = support_surface1;
1254 
1255  task[6]["stages"] = c;
1256  task[4]["stages"] = b;
1257 
1258  nh_->setParam("task/stages", task);
1259 
1260 }
1261 
1262 Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
1263  : Abstract_mediator(objects, pub), nh_(nh)
1264  , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
1265  , cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
1266  , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
1267  , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
1268  , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
1269  , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
1270  , job_reader_(std::make_unique<Job_reader>(nh_))
1271  , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
1272 
1273 
1274  if (planning_scene_monitor_->getPlanningScene()){
1275  planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
1276  "planning_scene");
1277  planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
1278  } else {
1279  ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
1280  return;
1281  }
1282 
1283  robot_model_loader::RobotModelLoaderPtr robot_model_loader;
1284  robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
1285  robot_model_ = robot_model_loader->getModel();
1286  ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
1287 
1288  // planner
1289  sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
1290 
1291  // cartesian
1292  cartesian_planner_->setMaxVelocityScaling(1.0);
1293  cartesian_planner_->setMaxAccelerationScaling(1.0);
1294  cartesian_planner_->setStepSize(.01);
1295 
1296  };
1297 
Moveit_mediator::planning_scene_diff_publisher_
std::shared_ptr< ros::Publisher > planning_scene_diff_publisher_
Definition: moveit_mediator.h:64
Moveit_robot::map
std::map< std::string, std::string > & map()
Definition: moveit_robot.h:48
Moveit_robot::mgi_hand
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi_hand()
Definition: moveit_robot.h:44
Robot::observers
std::vector< Abstract_robot_element * > & observers()
Definition: robot.h:29
Collision_helper
Definition: collision_helper.h:10
Wing_moveit_decorator
Definition: wing_moveit_decorator.h:13
Abstract_mediator
Abstract mediator.
Definition: impl/abstract_mediator.h:33
Moveit_mediator::publish_tables
void publish_tables()
Definition: moveit_mediator.cpp:24
Moveit_mediator::set_wings
void set_wings(std::vector< std::pair< std::vector< object_data >, int >> &wbp) override
Definition: moveit_mediator.cpp:87
Job_reader
Job reader.
Definition: job_reader.h:16
Wing_moveit_decorator::markers
moveit_msgs::CollisionObject * markers()
Definition: wing_moveit_decorator.h:21
Moveit_mediator::cartesian_planner_
std::shared_ptr< moveit::task_constructor::solvers::CartesianPath > cartesian_planner_
Definition: moveit_mediator.h:70
Cuboid_reader
Cuboid reader.
Definition: cuboid_reader.h:17
Moveit_mediator::nh_
std::shared_ptr< ros::NodeHandle > nh_
Definition: moveit_mediator.h:56
Collision_helper::collision_detector
bool collision_detector(const std::string &object1_name, const std::string &object2_name)
Definition: collision_helper.cpp:5
Moveit_mediator::job_reader_
std::unique_ptr< Job_reader > job_reader_
Definition: moveit_mediator.h:73
Moveit_mediator::cuboid_reader_
std::unique_ptr< Cuboid_reader > cuboid_reader_
Definition: moveit_mediator.h:74
Abstract_mediator::wings_
std::vector< std::vector< Abstract_robot_element * > > wings_
Definition: impl/abstract_mediator.h:40
task__
thread_local moveit::task_constructor::Task task__
Definition: moveit_mediator.cpp:8
Moveit_mediator::merge_ps
void merge_ps(moveit_msgs::PlanningScene &out, moveit_msgs::PlanningScene *in, Moveit_robot *mr)
Definition: moveit_mediator.cpp:828
Moveit_mediator::check_collision
bool check_collision(const int &robot) override
Definition: moveit_mediator.cpp:61
Moveit_mediator::parallel_exec
void parallel_exec(Moveit_robot &r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps)
Definition: moveit_mediator.cpp:871
Moveit_mediator::connect_robots
void connect_robots(Abstract_robot *robot) override
Definition: moveit_mediator.cpp:14
Moveit_mediator::psi_
std::unique_ptr< moveit::planning_interface::PlanningSceneInterface > psi_
Definition: moveit_mediator.h:62
Abstract_robot::name
std::string & name()
Definition: impl/abstract_robot.h:57
Moveit_mediator::Moveit_mediator
Moveit_mediator(std::vector< std::vector< tf2::Transform >> objects, ros::Publisher *pub, std::shared_ptr< ros::NodeHandle > const &nh)
Definition: moveit_mediator.cpp:1262
Robot
Definition: robot.h:19
Abstract_robot_element
Definition: impl/abstract_robot_element.h:9
Moveit_mediator::merge_acm
void merge_acm(moveit_msgs::PlanningScene &in)
Definition: moveit_mediator.cpp:812
etsg_
thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_
Definition: moveit_mediator.cpp:9
Moveit_robot
Definition: moveit_robot.h:12
Moveit_mediator::setup_task
void setup_task()
Definition: moveit_mediator.cpp:98
Moveit_mediator::mediate
void mediate() override
Definition: moveit_mediator.cpp:67
Moveit_mediator::acm_
std::map< std::string, std::vector< uint8_t > > acm_
Definition: moveit_mediator.h:76
Moveit_mediator::sampling_planner_
std::shared_ptr< moveit::task_constructor::solvers::PipelinePlanner > sampling_planner_
Definition: moveit_mediator.h:69
Moveit_mediator::planning_scene_monitor_
std::shared_ptr< planning_scene_monitor::PlanningSceneMonitor > planning_scene_monitor_
Definition: moveit_mediator.h:65
Moveit_mediator::ps_
std::shared_ptr< planning_scene::PlanningScene > ps_
Definition: moveit_mediator.h:63
Abstract_mediator::robots_
std::vector< Abstract_robot * > robots_
Definition: impl/abstract_mediator.h:35
moveit_mediator.h
Moveit_mediator::rewrite_task_template
void rewrite_task_template(Abstract_robot *r, moveit_msgs::CollisionObject &source, tf2::Transform &target)
Definition: moveit_mediator.cpp:1162
Moveit_mediator::robot_model_
std::shared_ptr< moveit::core::RobotModel > robot_model_
Definition: moveit_mediator.h:57
Wing
Definition: wing.h:7
Robot::register_observers
void register_observers(Abstract_robot_element *wd)
Definition: robot.cpp:182
Moveit_mediator::tasks_
std::multimap< std::string, std::pair< tf2::Vector3, std::queue< moveit_task_constructor_msgs::ExecuteTaskSolutionGoal > > > tasks_
Definition: moveit_mediator.h:77
Moveit_mediator::create_Task
moveit::task_constructor::Task create_Task(Moveit_robot *r, moveit_msgs::CollisionObject &source, tf2::Transform &target)
Definition: moveit_mediator.cpp:877
Moveit_mediator::task_planner
void task_planner()
Definition: moveit_mediator.cpp:696
Abstract_robot
Definition: impl/abstract_robot.h:25
task_writing
std::mutex task_writing
Definition: moveit_mediator.cpp:11
Moveit_robot::mgi
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi()
Definition: moveit_robot.h:43
Moveit_mediator::build_wings
void build_wings(std::bitset< 3 > &wing, int &robot) override
Definition: moveit_mediator.cpp:76


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