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