diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp index b474936caf7297e6cf0f624738e4417314be0e03..2f12ee7360367e1c47368733957e8257f4c86507 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; }