diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index 310cf4d896876d4c2aaa7487e63a8f13213eb007..a97854ca964b7a1a829fcee4581a8d3e2b5bbf99 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,14 +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){ @@ -377,7 +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;