gmapping 中的很多代码是没有什么用的,重点看的是gridslamprocessor/scanmatcher/slam_gmapping如果其中函数不懂再找对应的文件就行了
slam_mapping.cpp
A. startLiveSlam()函数的注释
https://blog.csdn.net/qq_25368751/article/details/80635848
B. slam_gmapping.cpp中的startReplay()函数
这个函数可以用于用bag包进行gmapping建图
代码是一起的,网页识别不出来
void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_topic)
{
double transform_publish_period;
ros::NodeHandle private_nh_("~");
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);
rosbag::Bag bag;
bag.open(bag_fname, rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back(std::string("/tf"));
topics.push_back(scan_topic);
rosbag::View viewall(bag, rosbag::TopicQuery(topics));
// Store up to 5 messages and there error message (if they cannot be processed right away)
std::queue<std::pair<sensor_msgs::LaserScan::ConstPtr, std::string> > s_queue;
foreach(rosbag::MessageInstance const m, viewall)
{
tf::tfMessage::ConstPtr cur_tf = m.instantiate<tf::tfMessage>();
if (cur_tf != NULL)
{
for (size_t i = 0; i < cur_tf->transforms.size(); ++i)
{
geometry_msgs::TransformStamped transformStamped;
tf::StampedTransform stampedTf;
transformStamped = cur_tf->transforms[i];
tf::transformStampedMsgToTF(transformStamped, stampedTf);
tf_.setTransform(stampedTf);
}
}
sensor_msgs::LaserScan::ConstPtr s = m.instantiate<sensor_msgs::LaserScan>();
if (s != NULL)
{
if (!(ros::Time(s->header.stamp)).is_zero())
{
s_queue.push(std::make_pair(s, ""));
}
// Just like in live processing, only process the latest 5 scans
if (s_queue.size() > 5)
{
ROS_WARN_STREAM("Dropping old scan: " << s_queue.front().second);
s_queue.pop();
}
// ignoring un-timestamped tf data
}
// Only process a scan if it has tf data
while (!s_queue.empty())
{
try
{
tf::StampedTransform t;
tf_.lookupTransform(s_queue.front().first->header.frame_id, odom_frame_, s_queue.front().first->header.stamp, t);
this->laserCallback(s_queue.front().first);
s_queue.pop();
}
// If tf does not have the data yet
catch(tf2::TransformException& e)
{
// Store the error to display it if we cannot process the data after some time
s_queue.front().second = std::string(e.what());
break;
}
}
}
bag.close();
}
依次函数体内使用到的节点,调用的函数等等
1.ros::NodeHandle 节点类的作用与使用
作用:在函数中是为了通过构造函数指定命名空间。
https://blog.csdn.net/jack_20/article/details/70746736
https://www.ncnynl.com/archives/201702/1297.html
2.push_back()函数
.push_back(X) 将元素X加入到c容器的最后一位。
3.rosbag.view
http://docs.ros.org/diamondback/api/rosbag/html/c++/classrosbag_1_1View.html