Method GetPoseFromTf

roborts_localization/localization_node.cpp:264–293

}

bool LocalizationNode::GetPoseFromTf(const std::string &target_frame,
                   const std::string &source_frame,
                   const ros::Time &timestamp,
                   Vec3d &pose)
{
  tf::Stamped<tf::Pose> ident(tf::Transform(tf::createIdentityQuaternion(),
                                            tf::Vector3(0, 0, 0)),
                              timestamp,
                              source_frame);
  tf::Stamped<tf::Pose> pose_stamp;
  try {
    this->tf_listener_ptr_->transformPose(target_frame,
                                          ident,
                                          pose_stamp);
  } catch (tf::TransformException &e) {
    LOG_ERROR << "Couldn't transform from "
              << source_frame
              << "to "
              << target_frame;
    return false;
  }

  pose.setZero();
  pose[0] = pose_stamp.getOrigin().x();
  pose[1] = pose_stamp.getOrigin().y();
  double yaw,pitch, roll;
  pose_stamp.getBasis().getEulerYPR(yaw, pitch, roll);
  pose[2] = yaw;
  return true;
}

void LocalizationNode::TransformLaserscanToBaseFrame(double &angle_min,
                                                     double &angle_increment,