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

some prints to combine planning scenes, can smell the end tho

parent d5a2fb1a
Branches
No related tags found
No related merge requests found
Pipeline #14998 failed
Stage: build
Hide whitespace changes
Inline Side-by-side
.catkin_tools/profiles/default/devel_collisions.txt
+ 1
− 1
View file @ 412de4d2
/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
src/mtc/src/impl/moveit_mediator.cpp
+ 126
− 0
View file @ 412de4d2
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment