ROS:通过topic发布栅格地图，让RVIZ显示

通过topic发布栅格地图，让RVIZ显示

本文主要同个一个简单的例子实现通过读地图map.pgm文件，使用topic发出后，在rviz上查看。
例子：

#include <ros/ros.h> 
#include <iostream>
#include <opencv2/core/core.hpp> 
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "nav_msgs/OccupancyGrid.h"

#include <string>

int main (int argc, char** argv) 
{ 
  //初始化节点 
  ros::init(argc, argv, "serial_example_node"); 
  //声明节点句柄 
  ros::NodeHandle nh; 

  std::string file_path;
  nh.param("file_path", file_path, file_path);
  if(file_path.empty())
  {
    std::cout<<"file path is empty"<<std::endl;
  }


  ros::Publisher map_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/map_loader/map", 1, true);



  std::string grid_map_path = file_path + "map_0.pgm";

  // std::cout<<"map_path : "<<grid_map_path<<std::endl;

  //加载栅格地图
  cv::Mat grid_map = cv::imread(grid_map_path.c_str(), cv::IMREAD_GRAYSCALE);

  // std::cout<<"grid_map("<<grid_map.rows<<", "<<grid_map.cols<<std::endl;

  // nav_msgs::GetMap::Response resp;
  nav_msgs::OccupancyGrid map_msg;
  map_msg.header.frame_id = "map";
  map_msg.info.width = grid_map.cols;
  map_msg.info.height = grid_map.rows;
  map_msg.info.resolution = 0.05;

  map_msg.info.origin.position.x = 1.02321215e+01 - 25;
  map_msg.info.origin.position.y = 1.19620981e+01 - 25;

  map_msg.info.origin.orientation.w = 1.0;

  map_msg.data.resize(map_msg.info.width * map_msg.info.height);

  for(int i = 0; i < grid_map.cols; i++)
  {
    for (int j = 0; j < grid_map.rows; j++)
    {
        int value = grid_map.at<uchar>(i, j);
        if (value == 0)
        {
            map_msg.data[i * grid_map.cols + j] = 100;  // occupied
        }
        else if (value == 255)
        {
            map_msg.data[i * grid_map.cols + j] = 0;  // free
        }
        else
        {
            map_msg.data[i * grid_map.cols + j] = -1;  // unknown
        }
    }
  }

  //指定循环的频率 
  ros::Rate loop_rate(5); 
  while(ros::ok()) 
  { 
    map_pub_.publish(map_msg);

  //处理ROS的信息，比如订阅消息,并调用回调函数 
  ros::spinOnce(); 
  loop_rate.sleep(); 
  } 
} 

此处关于原点坐标
是直接写的固定值，使用SLAM建图后，在地图文件夹一般般都会存在map.yaml和map_0.pgm文件，map.yaml文件中的坐标就是pgm栅格图片的中心位置对应的坐标。如果有多张pgm图片，就可以根据map.yaml中的坐标，将图片拼接成一张大图。
map.yaml的格式如下

%YAML:1.0
---
image: "map.pgm"
resolution: 5.0000000745058060e-02
submaps: 1.
occupied_thresh: 1.9600000000000001e-01
free_thresh: 6.5000000000000002e-01
submap_0: !!opencv-matrix
   rows: 1
   cols: 2
   dt: f
   data: [ 1.02321215e+01, 1.19620981e+01 ]

本例只有一张图片，因为图片大小是像素1000X1000的。分辨率是0.05m，即一个像素表示5cm。现在有了中心点的坐标(1.02321215e+01,1.19620981e+01)，求原点坐标，即使用中心点的坐标，X,Y分别减去半张图片的大小(500 * 0.05)即25m。得到的就是原点的坐标。

  map_msg.info.origin.position.x = 1.02321215e+01 - 25;
  map_msg.info.origin.position.y = 1.19620981e+01 - 25;

这个例子就没有做解析yaml的部分，直接写上了原点坐标。
launch文件也写的很简单。如下sample.launch

<?xml version="1.0" ?>
<launch>
	<node name="sample" pkg="sample" type="sample" output="screen"/>
	<param name= "file_path" value="$(find sample)/map/" type="string"/>
</launch>

这样程序写好后，就可以运行，然后发出的topic是/map_loader/map. 启动rviz，订阅这个topic就可以了。
rviz启动后，选择[add] 添加topic 选择map，即可。