Skip to content
Snippets Groups Projects
Commit 6ff534d6 authored by KingMaZito's avatar KingMaZito
Browse files

...

parent b1f20d50
No related branches found
No related tags found
No related merge requests found
......@@ -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;
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment