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