本文主要是介绍北达科他大学( 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)); //值域 0--phiint xCell, yCell;xCell = (int) ((x - xMin) / cellResolution);//取整后默认按照cellReso将点分配到cellyCell = (int) ((y - yMin) / cellResolution); if (phi > deviation) countGrid[yCell * xCells + xCell]++; //统计一个cell中垂直方向满足条件的点数
z = cloud_normals->points[i].normal_z,根据pcl::Normal的定义,当法向量是垂直向上的时候,points[i].normal_z的值是1,acos是0,为函数的最小值。根据acos函数的递减性质,非地面点的值应该都比地面点大。可以设置deviation值,决定障碍物点的阈值。然后统计一个cell中垂直方向满足条件的点数,用做下一步生成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; //minxgrid->info.origin.position.y = originY; //minygrid->data = *ocGrid;
}
这篇关于北达科他大学( North Dakota State University)cloud_to_map学习的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!