源码其实并不复杂

bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
  global_pose.setIdentity();
  tf::Stamped < tf::Pose > robot_pose;
  robot_pose.setIdentity();
  robot_pose.frame_id_ = robot_base_frame_;
  robot_pose.stamp_ = ros::Time();
  ros::Time current_time = ros::Time::now();  // save time for checking tf delay later
  try
  {
    tf_.transformPose(global_frame_, robot_pose, global_pose);
  }
  catch (tf::LookupException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ConnectivityException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ExtrapolationException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  // check global_pose timeout
  if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
  {
    ROS_WARN_THROTTLE(1.0,
                      "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
                      current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
    return false;
  }
  return true;
}

核心是函数 transformPose，tf_在这里是tf::TransformListener，但函数实际是继承自基类tf::Transformer，有两个重载，常用的是第一个

void Transformer::transformPose(const std::string&   target_frame,
                                const Stamped< tf::Pose >&   stamped_in,
                                Stamped< tf::Pose >&   stamped_out 
) const

void Transformer::transformPose(const std::string&   target_frame,
                                const ros::Time&   target_time,
                                const Stamped< tf::Pose >&   stamped_in,
                                const std::string&   fixed_frame,
                                Stamped< tf::Pose >&   stamped_out 
) const

第二个重载会调用函数lookupTransform

// Get the transform between two frames by frame ID assuming fixed frame.
void Transformer::lookupTransform ( const std::string &   target_frame,
	const ros::Time &   target_time,
	const std::string &   source_frame,
	const ros::Time &   source_time,
	const std::string &   fixed_frame,
	StampedTransform &  transform 
)   const

• target_frame The frame to which data should be transformed
• target_time The time to which the data should be transformed. (0 will get the latest)
• source_frame The frame where the data originated
• source_time The time at which the source_frame should be evaluated. (0 will get the latest)
• fixed_frame The frame in which to assume the transform is constant in time.
• transform The transform reference to fill.

可能抛出的异常

namespace tf{
  // Pass through exceptions from tf2
  typedef tf2::TransformException TransformException;
  typedef tf2::LookupException LookupException;
  typedef tf2::ConnectivityException ConnectivityException;
  typedef tf2::ExtrapolationException ExtrapolationException;
  typedef tf2::InvalidArgumentException InvalidArgument; 
 }

报警 Costmap2DROS transform timeout

报警对应的transform_tolerance_是在构造函数里写死的0.3，一开始我以为这个数太小了，于是改为2，结果没有改善。

以下报警的根源都是Costmap2DROS::getRobotPose

尝试的改善措施(全都无效):

• Costmap2DROS构造函数中设置: transform_tolerance_(2)
• timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this); 0.1秒间隔太短了。增大到1.5，稍有改善，但未解决根本问题，反而降低了代价地图的性能。
• 降低两个代价地图的update_frequency

所有报警基本来自Costmap2DROS::getRobotPose，或者说map—->base_link的tf变换不及时。

1. 如果不启动VSLAM，启动诠视相机的xv_sdk.launch，使用static_transform_republisher建立假的TF树，启动导航框架，一个报警也没有。因此问题还在VSLAM，怀疑是CPU而不是网络问题。
2. 启动诠视相机的xv_sdk.launch和VSLAM，但不发布tf，使用假的tf树，启动导航框架，仍然没有报警。
3. 继续缩小问题范围，发布假的tf: world—->map—->odom—-> base_link，导航框架不报警。但是如果由VSLAM发布map—->odom，就有报警了。

所以问题的原因：

1. VSLAM的map—->odom—-> base_link变换。
2. Nano的CPU没有使用到极致，还有多余算力

在此情况下规划路径时，全局路径可生成，因为全局规划频率是0，虽然有很多报警，总能成功规划一次。但局部路径就不同了，需要频繁规划。
只看TF的频率和tf echo有没有数据还不够，应该看Rviz里，base_link坐标系在map中的坐标是否能随着机器人的移动实时更新。先满足这一点，再运行导航框架。

另一种原因是网络带宽不足