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);
                }

            }
            //pcl::io::savePCDFileBinary("/media/Work/test.pcd",*pOut);
            std::cout<<"found "<< pOut->size() << " points\n";
            PclCloudPtr pclOut(new PclCloud());
            pcl::toPCLPointCloud2(*pOut,*pclOut);
            RosCloud outputCloud;
            pcl_conversions::fromPCL(*pclOut,outputCloud);
            outputCloud.header.frame_id = "map";
            outputCloud.header.stamp = ros::Time::now();
            oneLabel_pub_.publish(outputCloud);
            res.done = true;
            outFile << ss.rdbuf();
            outFile.close();
            return true;
        }

        

        void label_CB(const semantic_mapper::SemLabel msg){
            boost::mutex::scoped_lock l(m_lock_);
            std::cout<< "messgage received" << std::endl;
            label_time = msg.header.stamp;
            sensor_msgs::LaserScan::ConstPtr laser_scan = cache.getElemBeforeTime(label_time);

            if ((laser_scan) && (msg.prob[msg.lvl] > prob_thres_)){
                pub_sem_cloud(laser_scan,msg.prob,msg.lvl,msg.r,msg.g,msg.b);
            }else{
                     std::cout << "no scan found" << "\n";
                }

            }


        void pub_sem_cloud(const sensor_msgs::LaserScanConstPtr& laser_scan,std::vector<float> prob,int lvl,std::vector<int> cr,std::vector<int> cg,std::vector<int> cb){


                std::cout << "scan received\n";
                sensor_msgs::LaserScan scan(*laser_scan);
                scan.header = laser_scan->header;

                for (int i=0; i < scan.ranges.size(); i++){
                    if ((scan.ranges[i] == 0) || (scan.ranges[i] >  5)){
                          scan.ranges[i] = 3;
                    }
                }

                RosCloud input;
                try{

                projector_.transformLaserScanToPointCloud("base_footprint", scan, input, *tf_listener_);

                }catch(std::exception&){

                }
                PclCloudPtr source(new PclCloud());
                pcl_conversions::toPCL(input,*source);
                pcl::PointCloud<pcl::PointXYZ>::Ptr raw_c (new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr filled_c (new pcl::PointCloud<pcl::PointXYZRGB>);
                pcl::fromPCLPointCloud2(*source, *raw_c);

                for( pcl::PointCloud<pcl::PointXYZ>::iterator iter = raw_c->begin(); iter != raw_c->end(); ++iter)
                {
                    float alpha =  std::atan2(iter->x,iter->y);
                    if ((alpha < 1.0) || (alpha > 2.0))
                            continue;

                    float d = std::sqrt(iter->x*iter->x + iter->y*iter->y);
                    int max_n = int (d/0.1);
                    for (int i=10; i <max_n; i++){
                        for (int l=0;l < prob.size();l++){
                            pcl::PointXYZRGB p;
                            p.x = (iter->x * i) / max_n;
                            p.y = (iter->y * i) / max_n;
                            p.z = l;
                            uint8_t r = 0, g =0, b = 0;
                            r = cr[l];
                            g = cg[l];
                            b = cb[l];
                            uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
                            p.rgb = *reinterpret_cast<float*>(&rgb);
                            filled_c->push_back(p);
                        }
                        }
                }



                //PclCloudPtr pclOut(new PclCloud());
                //pcl::toPCLPointCloud2(*filled_c,*pclOut);
                //RosCloud outputCloud;
                //pcl_conversions::fromPCL(*pclOut,outputCloud);
                //outputCloud.header.frame_id = laser_scan->header.frame_id;
                //outputCloud.header.stamp = laser_scan->header.stamp;
                //cloudPub_.publish(outputCloud);

                tf::StampedTransform sensor_tf;
                try {
                    tf_listener_->waitForTransform("map", "base_link",ros::Time::now(),ros::Duration(0.2));
                    tf_listener_->lookupTransform("map", "base_link",
                            laser_scan->header.stamp, sensor_tf);
                } catch (tf::TransformException &ex) {
                    ROS_ERROR("%s", ex.what());
                    return;
                }

                geometry_msgs::Pose curr_pose;
                curr_pose.position.x = sensor_tf.getOrigin().x();
                curr_pose.position.y = sensor_tf.getOrigin().y();
                curr_pose.orientation.w = sensor_tf.getRotation().getW();
                curr_pose.orientation.z = sensor_tf.getRotation().getZ();


            filled_c->sensor_origin_[0] = sensor_tf.getOrigin().x();
            filled_c->sensor_origin_[1] = sensor_tf.getOrigin().y();
            filled_c->sensor_origin_[2] = sensor_tf.getOrigin().z();
            filled_c->sensor_orientation_.z() = sensor_tf.getRotation().z();
            filled_c->sensor_orientation_.w() = sensor_tf.getRotation().w();
            filled_c->sensor_orientation_.x() = sensor_tf.getRotation().x();
            filled_c->sensor_orientation_.y() = sensor_tf.getRotation().y();
            repositionCloud(filled_c, filled_c);

             octomap::Pointcloud octoCloud;

             for(pcl::PointCloud<pcl::PointXYZRGB>::const_iterator it= filled_c->begin(); it != filled_c->end(); it++){
                 octomap::point3d point(it->x, it->y, it->z);
                 float lo = std::log(prob[it->z]/(1-prob[it->z]));
                 semMap->updateNode(point,lo);
                 SemCell* cell = semMap->search(point);
                 CellColor cS(it->r,it->g,it->b);
                 cell->setColor(cS);
             }


             pcl::PointCloud<pcl::PointXYZRGB>::Ptr pOut(new pcl::PointCloud<pcl::PointXYZRGB>());
             for( SemMap::iterator itr = semMap->begin_leafs(), end = semMap->end_leafs(); itr != end; itr++){

                 SemCell *cell = semMap->search(itr.getCoordinate());
                 for (int l=0;l < prob.size();l++){
                    SemCell *tcell = semMap->search(itr.getCoordinate().x(),itr.getCoordinate().y(),l);

                    if (tcell->getOccupancy() > cell->getOccupancy()){
                        cell = tcell;                        

                    }
                 }
                 pcl::PointXYZRGB pcl_point;
                 pcl_point.x = itr.getCoordinate().x();pcl_point.y = itr.getCoordinate().y();pcl_point.z = 0;
                 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);
             }

             PclCloudPtr pclOut(new PclCloud());
             pcl::toPCLPointCloud2(*pOut,*pclOut);
             RosCloud outputCloud;
             pcl_conversions::fromPCL(*pclOut,outputCloud);
             outputCloud.header.frame_id = "map";
             outputCloud.header.stamp = laser_scan->header.stamp;
             cloudPub_.publish(outputCloud);

        }
        
        void scanCallback(const sensor_msgs::LaserScanConstPtr& laser_scan){         
            cache.add(laser_scan);
        }
private:
        void repositionCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out) {
                Eigen::Matrix3f rotMatrix =
                        cloud_in->sensor_orientation_.toRotationMatrix();
                Eigen::Matrix4f t;
                t(0, 0) = rotMatrix(0, 0);
                t(0, 1) = rotMatrix(0, 1);
                t(0, 2) = rotMatrix(0, 2);
                t(1, 0) = rotMatrix(1, 0);
                t(1, 1) = rotMatrix(1, 1);
                t(1, 2) = rotMatrix(1, 2);
                t(2, 0) = rotMatrix(2, 0);
                t(2, 1) = rotMatrix(2, 1);
                t(2, 2) = rotMatrix(2, 2);

                t(3, 0) = t(3, 1) = t(3, 2) = 0;
                t(3, 3) = 1;
                t(0, 3) = cloud_in->sensor_origin_[0];
                t(1, 3) = cloud_in->sensor_origin_[1];
                t(2, 3) = cloud_in->sensor_origin_[2];
                pcl::transformPointCloud(*cloud_in, *cloud_out, t);

            }


        // Robert Atkins, December 2010 (ratkins_at_fastmail_dot_fm).
        //
        // https://github.com/ratkins/RGBConverter
        void changeSaturation(uint8_t& r_in, uint8_t& g_in, uint8_t& b_in, double change) {


            double rd = (double) r_in;///255;
                double gd = (double) g_in;///255;
                double bd = (double) b_in;///255;
                double max = threeway_max(rd, gd, bd), min = threeway_min(rd, gd, bd);
                double h, s, v = max;

                double d = max - min;
                s = max == 0 ? 0 : d / max;

                if (max == min) {
                    h = 0; // achromatic
                } else {
                    if (max == rd) {
                        h = (gd - bd) / d + (gd < bd ? 6 : 0);
                    } else if (max == gd) {
                        h = (bd - rd) / d + 2;
                    } else if (max == bd) {
                        h = (rd - gd) / d + 4;
                    }
                    h /= 6;
                }


                s = s * change;

                 double r, g, b;
                int i = int(h * 6);
                    double f = h * 6 - i;
                    double p = v * (1 - s);
                    double q = v * (1 - f * s);
                    double t = v * (1 - (1 - f) * s);

                    switch(i % 6){
                        case 0: r = v, g = t, b = p; break;
                        case 1: r = q, g = v, b = p; break;
                        case 2: r = p, g = v, b = t; break;
                        case 3: r = p, g = q, b = v; break;
                        case 4: r = t, g = p, b = v; break;
                        case 5: r = v, g = p, b = q; break;
                    }

                    r_in = (uint8_t)r;
                    g_in = (uint8_t)g;
                    b_in = (uint8_t)b;

        }


        double threeway_max(double a, double b, double c) {
            return std::max(a, std::max(b, c));
        }

        double threeway_min(double a, double b, double c) {
            return std::min(a, std::min(b, c));
        }

        double hue2rgb(double p, double q, double t) {
            if(t < 0) t += 1;
            if(t > 1) t -= 1;
            if(t < 1/6) return p + (q - p) * 6 * t;
            if(t < 1/2) return q;
            if(t < 2/3) return p + (q - p) * (2/3 - t) * 6;
            return p;
        }

};

run_amcl_office.launch:

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<remap from="scan" to="scan"/>
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="1000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="global_frame_id" value="map"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="13.512"/>
<param name="initial_pose_y" value="-21.727"/>
<param name="initial_pose_a" value="5.2"/>
</node>
</launch>

semantic_label_publisher_node.py

#!/usr/bin/env python
'''
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/>.
'''
import roslib; roslib.load_manifest('semantic_label_publisher') 
import numpy as np
import rospy
import sys
sys.path.append('/home/nlsde/caffe-master/python')
sys.path.append('/home/nlsde/caffe-master/python/caffe')
import cv2
import cPickle
import gzip
import caffe
from sklearn.externals import joblib
from sklearn import svm
import os.path
import time
from read_cat import cats
from sensor_msgs.msg import Image , LaserScan

from cv_bridge import CvBridge, CvBridgeError

from semantic_label_publisher.msg import SemLabel
class SemanticLabel(object):
  def __init__(self,lname,lid,lcolor):
    self.label_name  = lname
    self.label_id    = int(lid)
    self.label_color = [int(lcolor[0]),int(lcolor[1]),int(lcolor[2])]

class SemLabelPub():
  def __init__(self,caffe_root,MODEL_FILE,PRETRAINED,MEAN_FILE,SVM_PICKLE_FILE,SUB_CAT_FILE):
    self.pub       = rospy.Publisher('semantic_label',SemLabel)
    self.image_pub = rospy.Publisher("sem_label_image",Image)

    self.image_topic = rospy.get_param('~image_topic','/camera/rgb/image_mono')
    self.image_sub   = rospy.Subscriber(self.image_topic,Image,self.image_callback)

    self.bridge = CvBridge()

    self.net         = caffe.Classifier(MODEL_FILE, PRETRAINED,caffe.TEST)
    self.transformer = caffe.io.Transformer({'data': (1,3,227,227)})

    self.transformer.set_transpose('data', (2,0,1))
    self.net.blobs['data'].reshape(1,3,227,227)

    db_mean = np.load(MEAN_FILE)
    self.transformer.set_mean('data', db_mean.mean(1).mean(1))

    caffe.set_mode_gpu()

    self.msg = SemLabel()

    with open(SUB_CAT_FILE, 'r') as f:
      txt_data = f.readlines()

    self.labels = list()
    for l in txt_data:
      if l[0] != '#':
        label_name    = l.split(" ")[0]
        label_id      = l.split(" ")[1] 
        label_color   = l.split(" ")[2].split(",")
        self.labels.append(SemanticLabel(label_name,label_id,label_color))


    self.font_size = 3
    self.font_thickness = 5

  def image_callback(self,data):
    print "new image received"
    try:
      cv_img = self.bridge.imgmsg_to_cv2(data, "bgr8")
      cv_img = cv2.resize(cv_img,(227,227))
      print 'image received'
    except CvBridgeError, e:
      print e

    im_input = self.transformer.preprocess('data',cv_img) 
    im_input = im_input[np.newaxis] 
    self.net.blobs['data'].reshape(*im_input.shape)
    self.net.blobs['data'].data[...] = im_input
    self.net.forward()

    if 'fc7' in self.net.blobs.keys():
      feature  =  self.net.blobs['fc7'].data
      prob     =  self.net.blobs['prob'].data
      result          = list()
      self.all_lables = list() 
      for l in self.labels:
          idx = l.label_id
          self.all_lables.append(l.label_name)
          result.append(prob[0,idx])
      result = np.array(result,np.dtype(float))
      result = result / np.sum(result)

    self.msg.header.stamp = data.header.stamp
    self.msg.header.frame_id = 'base_link'
    class_idx = np.argmax(result)    
    class_name = self.all_lables[class_idx]
    print class_name

    self.msg.r = [k.label_color[0] for k in self.labels]
    self.msg.g = [k.label_color[1] for k in self.labels] 
    self.msg.b = [k.label_color[2] for k in self.labels] 
    self.msg.prob = result 
    self.msg.lvl = class_idx

    self.pub.publish(self.msg)
    text_x = 10
    text_y = 100
    cv_img = cv2.resize(cv_img,(1280,960))
    font = cv2.FONT_HERSHEY_PLAIN
    for c in range(len(result)):
      cv2.rectangle(cv_img,(text_x+0,text_y),(text_x+0+int(300*float(result[c])),text_y - 20), (255,0,0),20)
      if c == class_idx:
        cv2.putText(cv_img,self.all_lables[c] + ' ' ,(text_x,text_y), font, self.font_size,(0,255,0),self.font_thickness)        
      else:
        cv2.putText(cv_img,self.all_lables[c] + ' ' ,(text_x,text_y), font, self.font_size,(0,0,255),self.font_thickness)        
      text_y += 50

    try:
      self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_img, "bgr8"))
    except CvBridgeError, e:
      print e



def main(args):
  rospy.init_node('sem_label_pub')
  caffe_root      = rospy.get_param('~caffe_root', '~/caffe/')
  MODEL_FILE      = rospy.get_param('~MODEL_FILE_PATH', '~/deploy.prototxt')
  PRETRAINED      = rospy.get_param('~PRETRAINED_PATH', '~/model.caffemodel')
  MEAN_FILE       = rospy.get_param('~MEAN_FILE_PATH', '~/mean.npy')
  SVM_PICKLE_FILE = rospy.get_param('~SVM_PICKLE_FILE_PATH', '~/clf.pkl')
  SUB_CAT_FILE    = rospy.get_param('~SUB_CAT_FILE','~/sub_cats.txt')
  try:
    #caffe_root = '/home/nlsde/caffe-master'
    #sys.path.insert(0, caffe_root + 'python')
    print('caffe_root is : ', caffe_root)
    ne = SemLabelPub(caffe_root,MODEL_FILE,PRETRAINED,MEAN_FILE,SVM_PICKLE_FILE,SUB_CAT_FILE)
  except rospy.ROSInterruptException: pass
  rospy.spin()

if __name__ == '__main__':
  main(sys.argv)

semantic_label_publisher/ launch/ default.launch:

<launch>

<node pkg="topic_tools" name="throttle" type="throttle" args="messages /camera/rgb/image_mono" />
<node pkg="semantic_label_publisher" type="semantic_label_publisher_node.py" name="semantic_label_publisher_node" output="screen">

<param name="image_topic" value="/camera/rgb/image_mono"/>

<param name="caffe_root" value="/home/nlsde/caffe-master" />

<param name="MODEL_FILE_PATH" value="/home/nlsde/hellow/catkin_ws/src/ros-semantic-mapper/param_files/deploy.prototxt" />

<param name="PRETRAINED_PATH" value="/home/nlsde/hellow/catkin_ws/src/ros-semantic-mapper/param_files/places.caffemodel" />

<param name="MEAN_FILE_PATH" value="/home/nlsde/hellow/catkin_ws/src/ros-semantic-mapper/param_files/mean.npy" />

<param name="SUB_CAT_FILE" value="/home/nlsde/hellow/catkin_ws/src/ros-semantic-mapper/param_files/my_cats.txt" />

</node>

</launch>

run_system_amcl_office.launch

<launch>
   <include file="$(find semantic_mapper)/launch/default.launch"/>
   <!--node pkg="map_server" type="map_server" name="map_server" args="$(find semantic_mapper)/maps/office.yaml" /-->
   <include file="$(find semantic_label_publisher)/launch/default.launch"/>
   <include file="$(find semantic_mapper)/launch/places/run_amcl_office.launch"/>

</launch>

rviz截图：

topic和tf:

2 调试过程中的问题

2.1 找不到 octomap-ros这个包，错误：

CMake Error at ros-semantic-mapper/semantic_mapper/CMakeLists.txt:17 (find_package):

By not providing “Findoctomap-ros.cmake” in CMAKE_MODULE_PATH this project

has asked CMake to find a package configuration file provided by

“octomap-ros”, but CMake did not find one.

Could not find a package configuration file provided by “octomap-ros” with

any of the following names:

octomap-rosConfig.cmake

octomap-ros-config.cmake

Add the installation prefix of “octomap-ros” to CMAKE_PREFIX_PATH or set

“octomap-ros_DIR” to a directory containing one of the above files. If

“octomap-ros” provides a separate development package or SDK, be sure it

has been installed.

解决：要用 octomap-ros包，只安装octomap包是不行的，还得装其他的：

https://answers.ros.org/question/248242/cmake-could-not-find-octomap_ros-when-run-catkin_make/

2.2 python找不到caffe

出现了 ImportError: No module named caffe 这样的错误，先检查是不是caffe配置的问题：
在terminal里运行python，并import caffe，发现仍然有这个错误，说明不是launch的问题，参考：
https://blog.csdn.net/gn102038/article/details/72802630
https://blog.csdn.net/striker_v/article/details/51596628

在终端的python下输入：

import sys
sys.path.append("/（你的caffe-master路径）/caffe-master/python")
sys.path.append("/（你的caffe-master路径）/caffe-master/python/caffe")
import caffe

出现如下错误：
can not find module skimage.io
参考：
https://blog.csdn.net/tsinghuahui/article/details/46790705
https://blog.csdn.net/gn102038/article/details/72802630
安装必要的包后，重新编译caffe：
$ sudo apt-get install python-numpy python-scipy python-matplotlib python-sklearn python-skimage python-h5py python-protobuf python-leveldb python-networkx python-nose python-pandas python-gflags Cython ipython
$ sudo apt-get update
cd caffe #进入caffe目录，可能你的是cd caffe-master

    sudo make clean #清除原编译
    sudo make -j4  # make -j4或者make -j8 这根据你自己电脑来定
    sudo make pycaffe   #最关键的一个
    sudo  make runtest   #测试编译

编译并测试成功后，重新在终端的python中测试import caffe，发现能够成功。
最后，在semantic_label_publisher_node.py文件中，添加两句：

sys.path.append('/home/nlsde/caffe-master/python')
sys.path.append('/home/nlsde/caffe-master/python/caffe')

改写后的文件：

import roslib; roslib.load_manifest('semantic_label_publisher')
import numpy as np
import rospy
import sys
sys.path.append('/home/nlsde/caffe-master/python')
sys.path.append('/home/nlsde/caffe-master/python/caffe')
import cv2
import cPickle
import gzip
import caffe