From 43c5be22e6b6a9e76b5a51462d3ef3af7bc0c6e9 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Thu, 9 Feb 2023 22:59:36 +0100
Subject: [PATCH] ...
---
src/impl/moveit_mediator.cpp | 24 ++++++++++++------------
1 file changed, 12 insertions(+), 12 deletions(-)
diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index 1f35062..f4af9c2 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);
}
--
GitLab