示例#1
0
bool
AmclThread::get_odom_pose(tf::Stamped<tf::Pose>& odom_pose, double& x,
                          double& y, double& yaw,
                          const fawkes::Time* t, const std::string& f)
{
  // Get the robot's pose
  tf::Stamped<tf::Pose>
    ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
			tf::Vector3(0, 0, 0)), t, f);
  try {
    tf_listener->transform_pose(odom_frame_id_, ident, odom_pose);
  } catch (Exception &e) {
    if (cfg_buffer_debug_) {
      logger->log_warn(name(), "Failed to compute odom pose (%s)",
		       e.what_no_backtrace());
    }
    return false;
  }
  x = odom_pose.getOrigin().x();
  y = odom_pose.getOrigin().y();
  double pitch, roll;
  odom_pose.getBasis().getEulerZYX(yaw, pitch, roll);

  //logger->log_info(name(), "Odom pose: (%f, %f, %f)", x, y, yaw);

  return true;
}
示例#2
0
bool
AmclNode::getOdomPose(tf::Stamped<tf::Pose>& odom_pose,
                      double& x, double& y, double& yaw,
                      const ros::Time& t, const std::string& f)
{
    // Get the robot's pose
    tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
                                 tf::Vector3(0,0,0)), t, f);
    try
    {
        this->tf_->transformPose(odom_frame_id_, ident, odom_pose);
    }
    catch(tf::TransformException e)
    {
        ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
        return false;
    }
    x = odom_pose.getOrigin().x();
    y = odom_pose.getOrigin().y();
    double pitch,roll;
    odom_pose.getBasis().getEulerYPR(yaw, pitch, roll);

    return true;
}