LeGo-LOAM源码阅读笔记(五)---transformFusion.cpp

最新推荐文章于 2024-06-13 09:32:05 发布

AutoMan-USTC

最新推荐文章于 2024-06-13 09:32:05 发布

阅读量1.9k

点赞数 1

分类专栏：激光SLAM

本文链接：https://blog.youkuaiyun.com/Zlp19970106/article/details/108039924

版权

文章目录

1.transformFusion节点框架
- 1.1 main函数
- 1.2 transformFusion构造函数
2.回调函数
- 2.1 odomAftMappedHandler
- 2.2 laserOdometryHandler
3.总结

1.transformFusion节点框架

1.1 main函数

int main(int argc, char** argv)
{
   
    ros::init(argc, argv, "lego_loam");
    
    TransformFusion TFusion;//构造函数,重要的两个回调函数

    ROS_INFO("\033[1;32m---->\033[0m Transform Fusion Started.");

    ros::spin();

    return 0;
}

1.2 transformFusion构造函数

//订阅话题:

   "/laser_odom_to_init"(nav_msgs::Odometry)，数据处理函数laserOdometryHandler
   "/aft_mapped_to_init"(nav_msgs::Odometry)，数据处理函数odomAftMappedHandler
   
//发布话题，这些topic有：

  "/integrated_to_init"(nav_msgs::Odometry)

2.回调函数

2.1 odomAftMappedHandler

通过odomAftMappedHandler函数获取精配准后的位姿作为transformAftMapped，而获取配准后的速度信息作为transformBefMapped准备下一次计算。

 //通过odomAftMappedHandler函数获取精配准后的位姿作为transformAftMapped，而获取配准后的速度信息作为transformBefMapped准备下一次计算
    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