ros-semantic-mapper 语义地图开源ROS包调试

2016年ICRA的一篇创建语义地图的论文：

Place Categorization and Semantic Mapping on a Mobile Robot

作者已经将代码打包成ROS包开源，直接运行就有不错的效果。这里记录下我自己的调试过程。

1 ros-semantic-mapper 的运行步骤：

git网址： https://github.com/fdayoub/ros-semantic-mapper/blob/master/README.md

操作步骤git里说的比较清楚了已经，这里只解释下后面不太好理解的几步：

• categoryIndex_places205.csv里存的是能够识别的场景及其label，我们可以把需要的场景和label拷贝到my_cats.txt文件中，并给它一个特定的颜色.
• 在semantic_label_publisher/launch/default.launch文件里，需要全部改成自己的路径.
• 启动launch，并播放bag后，打开rviz，把map, odom, semantic_mapper/cloud都显示出来，调整semantic_mapper/cloud的大小和形状，就可以看见涂抹的颜色了，在显示/sem_label_image，还能实时看见image和识别出各种场景的概率

在终端中输入下面的命令，即可启动ros-sematic-mapper：

roscore
rosbag play rosbag_G6_big_1.bag --clock 
roslaunch semantic_mapper run_system_amcl_office.launch

所有话题如下：

nlsde@nlsde-desktop:~$ rostopic list 
/amcl_pose
/camera/rgb/image_mono
/clock
/initialpose
/map
/odom
/oneLabel_cloud
/particlecloud
/rosout
/rosout_agg
/scan
/sem_label_image
/semantic_label
/semantic_mapper/cloud
/tf
/tf_static

可以看到我的数据集发布的话题为/scan /odom /camera/rgb/image_mono /tf
由于和作者给出的数据集的话题不同，所以需要在源码中把话题名称改了，改变的文件如下：
semantic_mapper.h:

/**
Copyright (C) 2016, by 
Feras Dayoub (feras.dayoub@gmail.com) 

This is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This software package is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Leser General Public License.
If not, see <http://www.gnu.org/licenses/>.
**/
#include "semantic_mapper/SemLabel.h"
#include <math.h>

#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <sensor_msgs/Image.h>

#include "message_filters/cache.h"
#include <message_filters/subscriber.h>
#include <laser_geometry/laser_geometry.h>


#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/PCLPointCloud2.h>
// #include <pcl/ros/conversions.h>
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/io/pcd_io.h>

#include <octomap/OcTree.h>
#include <octomap/OcTreeKey.h>
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
#include <octomap_ros/conversions.h>
#include <octomap/octomap_types.h>

#include "semantic_mapper/GetSemMap.h"



boost::mutex m_lock_;

typedef pcl::PCLPointCloud2 PclCloud;
typedef pcl::PCLPointCloud2::Ptr PclCloudPtr;
typedef sensor_msgs::PointCloud2 RosCloud;
typedef sensor_msgs::PointCloud2Ptr RosCloudPtr;


typedef octomap::ColorOcTreeNode SemCell;
typedef octomap::ColorOcTree SemMap;
typedef octomap::ColorOcTreeNode::Color CellColor;

#define Pr 0.299;
#define Pg 0.587;
#define Pb 0.114;

using namespace message_filters ;


class SemanticMapper{
   
    private:
        std::string inputLabelTopic_;
        std::string outputCloudTopic_;
        std::string laserTopic_;
        ros::NodeHandle nh_;
        ros::NodeHandle local_nh_;
        ros::Subscriber labelSub_;
        ros::Subscriber laserSub_;
        ros::Publisher cloudPub_;
        ros::Publisher oneLabel_pub_;
        sensor_msgs::Image image_;

        ros::ServiceServer save_image_srv;

        tf::TransformListener *tf_listener_;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr label_cloud_ptr_;
        bool send_semantic_cloud;
        laser_geometry::LaserProjection projector_;
        int current_color[3];
        ros::Time label_time;

        message_filters::Cache<sensor_msgs::LaserScan> cache;

        SemMap *semMap;
        double cell_resolution_;
        double sem_ProbHit_;
        double sem_ProbMiss_;

        double prob_thres_;


    public:
        SemanticMapper(
                        ros::NodeHandle nh,
                        ros::NodeHandle local_nh
                ):
            local_nh_(local_nh),
            nh_(nh)

        {
            tf_listener_ = new tf::TransformListener(local_nh_, ros::Duration(100));

            local_nh_.param<std::string>("label_topic",inputLabelTopic_,"/semantic_label");
            local_nh_.param<std::string>("output_cloud",outputCloudTopic_,"/semantic_mapper/cloud");
            local_nh_.param<std::string>("laser_topic",laserTopic_,"/scan");

            local_nh_.param<double>("prob_thres",prob_thres_,0.5);

            labelSub_ = nh_.subscribe(inputLabelTopic_, 1,&SemanticMapper::label_CB,this);

            laserSub_ = nh_.subscribe(laserTopic_, 1,&SemanticMapper::scanCallback,this);

            cloudPub_ = nh_.advertise<sensor_msgs::PointCloud2> (outputCloudTopic_,1);

            save_image_srv  = local_nh_.advertiseService("get_semantic_map",&SemanticMapper::get_sem_map_CB_, this);

            oneLabel_pub_ = nh_.advertise<sensor_msgs::PointCloud2> ("oneLabel_cloud", 30);


            send_semantic_cloud = false;
            current_color[0] = 0;
            current_color[1] = 0;
            current_color[2] = 0;
            cache.setCacheSize(1000);
            cell_resolution_ = 0.1;
            semMap = new SemMap(cell_resolution_);
            semMap->setClampingThresMax(1.0);
            semMap->setOccupancyThres(0);

        }

         
        bool get_sem_map_CB_(semantic_mapper::GetSemMap::Request& req,semantic_mapper::GetSemMap::Response& res){
            std::ofstream outFile;
            outFile.open("~/semMap.txt");
            std::stringstream ss;
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr pOut(new pcl::PointCloud<pcl::PointXYZRGB>());
            std::cout << "receive request for label_id  " << req.label_id  << "\n";
            for( SemMap::iterator itr = semMap->begin_leafs(), end = semMap->end_leafs(); itr != end; itr++){
                ss << itr.getCoordinate().x() <<" "<<itr.getCoordinate().y()<<" "<<itr.getCoordinate().z()<<" ";
                SemCell *cell0 = semMap->search(itr.getCoordinate());
                CellColor c = cell0->getColor();
                uint8_t r = (uint8_t)c.r;
                uint8_t g = (uint8_t)c.g;
                uint8_t b = (uint8_t)c.b;

                this->changeSaturation(r,g,b, cell0->getOccupancy());

                ss << cell0->getOccupancy() <<" "<<(int)r<<" "<<(int)g<<" "<< (int)b <<"\n";

                if (int(itr.getCoordinate().z()) == (req.label_id-1) ){
                    SemCell *cell = semMap->search(itr.getCoordinate());

                    pcl::PointXYZRGB pcl_point;

                    pcl_point.x = itr.getCoordinate().x();pcl_point.y = itr.getCoordinate().y();pcl_point.z = cell->getOccupancy();
                    CellColor c = cell->getColor();
                    pcl_point.r = c.r; pcl_point.g = c.g; pcl_point.b = c.b;
                    this->changeSaturation(pcl_point.r,pcl_point.g,pcl_point.b, cell->getOccupancy());

                    pOut->push_back(pcl_point);
                }

            }