From 6ff534d6b1e7114c16401b16d24da42aa781fcf5 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Fri, 10 Feb 2023 01:05:34 +0100
Subject: [PATCH] ...

---
 src/impl/moveit_mediator.cpp | 21 ++++++++++++++++++++-
 1 file changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index b474936..2f12ee7 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -570,6 +570,16 @@ void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScen
 	std::smatch match;
 	std::regex_match(mr->name(), match, rx_panda);
 
+	ROS_INFO("manipulate_acm");
+	for (auto& e : ps.allowed_collision_matrix.entry_names) ROS_INFO("%s", e.c_str());
+	for (auto& e : ps.allowed_collision_matrix.entry_values) {
+		for (auto en: e.enabled){
+			std::cout << en << " ";
+		}
+		std::cout << "\n";
+	}
+
+
 	// build panda link regex
 	std::stringstream ss;
 	ss << "panda_" << match[1] << "_.*";
@@ -925,7 +935,7 @@ void Moveit_mediator::task_planner(){
 			}
 		}
 
-		// merge_acm(ps_m);
+		merge_acm(ps_m);
 		planning_scene_diff_publisher_->publish(ps_m);
 	}
 
@@ -973,6 +983,15 @@ void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
 		i++;
 	}
 
+
+	ROS_INFO("Merge ACM");
+	for (auto& e : acmt.entry_names) ROS_INFO("%s", e.c_str());
+	for (auto& e : acmt.entry_values) {
+		for (auto en: e.enabled){
+			std::cout << en << " ";
+		}
+		std::cout << "\n";
+	}
 	ps_m.allowed_collision_matrix = acmt;
 }
 
-- 
GitLab