您的位置:首页 > 编程语言 > C语言/C++

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

2020-08-17 16:31 3471 查看

文章目录

  • 2.回调函数
  • 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, -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;
    }
    };

    2.2 laserOdometryHandler

    laserOdometryHandler
    是将粗配准的里程计信息与精配准的里程计信息融合计算,并在回调函数中便发送了最终外发的里程计话题。在该回调函数中的
    TF
    与里程计话题才是最终决定的。

    //laserOdometryHandler是将粗配准的里程计信息与精配准的里程计信息融合计算,并在回调函数中便发送了最终外发的里程计话题
    void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
    {
    currentHeader = laserOdometry->header;
    
    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();
    
    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);
    }

    3.总结

    粗配准的里程计信息是

    FeatureAssociation
    发出的,精配准的信息是
    mapOptimization
    发出的,均以200Hz的频率,当
    odomAftMappedHandler
    收到精配准信息后更新位姿,这个位姿将在
    laserOdometryHandler
    收到下一条粗配准信息后综合计算再发出。

    内容来自用户分享和网络整理,不保证内容的准确性,如有侵权内容,可联系管理员处理 点击这里给我发消息
    标签: