...

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;
 }