北达科他大学( North Dakota State University)cloud_to_map学习

一、ROS 动态参数

cfg文件：

#!/usr/bin/env python
#ROS动态参数文件
PACKAGE = "cloud_to_map"

import roslib;roslib.load_manifest(PACKAGE)
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
#以上代码初始化ROS并导入参数生成器。

gen.add("frame", str_t, 0, "Frame the Occupancy Grid should connect to", "map")
gen.add("search_radius", double_t, 0, "Search radius for approximating the normal vectors of a point",0.06, 0.0001, 1)
gen.add("deviation", double_t, 0, "Allowable deviation of the normal vector of a point from the normal vector of the ground plane before the point is considered an obstacle (in Radians)", 0.78539816339, 0, 1.57079632679)
gen.add("buffer", int_t, 0, "Number of points that must register as past the allowable amount of deviation before the corresponding cell is considered an obstacle (modifying search radius is generally more effective)", 0, 0, 100)
gen.add("loop_rate", double_t, 0, "Rate in Hz the node will attempt to run", 10, 0, 1000)
gen.add("cell_resolution", double_t, 0, "Resolution of the Occupancy Grid output (in m/cell)", 0.05, 0, 0.1)

"""以上代码为加入不同的参数。其中gen.add(...)格式如下：
gen.add(name, type, level, description, default, min, max)
name: 参数的名称
type: 参数类型
level:一个传递给回调的位掩码
description: 一个描述参数
default: 节点启动的初始值
min: 参数最小值
max: 参数最大值"""

exit(gen.generate(PACKAGE, "cloud_to_map", "cloud_to_map_node"))
#生成必要的文件并退出。
#上述的cfg中的代码为python代码。

参数调试：

frame：应该修改为和建图frame一致

search_radius：近似求解点的法向量的搜索半径

deviation：在该点被认为是障碍之前，点的法向量与地平面的法向量的允许偏差（以弧度表示）:越小图越密集

buffer：在相应单元被视为障碍之前必须注册为超过允许偏差量的点数（修改搜索半径通常更有效）：越大噪点越少

loop_rate：节点运行的频率（HZ）

cell_resolution:输出占据栅格地图的分辨率（m/cell）

只有在检索到参数服务器的参数值时才使用参数服务器的参数，如果服务器没有某个参数则使用程序的默认参数。

在点云地图有明显的上下边界限制（屋顶和地面都是平面的室内）修改buffer效果不错，可以通过修改deviation改变生成的地图的密度。

动态参数调整界面：

 

 

二、初始化栅格地图　initGrid

grid->header.seq = 1;

grid->header.frame_id = param.frame; //参考frame

grid->info.origin.position.z = 0;

grid->info.origin.orientation.w = 1;

grid->info.origin.orientation.x = 0;

grid->info.origin.orientation.y = 0;

grid->info.origin.orientation.z = 0;

OccupancyGrid具体结构在这里。

三、计算时间

四、Populate map with cost values

挨个对比点云中每个点的法向量和地面法向量的偏差deviation，当deviation大于某个值将此点作为障碍。代码如下：

  double deviation = param.deviation;
  for (size_t i = 0; i < currentPC->size(); i++) //遍历每个点
  {
    double x = currentPC->points[i].x;
    double y = currentPC->points[i].y;
    double z = cloud_normals->points[i].normal_z;
    double phi = acos(fabs(z));  //值域　０－－ｐｈｉ
    int xCell, yCell;
    xCell = (int) ((x - xMin) / cellResolution);//取整后默认按照cellReso将点分配到cell
    yCell = (int) ((y - yMin) / cellResolution);      
    if (phi > deviation) 
      countGrid[yCell * xCells + xCell]++;  //统计一个ｃｅｌｌ中垂直方向满足条件的点数

z = cloud_normals->points[i].normal_z，根据pcl::Normal的定义，当法向量是垂直向上的时候，points[i].normal_z的值是１,acos是０，为函数的最小值。根据acos函数的递减性质，非地面点的值应该都比地面点大。可以设置deviation值，决定障碍物点的阈值。然后统计一个ｃｅｌｌ中垂直方向满足条件的点数，用做下一步生成Occupancy Grid。

// ---------------------------------
// -----Generate Occupancy Grid-----
// ---------------------------------
void genOccupancyGrid(std::vector<signed char> &ocGrid, std::vector<int> &countGrid, int size) 
{
  int buf = param.buffer;
  for (int i = 0; i < size; i++)  //size:xCells * yCells
  {
    if (countGrid[i] < buf ) 
      ocGrid[i] = 0;
    else if (countGrid[i] > buf) 
      ocGrid[i] = 100;
    else if (countGrid[i] == 0) 
      ocGrid[i] = 0; // TODO Should be -1
   /* if (countGrid[i] < buf && countGrid[i]>0) 
      ocGrid[i] = 0;
    else if (countGrid[i] > buf) 
      ocGrid[i] = 100;
    else if (countGrid[i] == 0) 
      ocGrid[i] = -1; // TODO Should be -1  */    
  }
}
// -----------------------------------
// -----Update Occupancy Grid Msg-----
// -----------------------------------
void updateGrid(nav_msgs::OccupancyGridPtr grid, double cellRes, int xCells, int yCells,
                               double originX, double originY, std::vector<signed char> *ocGrid) 
{
  grid->header.seq++;
  grid->header.stamp.sec = ros::Time::now().sec;
  grid->header.stamp.nsec = ros::Time::now().nsec;
  grid->info.map_load_time = ros::Time::now();
  grid->info.resolution = cellRes;
  grid->info.width = xCells;
  grid->info.height = yCells;
  grid->info.origin.position.x = originX;  //minx
  grid->info.origin.position.y = originY;  //miny
  grid->data = *ocGrid;
}