Gmapping and TF frames

edit

Solved this problem by changing the depthimage_to_laserscan 'output_frame_id' to "camera_depth_frame" or "camera_link", ie:

 <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" >
    <param name="scan_height" value="3"/> 
    <param name="output_frame_id" value="camera_depth_frame" />
    <remap from="/image" to="/camera/depth/image_raw" />
</node>

Originally, had the output_frame_id has "camera_depth_optical_frame".

The tf tree is camera_link --> camera_depth_frame --> camera_depth_optical_frame but there's an inbuilt rotation of rpy (-1.571, 0, -1.571) rads in the last transform. Can be seen by:

rosrun tf tf_monitor camera_depth_frame camera_depth_optical_frame

Hence, the required static transform between base_link and the camera can be set as required for the particular robot.

It seems that the base_frame choosen to launch gmapping might not be the good one. How do you launch gmapping? In the source code of gmapping:

  v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
  tf::Stamped<tf::Vector3> up(v, scan.header.stamp,
                                      base_frame_);
      [...]

  // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment.
  if (fabs(fabs(up.z()) - 1) > 0.001)
  {
    ROS_WARN("Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f",
                 up.z());
    return false;
  }

Where laser_pose is the tf between the frame "base_frame" ( which is "base_link" by default ) and the frame of your laser