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

somethings wrong

parent 412de4d2
Branches
No related tags found
No related merge requests found
Pipeline #14999 failed
...@@ -243,7 +243,7 @@ void Moveit_mediator::setup_task(){ ...@@ -243,7 +243,7 @@ void Moveit_mediator::setup_task(){
if (i < tasks[0].front().solution.sub_trajectory.size()){ if (i < tasks[0].front().solution.sub_trajectory.size()){
t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory; t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff; ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff;
/*
ROS_INFO("[Robot 1] acm"); ROS_INFO("[Robot 1] acm");
ROS_INFO("default entry names"); ROS_INFO("default entry names");
for (auto den : ps1->allowed_collision_matrix.default_entry_names){ for (auto den : ps1->allowed_collision_matrix.default_entry_names){
...@@ -306,12 +306,14 @@ void Moveit_mediator::setup_task(){ ...@@ -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 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 header time, id %s, seq %i", ps1->world.octomap.header.frame_id.c_str(), ps1->world.octomap.header.seq);
ROS_INFO("world om Data"); ROS_INFO("world om Data");
*/
} }
if (i < tasks[1].front().solution.sub_trajectory.size()){ if (i < tasks[1].front().solution.sub_trajectory.size()){
t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory; t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory;
ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff; ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
/*
ROS_INFO("[Robot 2] acm"); ROS_INFO("[Robot 2] acm");
ROS_INFO("default entry names"); ROS_INFO("default entry names");
for (auto den : ps2->allowed_collision_matrix.default_entry_names){ for (auto den : ps2->allowed_collision_matrix.default_entry_names){
...@@ -375,6 +377,7 @@ void Moveit_mediator::setup_task(){ ...@@ -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 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 header time, id %s, seq %i", ps2->world.octomap.header.frame_id.c_str(), ps2->world.octomap.header.seq);
ROS_INFO("world om Data"); ROS_INFO("world om Data");
*/
} }
std::vector<std::thread> th; std::vector<std::thread> th;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment