LOAM_velodyne學習（四）

 

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​​​​​​​的消息，再一次更新位姿，我們畫圖來說明：

​

也就是說，最后采用的位姿是TransformMaintenance發布的integrated_to_init消息。而且由上面的分析可知，TransformMaintenance的發布頻率和laserOdometry的發布頻率是一致的。

 

撈一下https://blog.csdn.net/qq_21842097/article/details/81094922

遷移到博客園