diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp index 1f350623e2d250bc7c0dd029e4ea754f567402b5..f4af9c2bd0d78e84e1045b8ae1aec9a7f3fb41c8 100644 --- a/src/impl/moveit_mediator.cpp +++ b/src/impl/moveit_mediator.cpp @@ -908,18 +908,18 @@ void Moveit_mediator::task_planner(){ t.join(); } - // for(auto& exec : executions_){ - // for (int i = 0; i < robots_.size(); i++){ - // if (exec.first == robots_[i]->name()){ - // auto mr = dynamic_cast<Moveit_robot*>(robots_[i]); - // manipulate_acm(mr, exec.second.second); - // merge_ps(ps_m, exec.second.second, mr); - // } - // } - // } - - // merge_acm(ps_m); - // planning_scene_diff_publisher_->publish(ps_m); + for(auto& exec : executions_){ + for (int i = 0; i < robots_.size(); i++){ + if (exec.first == robots_[i]->name()){ + auto mr = dynamic_cast<Moveit_robot*>(robots_[i]); + manipulate_acm(mr, exec.second.second); + merge_ps(ps_m, exec.second.second, mr); + } + } + } + + merge_acm(ps_m); + planning_scene_diff_publisher_->publish(ps_m); }