栅格地图及其应用

1、栅格地图数据格式

在ROS中，栅格地图消息为nav_msgs/OccupancyGrid

通过指令rosmsg show nav_msgs/OccupancyGrid可以获得栅格地图格式如下：

$ rosmsg show OccupancyGrid
[nav_msgs/OccupancyGrid]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
nav_msgs/MapMetaData info
  time map_load_time
  float32 resolution
  uint32 width
  uint32 height
  geometry_msgs/Pose origin
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
int8[] data

关于地图大小及精度：地图宽度为width栅格，高度为height栅格，精度为resolution (m/栅格)

关于地图原点：The origin of the map [m, m, rad].  This is the real-world pose of the

# cell (0,0) in the map.

即为栅格(0,0)在地图坐标系中的实际坐标（单位为m)

关于data:

The map data, in row-major order, starting with (0,0).  Occupancy
# probabilities are in the range [0,100].  Unknown is -1.

（0表示空闲，白色；100表示占用，黑色)

栅格(cx,cy)在data中的编号ci的对应关系：ci=width*cy+cx

栅格地图元数据，可以通过以下指令获得：

获得地图元信息

rostopic echo /map_metadata -n 1

map_load_time: 

  secs: 1702601864

  nsecs: 716249167

resolution: 0.0500000007451

width: 384

height: 384

origin: 

  position: 

    x: -10.0

    y: -10.0

    z: 0.0

  orientation: 

    x: 0.0

    y: 0.0

    z: 0.0

    w: 1.0

上述地图元素数据说明，栅格地图的宽度和高度都是384个栅格，

每个栅格（正方形）的边长（栅格分辨率）为0.05米。

这个分辨率也说明，移动机器人的定位精度不会超过0.05米。

此外，上述数据还说明，第（0，0）个栅格，也就是

第0行第0列的栅格，在地图坐标系下的坐标为（-10m,-10m,0m)

方向四元数(0,0,0,1)表示两个坐标系方向相同（第（0，0）栅格与地图坐标系方向一致）

关于地图数据的表示：

地图数据存放在data数组中，data数组是一个基类型为int8(及8位的整型，相当于

C语言中的char类型)的一维数组，一维数组可以存放二维数据，采用的是按

行为主序（row-major order)，所谓按行为主序，就是先存放第1行，再

存放第2行，...，

栅格(cx,cy)在data中的编号ci的对应关系：ci=width*cy+cx

为了进一步理解行、列的概念，以及地图数据的存储（和上面的公式），我们

用以下代码来进行测试，

void mappub(void)
{
    //cell: (cx,cy)  data index ci=width*cy+cx
    //(0,5) 5*width  (1,5)  5*width+1
    //(10,0) 10
    map_.data.at(5*width) =0;
    map_.data.at(6*width) =0;
    map_.data.at(5*width+1) =0;
    map_.data.at(6*width+1) =0;    
    map_.data.at(10) =0;
    map_.data.at(11) =0;    
    map_publisher.publish(map_);
}

该代码执行后，对应地图如下图：

由此我们可以看到，前面所说的行，就是沿x轴方向，列则是y轴方向。

关于上述代码的补充说明，上述代码的map_为 nav_msgs::OccupancyGrid 类型，

显然，要获得其大小、分辨率及坐标原点，其表示方式为：

nav_msgs::OccupancyGrid map_;
int width=map_.info.width;
int height=map_.info.height;
float resolution=map_.info.resolution;
float ox=map_.info.origin.position.x;
float oy=map_.info.origin.position.y;

2、栅格地图构建

为了加深对上述栅格地图概念的理解，我们来通过程序构建一个栅格地图，

以夺宝奇兵比赛场地为例，在夺宝奇兵比赛中，机器人在一个3m*3m的封闭空间进行作业。

我们就假设地图大小为6m*6m，在其正中心的3m*3m区域为空闲，在此区域的边界厚度为2个栅格的部分为占用，其它部分为未知。分辨率设置为0.03m，坐标原点设置为(-3.0,-3.0,0)，方向四元数为(0,0,0,1)。

核心代码如下：

/**
 * 节点MapPubv1 
 * 从map话题发布nav_msgs/OccupancyGrid消息
    地图大小为6m*6m，在其正中心的3m*3m区域为空闲，
    在此区域的边界厚度为2个栅格的部分为占用，其它部分为未知。
    分辨率设置为0.03m，坐标原点设置为(-3.0,-3.0,0)，方向四元数为(0,0,0,1)
 
 * 开发者：段琢华,duanzhuohua@163.com 2024-3-21 
 */


#include "ros/ros.h"
#include "nav_msgs/OccupancyGrid.h"
//申明nav_msgs::OccupancyGrid消息类型 
 ros::Publisher map_pub;//发布者 
 nav_msgs::OccupancyGrid map_; 
 void mappub(void)
{
    //cell: (cx,cy)  data index ci=width*cy+cx
    //(0,5) 5*width  (1,5)  5*width+1
    //(10,0) 10
    map_.header.stamp=ros::Time::now();//当期时间;
    map_.header.frame_id="map";
    map_.info.width=int(6/0.03);//地图大小为6m*6m,分辨率设置为0.03m
    map_.info.height=int(6/0.03);
    map_.info.resolution=0.03;
    map_.info.origin.position.x=-3.0;//坐标原点设置为(-3.0,-3.0,0)
    map_.info.origin.position.y=-3.0;
    map_.info.origin.position.z=0;
    map_.info.origin.orientation.x=0;
    map_.info.origin.orientation.y=0;
    map_.info.origin.orientation.z=0;
    map_.info.origin.orientation.w=1;
    map_.data.resize(map_.info.width*map_.info.height);
    map_.data.assign(map_.data.size(),-1);
    int cx_low=int(1.5/0.03);
    int cy_low=int(1.5/0.03);
    int cx_len=int(3/0.03);
    int cy_len=int(3/0.03);
    int width=map_.info.width;
    //(cx_low,cy_low)~(cx_low+cx_len,cy_low) the row cy_low
    //(cx_low-1,cy_low-1)~(cx_low+cx_len+2,cy_low-1) the row cy_low-1
    //(cx_low,cy_low+cy_len)~(cx_low+cx_len,cy_low+cy_len) the row cy_low+cy_len
    //(cx_low-1,cy_low+cy_len+1)~(cx_low+cx_len+2,cy_low+cy_len+1) the row cy_low+cy_len+1
    
    //(cx_low,cy_low)~(cx_low,cy_low+cy_len) the col cx_low
    //(cx_low-1,cy_low-1)~(cx_low-1,cy_low+cy_len+2) the col cx_low-1
    //(cx_low+cx_len,cy_low)~(cx_low+cx_len,cy_low+cy_len) the col cx_low+cx_len
    //(cx_low+cx_len+1,cy_low-1)~(cx_low+cx_len+1,cy_low+cy_len+2) the col cx_low+cx_len+1
    
    int cx,cy,ci;
    for(cx=cx_low;cx<=cx_low+cx_len;cx++)
    {//(cx_low,cy_low)~(cx_low+cx_len,cy_low) the row cy_low
      cy=cy_low;
      ci=width*cy+cx ; 
      map_.data.at(ci)=100;
    }
    
    for(cx=cx_low-1;cx<=cx_low+cx_len+2;cx++)
    {//(cx_low-1,cy_low-1)~(cx_low+cx_len+2,cy_low-1) the row cy_low-1
      cy=cy_low-1;
      ci=width*cy+cx ; 
      map_.data.at(ci)=100;
    }
    
    for(cx=cx_low-1;cx<=cx_low+cx_len+2;cx++)
    {//(cx_low,cy_low+cy_len)~(cx_low+cx_len,cy_low+cy_len) the row cy_low+cy_len
      cy=cy_low+cy_len;
      ci=width*cy+cx ; 
      map_.data.at(ci)=100;
    }
    
    for(cx=cx_low-1;cx<=cx_low+cx_len+2;cx++)
    {//(cx_low-1,cy_low+cy_len+1)~(cx_low+cx_len+2,cy_low+cy_len+1) the row cy_low+cy_len+1
      cy=cy_low+cy_len+1;
      ci=width*cy+cx ; 
      map_.data.at(ci)=100;
    }
    
   
    
    ;;
    for(cy=cy_low;cy<=cy_low+cy_len;cy++)
    {//(cx_low,cy_low)~(cx_low,cy_low+cy_len) the col cx_low
      cx=cx_low;
      ci=width*cy+cx ; 
      map_.data.at(ci)=100;
    }
    
    for(cy=cy_low-1;cy<=cy_low+cy_len+2;cy++)
    {//(cx_low-1,cy_low-1)~(cx_low-1,cy_low+cy_len+2) the col cx_low-1
      cx=cx_low-1;
      ci=width*cy+cx ; 
      map_.data.at(ci)=100;
    }
    
    for(cy=cy_low-1;cy<=cy_low+cy_len+2;cy++)
    {//(cx_low+cx_len,cy_low)~(cx_low+cx_len,cy_low+cy_len) the col cx_low+cx_len
      cx=cx_low+cx_len;
      ci=width*cy+cx ; 
      map_.data.at(ci)=100;
    }
    
    for(cy=cy_low-1;cy<=cy_low+cy_len+2;cy++)
    {//(cx_low+cx_len+1,cy_low-1)~(cx_low+cx_len+1,cy_low+cy_len+2) the col cx_low+cx_len+1
      cx=cx_low+cx_len+1;
      ci=width*cy+cx ; 
      map_.data.at(ci)=100;
    }    
    //(cx_low+1,cy_low+1)~(cx_low+cx_len-1,cy_low+cy_len-1)  :0 free    
    for(cx=cx_low+1;cx<=cx_low+cx_len-1;cx++)
      for(cy=cy_low+1;cy<=cy_low+cy_len-1;cy++)
      {
        ci=width*cy+cx ; 
        map_.data.at(ci)=0;
      } 
    map_pub.publish(map_);
} 
int main(int argc, char **argv)
{
  ros::init(argc, argv, "MapPubv1");
  ros::NodeHandle n;   
  map_pub = n.advertise<nav_msgs::OccupancyGrid>("Map", 5);//初始化发布者对象  
  ros::Rate loop_rate(1);
  //设置发布频率
  while (ros::ok())
  {    
    //计算并发布map
    mappub();
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

构建（上述文件存放于simple_nodes包的src目录下，名字为MapPubv1.cpp）：

修改CMakeLists.txt文件，

（1）增加对nav_msgs包的依赖，如下

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
  turtlesim
  tf
  nav_msgs
)

（2）在CMakeLists.txt文件末尾增加如下两行，

add_executable(MapPubv1 src/MapPubv1.cpp)
target_link_libraries(MapPubv1 ${catkin_LIBRARIES})

转到工作区，利用catkin_make指令构建，成功后，

执行rosrun simple_nodes MapPubv1

在rviz中增加map话题，增加Axes,效果如下：

注：上述地图可以作为“夺宝奇兵”大赛的地图。

扩展训练：编写程序，制作更为复杂的地图。

3、栅格地图编辑

问题：给定地图上的两个点（地图坐标系下）p1,p2,在栅格地图上建立p1到p2两点的虚拟墙

（p1,p2连线所经历的栅格设置为占用）

基础算法：Bresenham 算法

参考:https://zhuanlan.zhihu.com/p/507645801

算法的C语言实现：

void Breseham_Line(int x1, int y1, int x2, int y2)
{
     int dx = x2 - x1;
     int dy = y2 - y1;
     int ux = ((dx > 0) << 1) - 1;//x的增量方向，取或-1
     int uy = ((dy > 0) << 1) - 1;//y的增量方向，取或-1
     int x = x1, y = y1, eps;//eps为累加误差

     eps = 0;dx = abs(dx); dy = abs(dy); 
     if (dx > dy) 
     {
         for (x = x1; x != x2; x += ux)
         {
              printf("%d %d\n",x,y);
              eps += dy;
              if ((eps << 1) >= dx)
              {
                   y += uy; eps -= dx;
              }
         }
     }
     else
     {
         for (y = y1; y != y2; y += uy)
         {
              printf("%d %d\n",x,y);
              eps += dx;
              if ((eps << 1) >= dy)
              {
                   x += ux; eps -= dy;
              }
         }
     }             
}

例如：可以通过调用Breseham_Line(0,0,8,3);来在栅格（0,0）到（8,3）之间画一条直线。

示例代码，MapBubv2.cpp，从Map订阅栅格地图（注：可以改为从地图服务获取地图），

添加一条直线，然后发布新的地图。

/**
 * 节点MapPubv2 
 * usage: MapBupv2 cx1 cy1 cx2 cy2
 从map话题 subscribe nav_msgs/OccupancyGrid消息
   add a line from(cx1,cy1) to (cx2,cy2) to map
   
 * 开发者：段琢华,duanzhuohua@163.com 2024-3-21 
 */


#include "ros/ros.h"
#include "nav_msgs/OccupancyGrid.h"
//申明nav_msgs::OccupancyGrid消息类型
void Breseham_Line(int x1, int y1, int x2, int y2);
void map_Callback(const nav_msgs::OccupancyGrid::ConstPtr& msg);

 
 ros::Publisher map_pub;//发布者
 
 nav_msgs::OccupancyGrid map_;
 int cx1,cy1,cx2,cy2;
 
 void map_Callback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{ 
  map_=*msg;
  Breseham_Line(cx1,cy1,cx2,cy2); 
}

 
 void Breseham_Line(int x1, int y1, int x2, int y2)
{
     
     int dx = x2 - x1;
     int dy = y2 - y1;
     int ux = ((dx > 0) << 1) - 1;//x的增量方向，取或-1
     int uy = ((dy > 0) << 1) - 1;//y的增量方向，取或-1
     int x = x1, y = y1, eps;//eps为累加误差

     eps = 0;dx = abs(dx); dy = abs(dy); 
     if (dx > dy) 
     {
         for (x = x1; x != x2; x += ux)
         {
              //printf("%d %d\n",x,y); 此处改为在栅格地图上输出
              int width=map_.info.width;//
              int ci=width*y+x;
              map_.data.at(ci)=100;
              
              eps += dy;
              if ((eps << 1) >= dx)
              {
                   y += uy; eps -= dx;
              }
         }
     }
     else
     {
         for (y = y1; y != y2; y += uy)
         {
              //printf("%d %d\n",x,y);
              int width=map_.info.width;//
              int ci=width*y+x;
              map_.data.at(ci)=100;
              
              eps += dx;
              if ((eps << 1) >= dy)
              {
                   x += ux; eps -= dy;
              }
         }
     }   
     map_pub.publish(map_);          
}
 




 
 
int main(int argc, char **argv)
{
  ros::init(argc, argv, "MapPubv2");
  if (argc<5){
   ROS_INFO("Usage rosrun simple_nodes MapPubv2 cx1 cy1 cx2 cy2.");   
   //
   return -1;
  }

  
  ros::NodeHandle n;
  
  cx1 = atoi(argv[1]);
  cy1 = atoi(argv[2]);
  cx2 = atoi(argv[3]);
  cy2 = atoi(argv[4]);
  
  ros::Subscriber sub = n.subscribe("Map", 5, map_Callback);
  


   
  map_pub = n.advertise<nav_msgs::OccupancyGrid>("Map", 5);//初始化发布者对象
  
  ros::Rate loop_rate(1);
  //设置发布频率
  while (ros::ok())
  {    

    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}