Costmap2DROS的函数getRobotPose

Costmap2DROS的函数getRobotPose | 沉默杀手

源码其实并不复杂

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