Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

For ROS navigation, there is a tf tree map->odom->base_link. The odom pose is the base_link frame pose relative to odom frame. The absolute/global pose is the base_link frame pose relative to map frame.

IMU and encoders provide the odom pose which drifts over time and thus only right for a short time.

GPS, amcl and indoor position system provide the absolute/global pose.

You got odom pose means you got the tf odom->base_link, and you got absolute/global pose means you got base_link pose relative to map. To correct the robot location, you have to provide the tf map->odom, which can be calculated from odom->base_link and base_link pose relative to map.

For details about how to get map->odom, you can refer to amcl wiki and amcl source.

The amcl code about map->odom(only the key part):

  // subtracting base to odom from map to base and send map to odom instead
  tf::Stamped<tf::Pose> odom_to_map;
  try
  {
    tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                         tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                                     hyps[max_weight_hyp].pf_pose_mean.v[1],
                                     0.0));
    tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
                                          laser_scan->header.stamp,
                                          base_frame_id_);
    this->tf_->transformPose(odom_frame_id_,
                             tmp_tf_stamped,
                             odom_to_map);
  }
  catch(tf::TransformException)
  {
    ROS_DEBUG("Failed to subtract base to odom transform");
    return;
  }

  latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                             tf::Point(odom_to_map.getOrigin()));
  latest_tf_valid_ = true;

  if (tf_broadcast_ == true)
  {
    // We want to send a transform that is good up until a
    // tolerance time so that odom can be used
    ros::Time transform_expiration = (laser_scan->header.stamp +
                                      transform_tolerance_);
    tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                        transform_expiration,
                                        global_frame_id_, odom_frame_id_);
    this->tfb_->sendTransform(tmp_tf_stamped);
    sent_first_transform_ = true;
  }