From 49371e2ce9372080368a48fce26ee4add39aab9b Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Thu, 9 Feb 2023 17:40:45 +0100
Subject: [PATCH] ...
---
src/impl/moveit_mediator.cpp | 16 +++++++++++++---
1 file changed, 13 insertions(+), 3 deletions(-)
diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index adf09ae..eca4db4 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -904,12 +904,22 @@ void Moveit_mediator::task_planner(){
}
}
-
for(auto& t : th){
if(t.joinable()) t.join();
}
- //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);
}
--
GitLab