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;//對收到的消息進行解析,得到transformSumgeometry_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的發布頻率是一致的。