欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

LOAM_velodyne学习(四)

程序员文章站 2022-03-07 11:36:12
...

TransformMaintenance

来到了最后一个模块,代码不是很长,我们在看完代码之后,再详细说明这个模块的功能

依然主函数开始

int main(int argc, char** argv)
{
  ros::init(argc, argv, "transformMaintenance");
  ros::NodeHandle nh;

  //订阅了两个节点
  ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> 
                                     ("/laser_odom_to_init", 5, laserOdometryHandler);

  ros::Subscriber subOdomAftMapped = nh.subscribe<nav_msgs::Odometry> 
                                     ("/aft_mapped_to_init", 5, odomAftMappedHandler);

  //发布一个节点
  ros::Publisher pubLaserOdometry2 = nh.advertise<nav_msgs::Odometry> ("/integrated_to_init", 5);
  pubLaserOdometry2Pointer = &pubLaserOdometry2;
  laserOdometry2.header.frame_id = "/camera_init";
  laserOdometry2.child_frame_id = "/camera";

  tf::TransformBroadcaster tfBroadcaster2;
  tfBroadcaster2Pointer = &tfBroadcaster2;
  laserOdometryTrans2.frame_id_ = "/camera_init";
  laserOdometryTrans2.child_frame_id_ = "/camera";

  ros::spin();

  return 0;
}

主函数非常简单,可以看出integrated_to_init消息是由发布器pubLaserOdometry2发布的,实际上就是由发布器pubLaserOdometry2Pointer发布的。我们找到pubLaserOdometry2Pointer,发现回调函数laserOdometryHandler就是pubLaserOdometry2Pointer的发布函数,这意味着,发现每次接收到laser_odom_to_init消息并调用回调函数laserOdometryHandler时,就发布一次integrated_to_init消息。

看来重点是这个回调函数laserOdometryHandler,我们来仔细看看

void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
  double roll, pitch, yaw;
  //对收到的消息进行解析,得到transformSum
  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();

  //位姿信息进行存储,准备发布
  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];
  pubLaserOdometry2Pointer->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]));
  tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2);
}

但是,这里还是有个小坑的。这个节点接收了两个消息,分别是laserOdometry节点和laserMapping节点发布的,而这两个节点发布的频率不同,那么是怎么处理的呢?

我们仔细看一看剩下的一个回调函数

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

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

  transformAftMapped[3] = odomAftMapped->pose.pose.position.x;
  transformAftMapped[4] = odomAftMapped->pose.pose.position.y;
  transformAftMapped[5] = odomAftMapped->pose.pose.position.z;

  transformBefMapped[0] = odomAftMapped->twist.twist.angular.x;
  transformBefMapped[1] = odomAftMapped->twist.twist.angular.y;
  transformBefMapped[2] = odomAftMapped->twist.twist.angular.z;

  transformBefMapped[3] = odomAftMapped->twist.twist.linear.x;
  transformBefMapped[4] = odomAftMapped->twist.twist.linear.y;
  transformBefMapped[5] = odomAftMapped->twist.twist.linear.z;
}

也是很简单的解析函数,作用是在接收到了laserMapping的消息后,更新位姿,这里注意,laserMapping发布的是优化过后的位姿!看到这里,就逐渐能明白作者如何完成两个不同频率之间的协调了。

当接收到laserMapping的消息后,立马更新位姿,这样得到了优化的结果;而这个优化结果会被回调函数laserOdometryHandler​​​​​​​里的transformAssociateToMap这一个函数一直利用来建图,一直到下一次接收到laserMapping​​​​​​​的消息,再一次更新位姿,我们画图来说明:

LOAM_velodyne学习(四)

也就是说,最后采用的位姿是TransformMaintenance发布的integrated_to_init消息。而且由上面的分析可知,TransformMaintenance的发布频率和laserOdometry的发布频率是一致的。

相关标签: slam