gmapping阅读第2天

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

  • 1
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值