Debugging code & Error output

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