diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index df2869b8f9f652879a7011617f9fdf78a12f26d5..310cf4d896876d4c2aaa7487e63a8f13213eb007 100644 --- a/src/mtc/src/impl/moveit_mediator.cpp +++ b/src/mtc/src/impl/moveit_mediator.cpp @@ -243,7 +243,7 @@ void Moveit_mediator::setup_task(){ if (i < tasks[0].front().solution.sub_trajectory.size()){ t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory; ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff; - + /* ROS_INFO("[Robot 1] acm"); ROS_INFO("default entry names"); for (auto den : ps1->allowed_collision_matrix.default_entry_names){ @@ -306,12 +306,14 @@ void Moveit_mediator::setup_task(){ ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps1->world.octomap.origin.position.x, ps1->world.octomap.origin.position.y, ps1->world.octomap.origin.position.z, ps1->world.octomap.origin.orientation.x, ps1->world.octomap.origin.orientation.y, ps1->world.octomap.origin.orientation.z, ps1->world.octomap.origin.orientation.w); ROS_INFO("world om header time, id %s, seq %i", ps1->world.octomap.header.frame_id.c_str(), ps1->world.octomap.header.seq); ROS_INFO("world om Data"); + */ } if (i < tasks[1].front().solution.sub_trajectory.size()){ t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory; ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff; + /* ROS_INFO("[Robot 2] acm"); ROS_INFO("default entry names"); for (auto den : ps2->allowed_collision_matrix.default_entry_names){ @@ -375,6 +377,7 @@ void Moveit_mediator::setup_task(){ ROS_INFO("world om origin %f %f %f, %f %f %f %f", ps2->world.octomap.origin.position.x, ps2->world.octomap.origin.position.y, ps2->world.octomap.origin.position.z, ps2->world.octomap.origin.orientation.x, ps2->world.octomap.origin.orientation.y, ps2->world.octomap.origin.orientation.z, ps2->world.octomap.origin.orientation.w); ROS_INFO("world om header time, id %s, seq %i", ps2->world.octomap.header.frame_id.c_str(), ps2->world.octomap.header.seq); ROS_INFO("world om Data"); + */ } std::vector<std::thread> th;