moveit_mediator.cpp
Go to the documentation of this file.
3 
4 #include <thread>
5 #include <mutex>
6 #include <algorithm>
7 #include <gb_grasp/MapGenerator.h>
8 #include <regex>
9 #include <actionlib_msgs/GoalStatusArray.h>
10 #include <chrono>
11 
12 MoveitMediator::MoveitMediator(std::shared_ptr<ros::NodeHandle> const& nh)
13  : AbstractMediator(nh)
14  , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
15  , cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
16  , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
17  , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
18  , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
19  , job_reader_(std::make_unique<JobReader>(nh_)){
20 
21  robot_model_loader::RobotModelLoaderPtr robot_model_loader;
22  robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
23  robot_model_ = robot_model_loader->getModel();
24  ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
25 
26  // planner
27  sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
28 
29  // cartesian
30  cartesian_planner_->setMaxVelocityScaling(1.0);
31  cartesian_planner_->setMaxAccelerationScaling(1.0);
32  cartesian_planner_->setStepSize(.01);
33 
34  };
35 
36 void MoveitMediator::connectRobots(std::unique_ptr<AbstractRobotDecorator> robot) {
37  ROS_INFO("connecting %s...", robot->name().c_str());
38  robot->spezifieRobotGroups();
39 
40 
41  acm_.insert(std::pair<std::string, std::vector<uint8_t>>(robot->map().at("base").c_str(), std::vector<uint8_t>()));
42 
43 
44  for(auto name: robot->mgi()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
45  for(auto name: robot->mgiHand()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
46 
47  for (auto link : robot->mgi()->getLinkNames()) rs_.insert_or_assign(link, std::vector<std::uint8_t>());
48  rs_.insert_or_assign(robot->map()["left_finger"], std::vector<std::uint8_t>());
49  rs_.insert_or_assign(robot->map()["right_finger"], std::vector<std::uint8_t>());
50  rs_.insert_or_assign(robot->map()["hand_link"], std::vector<std::uint8_t>());
51  rs_.insert_or_assign(robot->map()["base"], std::vector<std::uint8_t>());
52 
53  robots_.insert_or_assign(robot->name(), std::move(robot));
54 }
55 
56 
58  auto wd = wing_reader_->wingData();
59 
60  for (const auto& s_r : robots_){
61  try{
62  auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
63  ceti_bot->setObserverMask(static_cast<wing_config>(wd.at(s_r.first).second));
64  } catch(const std::out_of_range& oor) {
65  ROS_INFO("No mask data for %s", s_r.first.c_str());
66  }
67  }
68 
69  for (const auto& s_r : robots_){
70  auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
71  std::vector<std::unique_ptr<AbstractRobotElementDecorator>> panels(3);
72 
73  for (std::size_t j = 0; j < ceti_bot->observerMask().size(); j++){
74  if (ceti_bot->observerMask()[j]){
75  try{
76  tf2::Transform relative_tf = ceti_bot->tf().inverse() * wd.at(ceti_bot->name()).first[j].pose_;
77 
78  std::unique_ptr<AbstractRobotElement> panel = std::make_unique<MoveitPanel>(wd.at(s_r.first).first[j].name_, relative_tf, wd.at(s_r.first).first[j].size_);
79  std::unique_ptr<AbstractRobotElementDecorator> log = std::make_unique<LogDecorator>(std::move(panel));
80  panels[j] = std::move(log);
81  } catch (std::out_of_range &oor){
82  ROS_INFO("OOR in set_panel");
83  }
84  }
85  }
86  ceti_bot->setObservers(panels);
87  }
88 
89  for (const auto& s_r : robots_){
90  s_r.second->notify();
91  }
92 }
93 
95  ros::WallDuration sleep_time(1.0);
96 
97  for (const auto& s_r : robots_){
98  auto ceti_bot = dynamic_cast<CetiRobot*>(s_r.second->next());
99 
100  for (std::size_t k = 0; k < ceti_bot->observerMask().size(); k++){
101  if(ceti_bot->observerMask()[k]){
102  auto wmd = dynamic_cast<MoveitPanel*>(ceti_bot->observers()[k]->next());
103  psi_->applyCollisionObject(wmd->marker());
104  acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->marker().id.c_str(), std::vector<uint8_t>()));
105  sleep_time.sleep();
106  }
107  }
108  }
109 }
110 
112  setPanel();
113  publishTables();
114  taskPlanner();
115  ros::waitForShutdown();
116 }
117 
118 void MoveitMediator::manipulateACM(AbstractRobotDecorator* mr, moveit_msgs::PlanningScene& ps){
119  // First find ID from panda to start with
120  std::regex rx_panda(mr->pattern());
121  std::smatch match;
122  std::regex_match(mr->name(), match, rx_panda);
123 
124  // build panda link regex
125  std::stringstream ss;
126  ss << "panda_" << match[1] << "_.*";
127  std::regex rx_panda_links(ss.str());
128  std::regex rx_box("box_.*");
129  std::regex rx_table("table.*");
130 
131 
132  // Iterate task collsion matrix
133  for(int j = 0; j < ps.allowed_collision_matrix.entry_names.size(); j++ ){
134  if( mr->map().at("base") == ps.allowed_collision_matrix.entry_names[j]){
135  //In case an entry matches an robot spezific link
136 
137  for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
138  // For that specific entry, loop over values
139  int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
140 
141  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
142  acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
143  }
144 
145  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
146  acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
147  }
148 
149  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
150  acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
151  }
152 
153  if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
154  acm_.at(mr->map().at("base"))[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
155  }
156  }
157  }
158 
159  if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
160  //In case an entry matches an robot spezific link
161  ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
162  for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
163  //For that specific entry, loop over values
164  int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
165 
166  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
167  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
168  }
169 
170  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
171  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
172  }
173 
174  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
175  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
176  }
177 
178  if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
179  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
180  }
181  }
182  }
183 
184  if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_box)){
185  //In case an entry matches an robot spezific link
186  for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
187  //For that specific entry, loop over values
188  int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
189 
190  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
191  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
192  }
193 
194  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
195  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
196  }
197 
198  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
199  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
200  }
201 
202  if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
203  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
204  }
205  }
206  }
207 
208  if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_table)){
209  //In case an entry matches an robot spezific link
210  for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
211  //For that specific entry, loop over values
212  int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));
213 
214  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
215  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
216  }
217 
218  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
219  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
220  }
221 
222  if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
223  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
224  }
225 
226  if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
227  acm_.at(ps.allowed_collision_matrix.entry_names[j])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
228  }
229  }
230  }
231  }
232 }
233 
234 
235 
237  /* There are 2 ways to interprete a Task
238  1. A box position in acm is also the first entry in task,
239  2. A box position in acm is not the first entry in task, in that case we can might try for each position
240  */
241  auto jq = job_reader_->robotData();
242  auto cd = cuboid_reader_->cuboidBox();
243 
244  //std::vector<std::string> objs = {"bottle1", "bottle2"};
245 
246  for (int i = 0; i < cd.size(); i ++){
247  std::stringstream ss;
248  ss << "box_" << i;
249 
250  moveit_msgs::CollisionObject item;
251  item.id = ss.str();
252  item.header.frame_id = "world";
253  item.header.stamp = ros::Time::now();
254  item.primitives.resize(1);
255  item.primitives[0].type = item.primitives[0].BOX;
256  item.primitives[0].dimensions.resize(3);
257  item.primitives[0].dimensions[0] = cd[i].x_depth;
258  item.primitives[0].dimensions[1] = cd[i].y_width;
259  item.primitives[0].dimensions[2] = cd[i].z_heigth;
260 
261  item.primitive_poses.resize(1);
262  item.primitive_poses[0].position.x = cd[i].Pose.position.x;
263  item.primitive_poses[0].position.y = cd[i].Pose.position.y;
264  item.primitive_poses[0].position.z = cd[i].Pose.position.z;
265  item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
266  item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
267  item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
268  item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
269  item.operation = item.ADD;
270 
271  psi_->applyCollisionObject(item);
272  acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
273 
274  // Could also safe id's somewhere
275  }
276 
277  // Setup shared ACM
278  for(auto& a: acm_) a.second.resize(acm_.size(), 0);
279 
280  std::regex item("box_([0-9]+)");
281  std::smatch match;
282 
283  // ----------------------------------
284  // GROOT Strategie => erst FROM TEXT
285  // ----------------------------------
286 
287  BT::BehaviorTreeFactory factory;
288  factory.registerNodeType<Execution>("Execution");
289  factory.registerNodeType<PositionCondition>("PositionCondition");
290  factory.registerNodeType<Parallel_robot>("Parallel_robot");
291 
292  std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
293 
294  while(!jq.empty()){
295  for(auto& s_co : psi_->getObjects()){
296  if(!std::regex_match(s_co.first, match, item)) continue;
297 
298  std::pair<std::string, job_data> temp = jq.front();
299  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());
300  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);
301  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) {
302  std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> bt_list;
303 
304  AbstractRobotDecorator* ard = nullptr;
305  try{
306  ard= robots_.at(temp.first).get();
307  } catch (std::out_of_range& oor){
308  ROS_INFO("Robot not found");
309  ros::shutdown();
310  }
311 
312  // loop jobs
313  for (int k = 1; k < temp.second.jobs_.size(); k++){
314  moveit::task_constructor::Task mgt = createTask(ard, psi_->getObjects().at(s_co.first), temp.second.jobs_[k]);
315  if(mgt.plan(1)) {
316 
317  moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
318  mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
319  bt_list.push_back(e);
320 
321  moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first);
322  temp_co.operation = temp_co.MOVE;
323  temp_co.pose.position.x = temp.second.jobs_[k].getOrigin().getX();
324  temp_co.pose.position.y = temp.second.jobs_[k].getOrigin().getY();
325  temp_co.pose.position.z = temp.second.jobs_[k].getOrigin().getZ();
326  temp_co.pose.orientation.x = temp.second.jobs_[k].getRotation().getX();
327  temp_co.pose.orientation.y = temp.second.jobs_[k].getRotation().getY();
328  temp_co.pose.orientation.z = temp.second.jobs_[k].getRotation().getZ();
329  temp_co.pose.orientation.w = temp.second.jobs_[k].getRotation().getW();
330  psi_->applyCollisionObject(temp_co);
331  }
332  }
333 
334  /*
335  if(auto condition = dynamic_cast<Position_condition*>(node_it->get())) {ROS_INFO("om");condition->init(s_co.first, temp.second.jobs_.front().getOrigin(), psi_.get()); ++node_it;}
336 
337  for(auto& ets : bt_list)
338  if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("tr");execution->init(planning_scene_diff_publisher_.get(), mr->mgi().get(), ets); ++node_it;}
339  */
340 
341  for (int i = 0; i < cd.size(); i ++){
342  std::stringstream ss;
343  ss << "box_" << i;
344 
345  moveit_msgs::CollisionObject item;
346  item.id = ss.str();
347  item.header.frame_id = "world";
348  item.header.stamp = ros::Time::now();
349  item.primitives.resize(1);
350  item.primitives[0].type = item.primitives[0].BOX;
351  item.primitives[0].dimensions.resize(3);
352  item.primitives[0].dimensions[0] = cd[i].x_depth;
353  item.primitives[0].dimensions[1] = cd[i].y_width;
354  item.primitives[0].dimensions[2] = cd[i].z_heigth;
355 
356  item.pose.position.x = cd[i].Pose.position.x;
357  item.pose.position.y = cd[i].Pose.position.y;
358  item.pose.position.z = cd[i].Pose.position.z;
359  item.pose.orientation.x = cd[i].Pose.orientation.x;
360  item.pose.orientation.y = cd[i].Pose.orientation.y;
361  item.pose.orientation.z = cd[i].Pose.orientation.z;
362  item.pose.orientation.w = cd[i].Pose.orientation.w;
363  item.operation = item.MOVE;
364 
365  psi_->applyCollisionObject(item);
366  // Could also safe id's somewhere
367  }
368 
369 
370  try{
371  task_but_different.at(ard->name()).push_back(std::tuple<std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list));
372  } catch(std::out_of_range &oor) {
373  std::tuple<const std::string&, tf2::Vector3&, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>&> tuple(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list);
374  task_but_different.insert(std::pair<std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>>(ard->name(),{tuple}));
375  }
376  jq.pop_front();
377  } else {jq.push_back(temp);}
378  }
379  }
380 
381  std::stringstream ss;
382  ss << "<root main_tree_to_execute = \"MainTree\">\n";
383  ss << "<BehaviorTree ID=\"MainTree\">\n";
384  ss << "<Control ID=\"Parallel\" name=\"Agents\" success_threshold=\""<< 2 << "\" failure_threshold=\""<< 1 << "\">\n";
385 
386  for (const auto& s_r : robots_){
387  try {
388  auto a = task_but_different.at(s_r.first);
389  } catch (std::out_of_range& oor){
390  continue;
391  }
392  ss << "<Control ID=\"Parallel_robot\" name=\""<< s_r.first << "\" success_threshold=\""<< task_but_different.at(s_r.first).size() << "\" failure_threshold=\""<< 1 << "\">\n";
393  for (auto& p_obj: task_but_different.at(s_r.first)){
394  ss << "<SequenceStar name=\"root_sequence\">\n";
395  ss << "<Condition ID=\"PositionCondition\" name=\"Position_condition\"/>\n";
396  for(int j = 0; j < std::get<2>(p_obj).size(); j++){
397  ss << "<Action ID=\"Execution\" name=\"Execution\"/>\n";
398  }
399  ss << "</SequenceStar>\n";
400  }
401  ss << "</Control>\n";
402  }
403  ss << "</Control>\n";
404  ss << "</BehaviorTree>\n";
405  ss << "</root>\n";
406 
407  std::cout << ss.str();
408  auto tree = factory.createTreeFromText(ss.str());
409  auto node_it = tree.nodes.begin();
410 
411  for (const auto& s_r : robots_){
412  try {
413  auto a = task_but_different.at(s_r.first);
414  } catch (std::out_of_range& oor){
415  continue;
416  }
417  for (auto& p_obj: task_but_different.at(s_r.first)){
418  while (node_it->get()->type() == BT::NodeType::CONTROL) ++node_it;
419  if(node_it->get()->type() == BT::NodeType::ACTION) ROS_INFO("Action");
420  if(node_it->get()->type() == BT::NodeType::CONDITION) ROS_INFO("Condition");
421  if(node_it->get()->type() == BT::NodeType::CONTROL) ROS_INFO("Control");
422  if(node_it->get()->type() == BT::NodeType::DECORATOR) ROS_INFO("Decorator");
423 
424  if(auto condition = dynamic_cast<PositionCondition*>(node_it->get())) {ROS_INFO("init Condition");condition->init(std::get<0>(p_obj), std::get<1>(p_obj), psi_.get());++node_it;}
425  for(int j = 0; j < std::get<2>(p_obj).size(); j++){
426  if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("init Action");execution->init(&executions_, s_r.second.get(), std::get<2>(p_obj)[j]);++node_it;}
427  }
428  }
429  }
430 
431  BT::PublisherZMQ zmq(tree);
432  ros::Duration sleep(1);
433  BT::NodeStatus status = BT::NodeStatus::RUNNING;
434  auto t_start = std::chrono::high_resolution_clock::now();
435  while( status == BT::NodeStatus::RUNNING){
436  status = tree.tickRoot();
437 
438  // std::this_thread::sleep_for(std::chrono::milliseconds(100));
439 
440 
441  std::vector<std::thread> th;
442  moveit_msgs::PlanningScene ps_m;
443  bool is_executing = !(executions_.empty());
444 
445  for (const auto& s_r : robots_){
446  try{
447  ROS_INFO("%s", s_r.first.c_str());
448  auto temp = executions_.at(s_r.first);
449  th.push_back(std::thread(&MoveitMediator::parallelExec, this, std::ref(*s_r.second.get()), temp.first));
450  //mr->mgi()->execute(temp.first);
451  // ROS_INFO("acm in msg");
452  // for(int k = 0; k < temp.second.allowed_collision_matrix.entry_names.size(); k++){
453  // ROS_INFO("%s", temp.second.allowed_collision_matrix.entry_names[k].c_str());
454  // for (auto& enable : temp.second.allowed_collision_matrix.entry_values[k].enabled) std::cout << +enable << " ";
455  // std::cout<< "\n";
456  // }
457 
458  manipulateACM(s_r.second.get(), temp.second);
459  mergePS(ps_m, temp.second, s_r.second.get());
460  executions_.erase(s_r.first);
461  } catch (std::out_of_range& oor){}
462  }
463 
464  for(auto& t : th){
465  t.join();
466  }
467  if (is_executing){
468  mergeACM(ps_m);
469  planning_scene_diff_publisher_->publish(ps_m);
470  }
471 
472 
473  // for(auto& exec : executions_){
474  // for (int i = 0; i < robots_.size(); i++){
475  // if (exec.first == robots_[i]->name()){
476  // auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
477  // manipulate_acm(mr, exec.second.second);
478  // merge_ps(ps_m, exec.second.second, mr);
479 
480  // }
481  // }
482  // }
483 
484 
485  }
486  auto t_end = std::chrono::high_resolution_clock::now();
487  double elapsed_time_ms = std::chrono::duration<double, std::milli>(t_end-t_start).count();
488  ROS_INFO("%f", elapsed_time_ms);
489 
490  // clean up
491  for (int i = 0; i < cd.size(); i ++){
492  std::stringstream ss;
493  ss << "box_" << i;
494 
495  moveit_msgs::CollisionObject item;
496  item.id = ss.str();
497  item.header.frame_id = "world";
498  item.header.stamp = ros::Time::now();
499  item.primitives.resize(1);
500  item.primitives[0].type = item.primitives[0].BOX;
501  item.primitives[0].dimensions.resize(3);
502  item.primitives[0].dimensions[0] = cd[i].x_depth;
503  item.primitives[0].dimensions[1] = cd[i].y_width;
504  item.primitives[0].dimensions[2] = cd[i].z_heigth;
505 
506  item.pose.position.x = cd[i].Pose.position.x;
507  item.pose.position.y = cd[i].Pose.position.y;
508  item.pose.position.z = cd[i].Pose.position.z;
509  item.pose.orientation.x = cd[i].Pose.orientation.x;
510  item.pose.orientation.y = cd[i].Pose.orientation.y;
511  item.pose.orientation.z = cd[i].Pose.orientation.z;
512  item.pose.orientation.w = cd[i].Pose.orientation.w;
513  item.operation = item.MOVE;
514 
515  psi_->applyCollisionObject(item);
516  // Could also safe id's somewhere
517  }
518 }
519 
520 
521 void MoveitMediator::mergeACM(moveit_msgs::PlanningScene& ps_m){
522  moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
523  acmt.entry_values.resize(acm_.size());
524 
525  int i = 0;
526  for (auto& a : acm_){
527  acmt.entry_names.push_back(a.first);
528  acmt.entry_values[i].enabled = a.second;
529  i++;
530  }
531 
532  // ROS_INFO("merge_acm");
533  // for(int k = 0; k < acmt.entry_names.size(); k++){
534  // ROS_INFO("%s", acmt.entry_names[k].c_str());
535  // for (auto& enable : acmt.entry_values[k].enabled){
536  // std::cout << +enable << " ";
537  // }
538  // std::cout<< "\n";
539  // }
540 
541  ps_m.allowed_collision_matrix = acmt;
542 
543 }
544 
545 void MoveitMediator::mergePS(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene in, AbstractRobotDecorator* mr){
546  // get full mr link list
547 
548  for (auto ao : in.robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
549  out.robot_state.is_diff |= in.robot_state.is_diff;
550  out.is_diff |= in.is_diff;
551  out.robot_state.joint_state.header = in.robot_state.joint_state.header;
552  out.robot_model_name = "panda";
553 
554  std::vector<std::string> links;
555  for (auto link : mr->mgi()->getLinkNames()) links.push_back(link);
556  links.push_back(mr->map()["left_finger"]);
557  links.push_back(mr->map()["right_finger"]);
558  links.push_back(mr->map()["hand_link"]);
559  links.push_back(mr->map()["base"]);
560 
561  for (auto link : links) {
562  for(int i = 0; i < in.robot_state.joint_state.name.size(); i++){
563  if(link == in.robot_state.joint_state.name[i]){
564  out.robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]);
565  out.robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]);
566  out.robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
567  }
568  }
569 
570  for(int i = 0; i < in.link_padding.size(); i++){
571  if(link == in.link_padding[i].link_name){
572  out.link_padding.push_back(in.link_padding[i]);
573  }
574  }
575 
576  for(int i = 0; i < in.link_scale.size(); i++){
577  if(link == in.link_scale[i].link_name){
578  out.link_scale.push_back(in.link_scale[i]);
579  }
580  }
581  }
582 }
583 
584 
585 void MoveitMediator::parallelExec(AbstractRobotDecorator& mr, moveit_msgs::RobotTrajectory rt){
586  mr.mgi()->execute(rt);
587 }
588 
589 moveit::task_constructor::Task MoveitMediator::createTask(AbstractRobotDecorator* mr, moveit_msgs::CollisionObject& source, tf2::Transform& target){
590  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));
591  std::string support_surface1 = "nichts";
592  std::string support_surface2 = "nichts";
593 
594  for (const auto& s_r : robots_){
595  std::string str;
596  std::bitset<3> panel_mask;
597  if(s_r.second->checkSingleObjectCollision(t, str, panel_mask)) support_surface1 = str;
598  if(s_r.second->checkSingleObjectCollision(target, str, panel_mask)) support_surface2= str;
599  }
600 
601  ROS_INFO("ss1 %s", support_surface1.c_str());
602  ROS_INFO("ss2 %s", support_surface2.c_str());
603 
604 
605  const std::string object = source.id;
606  moveit::task_constructor::Task task_;
607 
608  std::string name = "Pick&Place";
609  task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
610  task_.loadRobotModel();
611  task_.setRobotModel(mr->mgi()->getRobotModel());
612 
613  // Set task properties
614  task_.setProperty("group", mr->name());
615  task_.setProperty("eef", mr->map()["eef_name"]);
616  task_.setProperty("hand", mr->map()["hand_group_name"]);
617  task_.setProperty("hand_grasping_frame", mr->map()["hand_frame"]);
618  task_.setProperty("ik_frame", mr->map()["hand_frame"]);
619 
620  moveit::task_constructor::Stage* current_state_ptr = nullptr;
621  {
622  auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
623  auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
624  applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
625  if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
626  comment = "object with id '" + object + "' is already attached and cannot be picked";
627  return false;
628  }
629  return true;
630  });
631 
632  current_state_ptr = applicability_filter.get();
633  task_.add(std::move(applicability_filter));
634  }
635 
636  { // Open Hand
637  auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
638  stage->setGroup(mr->map()["eef_name"]);
639  stage->setGoal("open");
640  task_.add(std::move(stage));
641  }
642 
643  { // Move-to pre-grasp
644  auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
645  "move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_} });
646  stage->setTimeout(5.0);
647  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
648  task_.add(std::move(stage));
649  }
650 
651  moveit::task_constructor::Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
652  {
653  auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object");
654  task_.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
655  grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
656 
657 
658  { // Approach Obj
659  auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner_);
660  stage->properties().set("marker_ns", "approach_object");
661  stage->properties().set("link", mr->map()["hand_frame"]);
662  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
663  stage->setMinMaxDistance(0.07, 0.2);
664 
665  // Set hand forward direction
666  geometry_msgs::Vector3Stamped vec;
667  vec.header.frame_id = mr->map()["hand_frame"];
668  vec.vector.z = 1.0;
669  stage->setDirection(vec);
670  grasp->insert(std::move(stage));
671  }
672 
673  {
674  // Sample grasp pose
675  auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose");
676  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
677  stage->properties().set("marker_ns", "grasp_pose");
678  stage->setPreGraspPose("open");
679  stage->setObject(object);
680  stage->setAngleDelta(M_PI / 2);
681  stage->setMonitoredStage(current_state_ptr); // Hook into current state
682 
683  // Compute IK
684  Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
685  Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
686  Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
687  Eigen::Translation3d trans(0.1f, 0, 0);
688  Eigen::Isometry3d ik = eigen * trans;
689 
690  auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage));
691  wrapper->setMaxIKSolutions(8);
692  wrapper->setMinSolutionDistance(1.0);
693  wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
694  wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
695  wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
696  grasp->insert(std::move(wrapper));
697  }
698 
699  {
700  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
701  stage->allowCollisions(
702  object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
703  true);
704  grasp->insert(std::move(stage));
705  }
706 
707  {
708  auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
709  stage->setGroup(mr->map()["eef_name"]);
710  stage->setGoal("close");
711  grasp->insert(std::move(stage));
712  }
713 
714  { // Attach obj
715  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
716  stage->attachObject(object, mr->map()["hand_frame"]);
717  attach_object_stage = stage.get();
718  grasp->insert(std::move(stage));
719  }
720 
721  { // Allow Collision obj table
722  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)");
723  stage->allowCollisions({ object }, support_surface1, true);
724  grasp->insert(std::move(stage));
725  }
726 
727  {
728  auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner_);
729  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
730  stage->setMinMaxDistance(0.1, 0.2);
731  stage->setIKFrame(mr->map()["hand_frame"]);
732  stage->properties().set("marker_ns", "lift_object");
733 
734  // Set upward direction
735  geometry_msgs::Vector3Stamped vec;
736  vec.header.frame_id = "world";
737  vec.vector.z = 1.0;
738  stage->setDirection(vec);
739  grasp->insert(std::move(stage));
740  }
741 
742  { // forbid collision
743  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
744  stage->allowCollisions({ object }, support_surface1, false);
745  grasp->insert(std::move(stage));
746  }
747 
748  // Add grasp container to task
749  task_.add(std::move(grasp));
750  }
751 
752  {
753  auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
754  "move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_ } });
755  stage->setTimeout(5.0);
756  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
757  task_.add(std::move(stage));
758  }
759 
760  {
761  auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object");
762  task_.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
763  place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" });
764 
765  {
766  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow cokbkmomsurface)");
767  stage->allowCollisions( {object} , support_surface2, true);
768  place->insert(std::move(stage));
769  }
770 
771 
772  {
773  auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner_);
774  stage->properties().set("marker_ns", "lower_object");
775  stage->properties().set("link", mr->map()["hand_frame"]);
776  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
777  stage->setMinMaxDistance(.03, .13);
778 
779  // Set downward direction
780  geometry_msgs::Vector3Stamped vec;
781  vec.header.frame_id = "world";
782  vec.vector.z = -1.0;
783  stage->setDirection(vec);
784  place->insert(std::move(stage));
785  }
786 
787  {
788  // Generate Place Pose
789  auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose");
790  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "ik_frame" });
791  stage->properties().set("marker_ns", "place_pose");
792  stage->setObject(object);
793 
794  // Set target pose
795  geometry_msgs::PoseStamped p;
796  p.header.frame_id = "world";
797  p.pose.position.x = target.getOrigin().getX();
798  p.pose.position.y = target.getOrigin().getY();
799  p.pose.position.z = target.getOrigin().getZ();
800  p.pose.orientation.x = target.getRotation().getX();
801  p.pose.orientation.y = target.getRotation().getY();
802  p.pose.orientation.z = target.getRotation().getZ();
803  p.pose.orientation.w = target.getRotation().getW();
804 
805 
806 
807  stage->setPose(p);
808  stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage
809 
810  // Compute IK
811  Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
812  Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
813  Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
814  Eigen::Translation3d trans(0.1f, 0, 0);
815  Eigen::Isometry3d ik = eigen * trans;
816  auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage));
817  wrapper->setMaxIKSolutions(2);
818  wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
819  wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
820  wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
821  place->insert(std::move(wrapper));
822  }
823 
824  {
825  auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
826  stage->setGroup(mr->map()["eef_name"]);
827  stage->setGoal("open");
828  place->insert(std::move(stage));
829  }
830 
831  {
832  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
833  stage->allowCollisions(
834  object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
835  false);
836  place->insert(std::move(stage));
837  }
838 
839  {
840  auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object");
841  stage->detachObject(object, mr->map()["hand_frame"]);
842  place->insert(std::move(stage));
843  }
844 
845  {
846  auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner_);
847  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
848  stage->setMinMaxDistance(.1, .2);
849  stage->setIKFrame(mr->map()["hand_frame"]);
850  stage->properties().set("marker_ns", "retreat");
851  geometry_msgs::Vector3Stamped vec;
852  vec.header.frame_id = mr->map()["hand_frame"];
853  vec.vector.z = -1.0;
854  stage->setDirection(vec);
855  place->insert(std::move(stage));
856  }
857 
858  {
859  auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
860  stage->setGroup(mr->map()["eef_name"]);
861  stage->setGoal("close");
862  place->insert(std::move(stage));
863  }
864 
865  // Add place container to task
866  task_.add(std::move(place));
867  }
868 
869  {
870  auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner_);
871  stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
872  stage->setGoal("ready");
873  stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
874  task_.add(std::move(stage));
875  }
876 
877  return task_;
878 }
879 
880 
881 
MoveitMediator::setPanel
void setPanel() override
Sets panels for robots.
Definition: moveit_mediator.cpp:57
MoveitMediator::rs_
std::map< std::string, std::vector< uint8_t > > rs_
shared robot state between all robots
Definition: moveit_mediator.h:70
MoveitMediator::acm_
std::map< std::string, std::vector< uint8_t > > acm_
shared allowed collision matrix between robots
Definition: moveit_mediator.h:69
abstract_mediator.h
CetiRobot::setObserverMask
void setObserverMask(int i)
Definition: ceti_robot.h:83
MoveitMediator::mergeACM
void mergeACM(moveit_msgs::PlanningScene &in)
Threaded function which calls executaion on a Robot.
Definition: moveit_mediator.cpp:521
MoveitPanel
Definition: moveit_panel.h:13
wing_config
wing_config
Definition: abstract_robot.h:10
MoveitMediator::executions_
std::map< std::string, std::pair< moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene > > executions_
Shared execution map containing groot node information.
Definition: moveit_mediator.h:72
MoveitMediator::job_reader_
std::unique_ptr< JobReader > job_reader_
jobReader instancde which reads task information
Definition: moveit_mediator.h:68
Execution
Execution node as StatefulActionNode.
Definition: execution.h:23
MoveitMediator::taskPlanner
void taskPlanner()
Threaded function which calls executaion on a Robot.
Definition: moveit_mediator.cpp:236
MoveitMediator::MoveitMediator
MoveitMediator(std::shared_ptr< ros::NodeHandle > const &nh)
AbstractMediator constructor.
Definition: moveit_mediator.cpp:12
AbstractRobotDecorator::pattern
std::string & pattern()
Definition: abstract_robot_decorator.h:82
AbstractMediator::wing_reader_
std::unique_ptr< WingReader > wing_reader_
Wing_reader which collects panel information of robots.
Definition: abstract_mediator.h:54
MoveitMediator::ps_
std::shared_ptr< planning_scene::PlanningScene > ps_
Shared Planning Scene.
Definition: moveit_mediator.h:63
AbstractMediator
AbstractMediator.
Definition: abstract_mediator.h:43
CetiRobot
Concrete Ceti-Robot.
Definition: ceti_robot.h:14
MoveitMediator::cartesian_planner_
std::shared_ptr< moveit::task_constructor::solvers::CartesianPath > cartesian_planner_
Moveit task Constructior cartesian planner.
Definition: moveit_mediator.h:66
MoveitMediator::parallelExec
void parallelExec(AbstractRobotDecorator &mr, moveit_msgs::RobotTrajectory rt)
Threaded function which calls executaion on a Robot.
Definition: moveit_mediator.cpp:585
Parallel_robot
Definition: parallel_robot.h:9
MoveitMediator::manipulateACM
void manipulateACM(AbstractRobotDecorator *mr, moveit_msgs::PlanningScene &ps)
Manipulate ACM by extracting inforamtion of spicified robot.
Definition: moveit_mediator.cpp:118
AbstractMediator::robots_
std::map< std::string, std::unique_ptr< AbstractRobotDecorator > > robots_
Robots agents.
Definition: abstract_mediator.h:47
AbstractRobotDecorator::name
std::string & name() override
Redirects name call to next_.
Definition: abstract_robot_decorator.h:47
AbstractRobotDecorator
Abstract Robot Decorator.
Definition: abstract_robot_decorator.h:22
MoveitMediator::planning_scene_diff_publisher_
std::shared_ptr< ros::Publisher > planning_scene_diff_publisher_
Publisher to manage PlanningScene diffs.
Definition: moveit_mediator.h:64
PositionCondition
PositionCondition Node.
Definition: position_condition.h:21
AbstractRobotDecorator::mgi
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > mgi()
Definition: abstract_robot_decorator.h:87
MoveitMediator::connectRobots
void connectRobots(std::unique_ptr< AbstractRobotDecorator > robot) override
connect robot and initialize Moveit components
Definition: moveit_mediator.cpp:36
MoveitMediator::psi_
std::unique_ptr< moveit::planning_interface::PlanningSceneInterface > psi_
PlanningSceneInteface to manage Scene Objects.
Definition: moveit_mediator.h:62
MoveitMediator::sampling_planner_
std::shared_ptr< moveit::task_constructor::solvers::PipelinePlanner > sampling_planner_
Moveit task Constructior simple planner.
Definition: moveit_mediator.h:65
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
MoveitMediator::robot_model_
std::shared_ptr< moveit::core::RobotModel > robot_model_
Moveit Robot-Model as specified in SDF.
Definition: moveit_mediator.h:60
moveit_mediator.h
MoveitMediator::createTask
moveit::task_constructor::Task createTask(AbstractRobotDecorator *r, moveit_msgs::CollisionObject &source, tf2::Transform &target)
Create Task with Moveit Task Constructor.
Definition: moveit_mediator.cpp:589
JobReader
Job reader.
Definition: job_reader.h:16
MoveitMediator::mergePS
void mergePS(moveit_msgs::PlanningScene &out, moveit_msgs::PlanningScene in, AbstractRobotDecorator *mr)
Merge Planning scene.
Definition: moveit_mediator.cpp:545
MoveitMediator::publishTables
void publishTables()
publish all panels in the planning scene
Definition: moveit_mediator.cpp:94
MoveitMediator::mediate
void mediate() override
mediator function
Definition: moveit_mediator.cpp:111


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