Costmap2DROS中getRobotPose函数调用分析

costmap_2d命名空间中getRobotPose函数的定义

  /**
   * @brief Get the pose of the robot in the global frame of the costmap
   * @param global_pose Will be set to the pose of the robot in the global frame of the costmap
   * @return True if the pose was set successfully, false otherwise
   */
  bool getRobotPose(tf::Stamped<tf::Pose>& global_pose) const;

实现：

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

  // get the global pose of the robot
  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;
}

然后是调用tf中的transformPose函数，函数实现如下：

void TransformListener::transformPose(const std::string& target_frame,
     const geometry_msgs::PoseStamped& msg_in,
     geometry_msgs::PoseStamped& msg_out) const
{
   tf::assertQuaternionValid(msg_in.pose.orientation);
 
   Stamped<Pose> pin, pout;
   poseStampedMsgToTF(msg_in, pin);
   transformPose(target_frame, pin, pout);
   poseStampedTFToMsg(pout, msg_out);
}

poseStampedMsgToTF函数定义如下：

static inline void poseStampedMsgToTF(const geometry_msgs::PoseStamped & msg, Stamped<Pose>& bt)
{
  poseMsgToTF(msg.pose, bt); 
  bt.stamp_ = msg.header.stamp; 
  bt.frame_id_ = msg.header.frame_id;
};

poseMsgToTF函数定义如下：

static inline void poseMsgToTF(const geometry_msgs::Pose& msg, Pose& bt)
{
  bt = Transform(Quaternion(msg.orientation.x, msg.orientation.y, 
                 msg.orientation.z, msg.orientation.w), Vector3(msg.position.x, 
                 msg.position.y, msg.position.z));
};

为什么又调用了transformPose函数？？？

transformPose(target_frame, pin, pout);

poseStampedTFToMsg函数定义如下：

static inline void poseStampedTFToMsg(const Stamped<Pose>& bt, geometry_msgs::PoseStamped & msg)
{
  poseTFToMsg(bt, msg.pose); 
  msg.header.stamp = bt.stamp_; 
  msg.header.frame_id = bt.frame_id_;
};

poseTFToMsg函数定义如下：

static inline void poseTFToMsg(const Pose& bt, geometry_msgs::Pose& msg)
{
  pointTFToMsg(bt.getOrigin(), msg.position);  
  quaternionTFToMsg(bt.getRotation(), msg.orientation);
};

pointTFToMsg函数

static inline void pointTFToMsg(const Point& bt_v, geometry_msgs::Point& msg_v) {
  msg_v.x = bt_v.x(); 
  msg_v.y = bt_v.y(); 
  msg_v.z = bt_v.z();
};

quaternionTFToMsg函数

static inline void quaternionTFToMsg(const Quaternion& bt, geometry_msgs::Quaternion& msg) 
{
  if (fabs(bt.length2() - 1 ) > QUATERNION_TOLERANCE) 
    {
      ROS_WARN("TF to MSG: Quaternion Not Properly Normalized");
      Quaternion bt_temp = bt; 
      bt_temp.normalize();
      msg.x = bt_temp.x(); msg.y = bt_temp.y(); msg.z = bt_temp.z();  msg.w = bt_temp.w();
    }
  else
  {
    msg.x = bt.x(); msg.y = bt.y(); msg.z = bt.z();  msg.w = bt.w();
  }
};