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的发布频率是一致的。

LOAM_velodyne学习（四）相关推荐

C#多线程学习(四) 多线程的自动管理(线程池) (转载系列)——继续搜索引擎研究...
在多线程的程序中,经常会出现两种情况: 一种情况: 应用程序中,线程把大部分的时间花费在等待状态,等待某个事件发生,然后才能给予响应这一般使用ThreadPo ...
python学习四(处理数据)
python学习四(处理数据) head first python中的一个数据处理的例子有四个U10选手的600米成绩,请取出每个选手跑的最快的3个时间.以下是四位选手的9次成绩 James 2-3 ...
PyTorch框架学习四——计算图与动态图机制
PyTorch框架学习四--计算图与动态图机制一.计算图二.动态图与静态图三.torch.autograd 1.torch.autograd.backward() 2.torch.autogra ...
Docker学习四：Docker 网络
前言本次学习来自于datawhale组队学习: 教程地址为: https://github.com/datawhalechina/team-learning-program/tree/master/ ...
（转）SpringMVC学习(四)——Spring、MyBatis和SpringMVC的整合
http://blog.csdn.net/yerenyuan_pku/article/details/72231763 之前我整合了Spring和MyBatis这两个框架,不会的可以看我的文章MyBa ...
算法学习四：算法性能分析理论基础——函数增长与渐进分析
算法学习四:算法性能分析理论基础--函数增长与渐进分析在算法性能分析过程中,特别是在算法运行效率分析中,我们经常使用渐渐分析法,它使我们在分析算法性能时不必纠结于不同硬件平台的差异性,着重考虑算法的 ...
Tensorflow学习四---高阶操作
Tensorflow学习四-高阶操作 Merge and split 1.tf.concat 拼接 a = tf.ones([4,32,8]) b = tf.ones([2,32,8]) print( ...
[jQuery学习系列四 ]4-Jquery学习四-事件操作
[jQuery学习系列四 ]4-Jquery学习四-事件操作前言: 今天看知乎偶然看到中国有哪些类似于TED的节目, 回答中的一些推荐我给记录下来了, 顺便也在这里贴一下: 一席云集听道推酷 ...
AVI音视频封装格式学习(四)——linux系统C语言AVI格式音视频封装应用
拖了很久的AVI音视频封装实例,花了一天时间终于调完了,兼容性不是太好,但作为参考学习使用应该没有问题.RIFF和AVI以及WAV格式,可以参考前面的一些文章.这里详细介绍将一个H264视频流和一个2 ...
C1认证学习四（多媒体基础参数）
C1认证学习四(多媒体基础参数) 文章目录 C1认证学习四(多媒体基础参数) 任务学习任务目标参数的定义比特率采样率采样位深任务学习所谓的多媒体指的是多种媒体的综合,一般都包含有图像.声 ...

LOAM_velodyne学习（四）

TransformMaintenance

LOAM_velodyne学习（四）相关推荐

最新文章

热门文章