TransformMaintenance源码分析

还是先看节点输入输出:

可以看到该节点订阅了 laserOdometry 节点发布的/laser_odom_to_init 消息(Lidar 里程计估计位姿到初始坐标系的变换)以及 laserMapping 节点发布的/aft_mapped_to_init 消息(laserMapping 节点优化后的位姿到初始坐标系的变换),经过节点处理发布/integrated_to_init 消息。所以要搞清楚这个transformMaintenance 节点到底是干嘛的,我们首先需要弄清楚它所发布的消息到底是什么含义。这个节点的主函数非常简单:

/** Main node entry point. */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "transformMaintenance");
  ros::NodeHandle node;
  ros::NodeHandle privateNode("~");

  loam::TransformMaintenance transMaintenance;

  if (transMaintenance.setup(node, privateNode)) {
    // initialization successful
    ros::spin();
  }

  return 0;
}

可以看出/integrated_to_init 消息是由发布器 pubLaserOdometry2 发布的,顺藤摸瓜,发现每次接收到/laser_odom_to_init 消息并调用回调函数 laserOdometryHandler 时,就发布一次该消息。通过该回调函数可以看出,他完成的就是一个简单则坐标转换,得到经过 laserMapping 优化后的 Lidar 位姿。但是!!!这个节点接收了两个消息,分别是 laserOdometry 节点和 laserMapping 节点发布的,而这两个节点发布的频率不同啊!!看图可以知道,论文中说 1Hz 和 10Hz,实际是 5Hz 和 10Hz。所以这个/integrated_to_init消息到底是什么?实际它就是融合后的 Lidar 轨迹,说白了就是:有优化结果了就拿这一时刻的优化结果作为轨迹,没有优化结果只有里程计结果了,就直接拿里程计结果作为这一时刻的轨迹。

void TransformMaintenance::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
  double roll, pitch, yaw;
  geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
  tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);

  _transformSum[0] = -pitch;
  _transformSum[1] = -yaw;
  _transformSum[2] = roll;

  _transformSum[3] = laserOdometry->pose.pose.position.x;
  _transformSum[4] = laserOdometry->pose.pose.position.y;
  _transformSum[5] = laserOdometry->pose.pose.position.z;

  transformAssociateToMap();//这个函数调用有优化结果后的数据 _transformMapped

  geoQuat = tf::createQuaternionMsgFromRollPitchYaw
      (_transformMapped[2], -_transformMapped[0], -_transformMapped[1]);

  _laserOdometry2.header.stamp = laserOdometry->header.stamp;
  _laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
  _laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
  _laserOdometry2.pose.pose.orientation.z = geoQuat.x;
  _laserOdometry2.pose.pose.orientation.w = geoQuat.w;
  _laserOdometry2.pose.pose.position.x = _transformMapped[3];
  _laserOdometry2.pose.pose.position.y = _transformMapped[4];
  _laserOdometry2.pose.pose.position.z = _transformMapped[5];
  _pubLaserOdometry2.publish(_laserOdometry2);

  _laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
  _laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
  _laserOdometryTrans2.setOrigin(tf::Vector3(_transformMapped[3], _transformMapped[4], _transformMapped[5]));
  _tfBroadcaster2.sendTransform(_laserOdometryTrans2);
}