Skip to content

Instantly share code, notes, and snippets.

@felixvd

felixvd/gist:993620d534898b1f159014451c97a7a7

Created May 24, 2020 17:31
Show Gist options
  • Save felixvd/993620d534898b1f159014451c97a7a7 to your computer and use it in GitHub Desktop.
Save felixvd/993620d534898b1f159014451c97a7a7 to your computer and use it in GitHub Desktop.
Download ZIP
Debugging code & Error output
Raw
gistfile1.txt
Lines 1714-1735:
if (object.operation == moveit_msgs::CollisionObject::ADD && world_->hasObject(object.id))
world_->removeObject(object.id);
ROS_WARN_STREAM("Received collision object with id: " << object.id);
ROS_WARN_STREAM("header.frame_id: " << object.header.frame_id);
const Eigen::Isometry3d& world_to_object_header_transform = getTransforms().getTransform(object.header.frame_id);
ROS_WARN_STREAM("world_to_object_header_transform: ");
ROS_WARN_STREAM(world_to_object_header_transform.matrix() << std::endl);
Eigen::Isometry3d object_header_to_pose_transform;
PlanningScene::poseMsgToEigen(object.pose, object_header_to_pose_transform);
ROS_WARN_STREAM("object.pose: " << object.pose);
ROS_WARN_STREAM("object_header_to_pose_transform: ");
ROS_WARN_STREAM(object_header_to_pose_transform.matrix() << std::endl);
const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * object_header_to_pose_transform;
// const Eigen::Isometry3d object_frame_transform = getTransforms().getTransform(object.header.frame_id);
ROS_WARN_STREAM("object_frame_transform: ");
ROS_WARN_STREAM(object_frame_transform.matrix() << std::endl);
======
[ WARN] [1590340304.018990615]: Received collision object with id: box
[ WARN] [1590340304.019157245]: header.frame_id: panda_hand
[ WARN] [1590340304.019264379]: world_to_object_header_transform:
[ WARN] [1590340304.019620746]: 1 0.000398163 4.62412e-17 0.30702
0.000398163 -1 -6.92765e-12 -5.22133e-12
-2.7121e-15 6.92765e-12 -1 0.59027
0 0 0 1
[ WARN] [1590340304.019730778]: Empty quaternion found in pose message. Setting to neutral orientation.
[ WARN] [1590340304.019837608]: object.pose: position:
x: 0
y: 0
z: 0
orientation:
x: 0
y: 0
z: 0
w: 0
[ WARN] [1590340304.020002534]: object_header_to_pose_transform:
[ WARN] [1590340304.020219844]: 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
[ WARN] [1590340304.020317375]: object_frame_transform:
[ WARN] [1590340304.020501038]: 1 0.000398163 4.62412e-17 0.30702
0.000398163 -1 -6.92765e-12 -5.22133e-12
-2.7121e-15 6.92765e-12 -1 0.59027
0 0 0 1
[ WARN] [1590340304.020889913]: Received collision object with id: cylinder
[ WARN] [1590340304.020969532]: header.frame_id: panda_hand
[ WARN] [1590340304.021041388]: world_to_object_header_transform:
[ WARN] [1590340304.021229390]: 1 0.000398163 4.62412e-17 0.30702
0.000398163 -1 -6.92765e-12 -5.22133e-12
-2.7121e-15 6.92765e-12 -1 0.59027
0 0 0 1
[ WARN] [1590340304.021311758]: Empty quaternion found in pose message. Setting to neutral orientation.
[ WARN] [1590340304.021405977]: object.pose: position:
x: 0
y: 0
z: 0
orientation:
x: 0
y: 0
z: 0
w: 0
[ WARN] [1590340304.021498794]: object_header_to_pose_transform:
[ WARN] [1590340304.021676566]: 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
[ WARN] [1590340304.021752322]: object_frame_transform:
[ WARN] [1590340304.022026749]: 1 0.000398163 4.62412e-17 0.30702
0.000398163 -1 -6.92765e-12 -5.22133e-12
-2.7121e-15 6.92765e-12 -1 0.59027
0 0 0 1
[ WARN] [1590340304.023724919]: Received collision object with id: box
[ WARN] [1590340304.023893651]: header.frame_id: world
[ WARN] [1590340304.024049281]: Received collision object with id: box
[ WARN] [1590340304.024227211]: header.frame_id: world
[ WARN] [1590340304.024330061]: world_to_object_header_transform:
[ WARN] [1590340304.024456119]: world_to_object_header_transform:
[ WARN] [1590340304.024680996]: 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
[ WARN] [1590340304.024829819]: 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
[ WARN] [1590340304.025087557]: object.pose: position:
x: 4.66536e-62
y: 5.40156e-67
z: 8.31857e-72
orientation:
x: 3.00735e-67
y: -6.9902e-77
z: -1.47439e+179
w: 0.5
[ WARN] [1590340304.025306286]: object.pose: position:
x: 4.66536e-62
y: 5.40156e-67
z: 8.31857e-72
orientation:
x: 3.00735e-67
y: -6.9902e-77
z: -1.47439e+179
w: 0.5
[ WARN] [1590340304.025472083]: object_header_to_pose_transform:
[ WARN] [1590340304.025656291]: object_header_to_pose_transform:
[ WARN] [1590340304.025817958]: 1 0 -0 4.66536e-62
-0 1 0 5.40156e-67
0 0 1 8.31857e-72
0 0 0 1
[ WARN] [1590340304.025955850]: 1 0 -0 4.66536e-62
-0 1 0 5.40156e-67
0 0 1 8.31857e-72
0 0 0 1
[ WARN] [1590340304.026093285]: object_frame_transform:
[ WARN] [1590340304.026356032]: 1 0 0 4.66536e-62
0 1 0 5.40156e-67
0 0 1 8.31857e-72
0 0 0 1
[ WARN] [1590340304.026787719]: object_frame_transform:
[ WARN] [1590340304.026983516]: Received collision object with id: cylinder
[ WARN] [1590340304.027174231]: header.frame_id: world
[ WARN] [1590340304.027317590]: world_to_object_header_transform:
[ WARN] [1590340304.027447215]: 1 0 0 4.66536e-62
0 1 0 5.40156e-67
0 0 1 8.31857e-72
0 0 0 1
[ WARN] [1590340304.027656353]: 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
[ WARN] [1590340304.027875304]: Received collision object with id: cylinder
[ WARN] [1590340304.027994474]: header.frame_id: world
[ WARN] [1590340304.028089523]: world_to_object_header_transform:
[ WARN] [1590340304.028308926]: object.pose: position:
x: 4.66536e-62
y: 5.40156e-67
z: 8.31857e-72
orientation:
x: 3.00735e-67
y: -6.9902e-77
z: -1.47439e+179
w: 0.5
[ WARN] [1590340304.028437905]: object_header_to_pose_transform:
[ WARN] [1590340304.028546540]: 1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
[ WARN] [1590340304.028705077]: 1 0 -0 4.66536e-62
-0 1 0 5.40156e-67
0 0 1 8.31857e-72
0 0 0 1
[ WARN] [1590340304.028823059]: object_frame_transform:
[ WARN] [1590340304.028965909]: object.pose: position:
x: 4.66536e-62
y: 5.40156e-67
z: 8.31857e-72
orientation:
x: 3.00735e-67
y: -6.9902e-77
z: -1.47439e+179
w: 0.5
[ WARN] [1590340304.029114164]: 1 0 0 4.66536e-62
0 1 0 5.40156e-67
0 0 1 8.31857e-72
0 0 0 1
[ WARN] [1590340304.029277497]: object_header_to_pose_transform:
[ WARN] [1590340304.029565606]: 1 0 -0 4.66536e-62
-0 1 0 5.40156e-67
0 0 1 8.31857e-72
0 0 0 1
[ WARN] [1590340304.029672535]: object_frame_transform:
[ WARN] [1590340304.029914478]: 1 0 0 4.66536e-62
0 1 0 5.40156e-67
0 0 1 8.31857e-72
0 0 0 1
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment