diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index ae68512be7721c72607eab17a3b7a6c2ffe6e34a..27a8bcbdcfd9af4e54c051436217a1b07a5cf042 100644 --- a/.catkin_tools/profiles/default/devel_collisions.txt +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -1,4 +1,4 @@ /home/matteo/ws_panda/devel/./cmake.lock 42 -/home/matteo/reachability/devel/./cmake.lock 26240 +/home/matteo/reachability/devel/./cmake.lock 26253 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index f5effccc3e96ca14165a1b368ae083202fa13cdf..df2869b8f9f652879a7011617f9fdf78a12f26d5 100644 --- a/src/mtc/src/impl/moveit_mediator.cpp +++ b/src/mtc/src/impl/moveit_mediator.cpp @@ -243,12 +243,138 @@ 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){ + ROS_INFO("%s", den.c_str()); + } + ROS_INFO("default entry values"); + for (auto den : ps1->allowed_collision_matrix.default_entry_values){ + ROS_INFO("%i", den); + } + ROS_INFO("entry names"); + for (auto den : ps1->allowed_collision_matrix.entry_names){ + ROS_INFO("%i", den); + } + ROS_INFO("entry values"); + for (auto den : ps1->allowed_collision_matrix.entry_values){ + ROS_INFO("%i", den); + } + ROS_INFO("[Robot 1] fft"); + for (auto ft : ps1->fixed_frame_transforms){ + ROS_INFO("child frame id: %s, header id: %s, seq %i, time", ft.child_frame_id.c_str(), ft.header.frame_id.c_str(), ft.header.seq); + } + ROS_INFO("[Robot 1] is_diff"); + ROS_INFO("is_diff: %i", ps1->is_diff); + ROS_INFO("[Robot 1] link padding"); + for (auto lp : ps1->link_padding){ + ROS_INFO("link padding name: %s, padding %d", lp.link_name.c_str(), lp.padding); + } + ROS_INFO("[Robot 1] link scale"); + for (auto ls : ps1->link_scale){ + ROS_INFO("link scale name: %s, padding %d", ls.link_name.c_str(), ls.scale); + } + ROS_INFO("[Robot 1] name"); + ROS_INFO("name: %s", ps1->name.c_str()); + ROS_INFO("[Robot 1] object color"); + for (auto oc : ps1->object_colors){ + ROS_INFO("object color %f %f %f %f, id %s", oc.color.r, oc.color.g, oc.color.b, oc.color.a, oc.id.c_str()); + } + ROS_INFO("[Robot 1] rmn"); + ROS_INFO("rmn %s", ps1->robot_model_name.c_str()); + ROS_INFO("[Robot 1] rs"); + for (auto aco : ps1->robot_state.attached_collision_objects){ + ROS_INFO("aco dp header frame id: %s, seq %i, time", aco.detach_posture.header.frame_id.c_str(), aco.detach_posture.header.seq); + for (auto jn : aco.detach_posture.joint_names){ + ROS_INFO("aco dp joint name: %s", jn.c_str()); + } + ROS_INFO("aco dp Points"); + for (auto p : aco.detach_posture.points){ + for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %d", pa); + for (auto pa : p.effort) ROS_INFO("aco dp points effort: %d", pa); + ROS_INFO("aco dp points time from start"); + for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %d", pa); + for (auto pa : p.positions) ROS_INFO("aco dp points tra: %d", pa); + } + ROS_INFO("aco link names %s", aco.link_name.c_str()); + ROS_INFO("aco collis obj id %s", aco.object.id.c_str()); + for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.c_str()); + ROS_INFO("aco collis obj weight %d", aco.weight); + } + for (auto p : ps1->world.collision_objects) ROS_INFO("world co id %s", p.id); + 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){ + ROS_INFO("%s", den.c_str()); + } + ROS_INFO("default entry values"); + for (auto den : ps2->allowed_collision_matrix.default_entry_values){ + ROS_INFO("%i", den); + } + ROS_INFO("entry names"); + for (auto den : ps2->allowed_collision_matrix.entry_names){ + ROS_INFO("%i", den); + } + ROS_INFO("entry values"); + for (auto den : ps2->allowed_collision_matrix.entry_values){ + ROS_INFO("%i", den); + } + ROS_INFO("[Robot 2] fft"); + for (auto ft : ps2->fixed_frame_transforms){ + ROS_INFO("child frame id: %s, header id: %s, seq %i, time", ft.child_frame_id.c_str(), ft.header.frame_id.c_str(), ft.header.seq); + } + ROS_INFO("[Robot 2] is_diff"); + ROS_INFO("is_diff: %i", ps2->is_diff); + + ROS_INFO("[Robot 2] link padding"); + for (auto lp : ps2->link_padding){ + ROS_INFO("link padding name: %s, padding %d", lp.link_name.c_str(), lp.padding); + } + ROS_INFO("[Robot 2] link scale"); + for (auto ls : ps2->link_scale){ + ROS_INFO("link scale name: %s, padding %d", ls.link_name.c_str(), ls.scale); + } + ROS_INFO("[Robot 2] name"); + ROS_INFO("name: %s", ps2->name.c_str()); + ROS_INFO("[Robot 2] object color"); + for (auto oc : ps2->object_colors){ + ROS_INFO("object color %f %f %f %f, id %s", oc.color.r, oc.color.g, oc.color.b, oc.color.a, oc.id.c_str()); + } + ROS_INFO("[Robot 2] rmn"); + ROS_INFO("rmn %s", ps2->robot_model_name.c_str()); + ROS_INFO("[Robot 2] rs"); + for (auto aco : ps2->robot_state.attached_collision_objects){ + ROS_INFO("aco dp header frame id: %s, seq %i, time", aco.detach_posture.header.frame_id.c_str(), aco.detach_posture.header.seq); + for (auto jn : aco.detach_posture.joint_names){ + ROS_INFO("aco dp joint name: %s", jn.c_str()); + } + ROS_INFO("aco dp Points"); + for (auto p : aco.detach_posture.points){ + for (auto pa : p.accelerations) ROS_INFO("aco dp points acc: %d", pa); + for (auto pa : p.effort) ROS_INFO("aco dp points effort: %d", pa); + ROS_INFO("aco dp points time from start"); + for (auto pa : p.velocities) ROS_INFO("aco dp points vel: %d", pa); + for (auto pa : p.positions) ROS_INFO("aco dp points tra: %d", pa); + } + ROS_INFO("aco link names %s", aco.link_name.c_str()); + ROS_INFO("aco collis obj id %s", aco.object.id.c_str()); + for (auto p : aco.touch_links) ROS_INFO("aco touch link %s", p.c_str()); + ROS_INFO("aco collis obj weight %d", aco.weight); + } + for (auto p : ps2->world.collision_objects) ROS_INFO("world co id %s", p.id); + 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;