大家好,又见面了,我是你们的朋友全栈君。
前言:
最近正好 用到GMapping,需要改代码, 但看过也总是在忘,那干脆写篇博客记录 下来同时也可以帮助想要了解GMapping代码的同学。
代码的入口依然是main函数,但GMapping代码中由很多是没有用的,所以并 不需要挨个看,可以说代码的作者代码能力挺强但代码风格却是不敢恭维。这里就 不带大家挨个文件度代码,只是对几个主要的 函数进行介绍。
在看代码前,读者最好选择一个IDE来看代码,因为会涉及大量的跳转,如果只是手动去找的话就太累了。yi
main.cpp
int
main(int argc, char** argv)
{
ros::init(argc, argv, "slam_gmapping");
SlamGMapping gn;
gn.startLiveSlam();
ros::spin();
return(0);
}
代码的入口 ,主要作用就是生成了一个类对象,类对象调用成员函数startLiveSlam()。说明一下ros里面基础知识这里就不过多介绍,读者自学一下ros就行。
startLiveSlam()
/*订阅一些主题 发布一些主题*/
void SlamGMapping::startLiveSlam()
{
entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
//订阅激光数据 同时和odom_frame之间的转换同步
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
std::cout <<"Subscribe LaserScan & odom!!!"<<std::endl;
/*发布转换关系的线程*/
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}
这个函数的功能就是发布发布四个消息,订阅一个话题,会涉及boost库的一些知识,网上都有搜一下就行。下一步进行回调函数laserCallback中。
laserCallback()
void SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)//throttle_scans_实现降频,如果激光频率较高而
return; //处理器计算能力有限,可以降低处理激光数据的频率
static ros::Time last_map_update(0,0);
// We can't initialize the mapper until we've got the first scan
if(!got_first_scan_)
{
if(!initMapper(*scan))//第一帧激光数据时执行初始化
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose))//初始化完后,再有激光数据时执行addScan函数
{
ROS_DEBUG("scan processed");
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
map_to_odom_mutex_.lock();
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
map_to_odom_mutex_.unlock();
/*如果没有地图那肯定需要直接更新,如果有地图了则需要到时间了,才更新地图了*/
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
{
/*多久更新一次地图*/
updateMap(*scan);
last_map_update = scan->header.stamp;
ROS_DEBUG("Updated the map");
}
}
else
ROS_DEBUG("cannot process scan");
}
未完待续
发布者:全栈程序员-用户IM,转载请注明出处:https://javaforall.cn/148589.html原文链接:https://javaforall.cn
【正版授权,激活自己账号】: Jetbrains全家桶Ide使用,1年售后保障,每天仅需1毛
【官方授权 正版激活】: 官方授权 正版激活 支持Jetbrains家族下所有IDE 使用个人JB账号...