通過SLAM或其他方式構建的點雲地圖是無法直接用於導航的，我知道的解決方案有三種：
一、將點雲地圖二維投影，轉換為可用於導航的二維柵格地圖；
二、將點雲轉換為Octomap八叉樹地圖，即可使用導航算法，比如RRT*進行三維導航；
三、將實時點雲數據轉換為實時激光數據，這樣就可以愉快的使用ROS的move_base和acml包了。
此博客為第一種方案的實現案例
構建點雲地圖
構建點雲地圖需要深度圖和對應的位姿，這里使用高翔的<視覺SLAM14講>的深度圖和位姿。這里構建的是一個ROS功能包，代碼如下：
#include <stdio.h>
#include <iostream>
#include <algorithm>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
//#include <pcl/visualization/pcl_visualizer.h>
//#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>
// 定義點雲使用的格式：這里用的是XYZRGB
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
using namespace std;
void PCLTest();
ros::Publisher pub_pcs;
ros::Publisher pub_pcr;
int main(int argc, char** argv)
{
ros::init(argc, argv, "pcl_test_node");//初始化節點
ros::start();//啟動節點
ros::NodeHandle nh;
pub_pcs=nh.advertise<sensor_msgs::PointCloud2>("/point_cloud",1);
pub_pcr=nh.advertise<PointCloud>("/point_cloud_raw",1);
ROS_INFO_STREAM("Initing");
PCLTest();
ROS_INFO_STREAM("pcl_test節點結束");
return 0;
}
void PCLTest()
{
vector<cv::Mat> colorImgs, depthImgs; // 彩色圖和深度圖
vector<Eigen::Isometry3d> poses; // 相機位姿
ifstream fin("./data/pose.txt");
if (!fin)
{
cerr<<"cannot find pose file"<<endl;
return;
}
for ( int i=0; i<5; i++ )
{
boost::format fmt( "./data/%s/%d.%s" ); //圖像文件格式
colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1讀取原始圖像
double data[7] = {0};
for ( int i=0; i<7; i++ )
{
fin>>data[i];
}
Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
Eigen::Isometry3d T(q);
T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
poses.push_back( T );
}
// 計算點雲並拼接
// 相機內參
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.0;
cout<<"正在將圖像轉換為點雲..."<<endl;
// 新建一個點雲
PointCloud::Ptr pointCloud( new PointCloud );
//pcl::visualization::CloudViewer viewer("pcd viewer");
for ( int i=0; i<5; i++ )
{
PointCloud::Ptr current( new PointCloud );
cout<<"轉換圖像中: "<<i+1<<endl;
cv::Mat color = colorImgs[i];
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d T = poses[i];
for ( int v=0; v<color.rows; v++ )
for ( int u=0; u<color.cols; u++ )
{
unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
if ( d==0 ) continue; // 為0表示沒有測量到
if ( d >= 3500 ) continue; // 深度太大時不穩定，去掉
Eigen::Vector3d point;
point[2] = double(d)/depthScale;
point[0] = (u-cx)*point[2]/fx;
point[1] = (v-cy)*point[2]/fy;
Eigen::Vector3d pointWorld = T*point;
PointT p ;
p.x = pointWorld[0];
p.y = pointWorld[1];
p.z = pointWorld[2];
p.b = color.data[ v*color.step+u*color.channels() ];
p.g = color.data[ v*color.step+u*color.channels()+1 ];
p.r = color.data[ v*color.step+u*color.channels()+2 ];
current->points.push_back( p );
}
//利用統計濾波器方法去除孤立點。
PointCloud::Ptr tmp ( new PointCloud );
pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
statistical_filter.setMeanK(50);
statistical_filter.setStddevMulThresh(1.0);
statistical_filter.setInputCloud(current);
statistical_filter.filter( *tmp );
(*pointCloud) += *tmp;
//viewer.showCloud(pointCloud);
//getchar();
}
getchar();
pointCloud->is_dense = false;
cout<<"點雲共有"<<pointCloud->size()<<"個點."<<endl;
//體素濾波器（Voxel Filter）進行降采樣
pcl::VoxelGrid<PointT> voxel_filter;
voxel_filter.setLeafSize( 0.01, 0.01, 0.01 ); //分辨率
PointCloud::Ptr tmp ( new PointCloud );
voxel_filter.setInputCloud( pointCloud );
voxel_filter.filter( *tmp );
tmp->swap(*pointCloud);
cout<<"濾波之后，點雲共有"<<pointCloud->size()<<"個點."<<endl;
sensor_msgs::PointCloud2 pmsg;
pcl::toROSMsg(*pointCloud, pmsg);
pmsg.header.frame_id = "map";
pub_pcs.publish(pmsg);
pub_pcr.publish(*pointCloud);
getchar();
//while (!viewer.wasStopped ())
//{
//}
pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
}
PCLTest函數是構建點雲的函數，首先從pose.txt中讀取位姿，然后讀取rgb圖像和深度圖像，接着將像素坐標轉換為世界坐標，在存入點雲中，每張圖片構建的點雲都進行統計濾波。構建完成點雲之后，進行體素濾波。最后發布點雲。這里發布兩個消息：sensor_msgs::PointCloud2 和 PointCloud，其中sensor_msgs::PointCloud2類型的點雲用於rviz點雲可視化，而PointCloud則用於給cloud_to_map包轉換為二維地圖。
需要注意的是，若想直接發布點雲類型 pcl::PointCloud
#include <pcl_ros/point_cloud.h>。若想將pcl::PointCloud
#include <pcl_conversions/pcl_conversions.h>。
程序的最后將點雲地圖保存為map.pcd。我們可以使用pcl_viewer查看：
點雲地圖的二維投影
這里通過cloud_to_map功能包將點雲地圖轉換二維地圖。cloud_to_map好象是北達科他大學( North Dakota State University)，更具體的信息我不知道了。下載地址為：PointCloud-to-grid-map上下載的，但是直接使用貌似有點問題，而且這個程序為了動態調參，寫的太復雜了，我將其簡化了。其主程序如下：
#include <iostream>
#include <fstream>
#include <stdint.h>
#include <math.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <nav_msgs/OccupancyGrid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/console/parse.h>
#include <pcl/filters/passthrough.h>
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef pcl::PointCloud<pcl::Normal> NormalCloud;
using namespace std;
/* 全局變量 */
PointCloud::ConstPtr currentPC;
bool newPointCloud = false;
double cellResolution=0.05;
double deviation = 0.785398;
int buffer_size=50;//每個柵格法線垂直的閾值
ros::Publisher pub;
void calcSize(double *xMax, double *yMax, double *xMin, double *yMin)
{
pcl::PointXYZRGB minPt, maxPt;
pcl::getMinMax3D(*currentPC, minPt, maxPt);
*xMax=maxPt.x;
*yMax=maxPt.y;
*xMin=minPt.x;
*yMin=minPt.y;
}
//得到柵格地圖
void computeGrid( std::vector<signed char> &ocGrid, double xMin, double yMin,int xCells, int yCells)
{
cout<<"開始計算法線"<<endl;
//計算點雲的法線
NormalCloud::Ptr cloud_normals(new NormalCloud);
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setInputCloud(currentPC);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
ne.setSearchMethod(tree);
ne.setRadiusSearch(0.06);
ne.compute(*cloud_normals);
cout<<"判斷法線是否垂直"<<endl;
int size=xCells*yCells;
std::vector<int> countGrid(size);
//判斷每個點雲的法向量是否垂直
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;
int xc = (int) ((x - xMin) / cellResolution); //取整后默認按照cellResolution將點分配到ｃｅｌｌ
int yc = (int) ((y - yMin) / cellResolution);
/*
法線是單位向量，z是發現的z值，z越接近於1,表明法線越垂直。
而acos(z)使得z值越接近於1,結果越小.
即acos(z)的結果越大，z越不垂直
*/
double normal_value = acos(fabs(z));//值域 ０－－ｐｈｉ 地面fabs(z)應該是１ acos是０，最小值
if (normal_value > deviation) //根據acos函數的遞減性質，非地面點的值應該都比地面點大。可以設置deviation值，決定障礙物點的閾值
countGrid[yc * xCells + xc]++; //統計一個ｃｅｌｌ中垂直方向滿足條件的點數
}
cout<<"計算占據概率"<<endl;
//根據閾值計算占據概率
for (int i = 0; i < size; i++) //size:xCells * yCells
{
if (countGrid[i] < buffer_size && countGrid[i]>0)
ocGrid[i] = 0;
else if (countGrid[i] > buffer_size)
ocGrid[i] = 100;
else if (countGrid[i] == 0)
ocGrid[i] = 0; // TODO Should be -1
}
}
void updateGrid(nav_msgs::OccupancyGridPtr grid, int xCells, int yCells,
double originX, double originY, std::vector<signed char> *ocGrid)
{
static int seq=0;
grid->header.frame_id = "map";
grid->header.seq=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 = cellResolution;
grid->info.width = xCells;
grid->info.height = yCells;
grid->info.origin.position.x = originX; //minx
grid->info.origin.position.y = originY; //miny
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;
grid->data = *ocGrid;
}
void callback(const PointCloud::ConstPtr& msg)
{
currentPC=msg;
ROS_INFO_STREAM("Convertor節點——接收到點雲");
/*計算點雲的最大和最小值*/
double xMax = 0, yMax = 0, xMin = 0, yMin = 0;
calcSize(&xMax, &yMax, &xMin, &yMin);
cout<<"極值："<<xMax<<" "<<yMax<<" "<<xMin<<" "<<yMin<<" "<<endl;
/* 確定柵格地圖的長和寬 */
int xCells = ((int) ((xMax - xMin) / cellResolution)) + 1;
int yCells = ((int) ((yMax - yMin) / cellResolution)) + 1;
cout<<"地圖大小："<<xCells<<" "<<yCells<<endl;
/*計算柵格地圖*/
std::vector<signed char> ocGrid(yCells * xCells); //存儲每個ｃｅｌｌ的值 ０或者１００
computeGrid(ocGrid, xMin, yMin, xCells, yCells);
cout<<"成功計算得到柵格地圖"<<endl;
//發布地圖消息
nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid);
updateGrid(grid, xCells, yCells, xMin, yMin, &ocGrid);
pub.publish(grid);
ROS_INFO_STREAM("Convertor節點——發布柵格地圖");
}
int main(int argc, char** argv)
{
setlocale(LC_ALL, "");
ros::init(argc, argv, "convertor_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<PointCloud>("/point_cloud/raw", 1, callback);
pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 1);
//構造占據網格消息
nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid);
grid->header.seq = 1;
grid->header.frame_id = "map";//父坐標系
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;
ROS_INFO_STREAM("Convertor節點初始化完成");
ros::Rate loop_rate(0.2);
ros::Duration t(10);
while (ros::ok())
{
ros::spinOnce();
t.sleep();
}
}
cloud_to_map訂閱點雲：/point_cloud_raw，發布的柵格地圖：/map。而且dynamic_reconfigure功能包動態配置參數。
在rviz中顯示柵格地圖：
這個程序通過判斷法向量的方法獲得地圖，但我覺得這種方法計算量大，而是感覺效果一半。我這里提出一種更簡單的方法，直接將z軸0-0.5范圍內的點雲投影，得到柵格地圖，程序如下：
#include <iostream>
#include <fstream>
#include <stdint.h>
#include <math.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <nav_msgs/OccupancyGrid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/console/parse.h>
#include <dynamic_reconfigure/server.h>
#include <cloud_to_map/cloud_to_map_nodeConfig.h> //autogenerated for the cloud_to_map package by the dynamic_reconfigure package.
/* Define the two point cloud types used in this code */
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef pcl::PointCloud<pcl::Normal> NormalCloud;
/* Global */
PointCloud::ConstPtr currentPC;
bool newPointCloud = false;
bool reconfig = false;
// -----------------------------------------------
// -----Define a structure to hold parameters-----
// -----------------------------------------------
struct Param
{
std::string frame;
double searchRadius;
double deviation;
int buffer;
double loopRate;
double cellResolution;
};
// -----------------------------------
// -----Define default parameters-----
//If the value cannot be retrieved from the server, default_val is used instead.
// -----------------------------------
Param param;
boost::mutex mutex; //多線程
void loadDefaults(ros::NodeHandle& nh)
{
nh.param<std::string>("frame", param.frame, "world");
nh.param("search_radius", param.searchRadius, 0.05);
nh.param("deviation", param.deviation, 0.78539816339);
nh.param("buffer", param.buffer, 5);
nh.param("loop_rate", param.loopRate, 10.0);
nh.param("cell_resolution", param.cellResolution, 0.05);
}
// ------------------------------------------
// -----Callback for Dynamic Reconfigure-----調用動態參數配置
// ------------------------------------------
void callbackReconfig(cloud_to_map::cloud_to_map_nodeConfig &config, uint32_t level)
{
boost::unique_lock<boost::mutex>(mutex);
param.frame = config.frame.c_str();
param.searchRadius = config.search_radius;
param.deviation = config.deviation;
param.buffer = config.buffer;
param.loopRate = config.loop_rate;
//param.cellResolution = config.cell_resolution;
param.cellResolution=0.05;
reconfig = true;
ROS_INFO("\n\nReconfigure Request: \nframe:%s \ndeviation:%f \nloop_rate:%f \ncell_resolution:%f \nsearch_radius:%f",
param.frame.c_str(), param.deviation,param.loopRate,param.cellResolution, param.searchRadius);
}
// ------------------------------------------------------
// -----Update current PointCloud if msg is received-----
// ------------------------------------------------------
void callback(const PointCloud::ConstPtr& msg)
{
boost::unique_lock<boost::mutex>(mutex); //unique_lock是write lock。被鎖后不允許其他線程執行被shared_lock或unique_lock的代碼。
//在寫操作時，一般用這個，可以同時限制unique_lock的寫和share_lock的讀。
//ROS_INFO("callback is finshed \n");
currentPC = msg;
newPointCloud = true;
}
// ----------------------------------------------------------------
// -----Calculate surface normals with a search radius of 0.05-----
// ----------------------------------------------------------------
void calcSurfaceNormals(PointCloud::ConstPtr& cloud, pcl::PointCloud<pcl::Normal>::Ptr normals)
{
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setInputCloud(cloud);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
ne.setSearchMethod(tree);
ne.setRadiusSearch(param.searchRadius);
ne.compute(*normals);
}
// ---------------------------------------
// -----Initialize Occupancy Grid Msg-----
// ---------------------------------------
void initGrid(nav_msgs::OccupancyGridPtr grid)
{
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;
std::cout<<"initGrid finshed!\n"<< std::endl;
/* nav_msgs/OccupancyGrid
# This represents a 2-D grid map, in which each cell represents the probability of occupancy.
Header header
#MetaData for the map
MapMetaData info
# The map data, in row-major order, starting with (0,0). Occupancy
# probabilities are in the range [0,100]. Unknown is -1.
int8[] data
Header header
uint32 seq
time stamp
string frame_id
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 */
}
// ------------------------------------------
// -----Calculate size of Occupancy Grid-----
// ------------------------------------------
void calcSize(double *xMax, double *yMax, double *xMin, double *yMin)
{
for (size_t i = 0; i < currentPC->size(); i++)
{
double x = currentPC->points[i].x;
double y = currentPC->points[i].y;
if (*xMax < x) {
*xMax = x;
}
if (*xMin > x) {
*xMin = x;
}
if (*yMax < y) {
*yMax = y;
}
if (*yMin > y) {
*yMin = y;
}
}
}
// ---------------------------------------
// -----Populate 填充 map with cost values-----
// ---------------------------------------
void populateMap(NormalCloud::Ptr cloud_normals, std::vector<int> &countGrid, double xMin, double yMin,
double cellResolution, int xCells, int yCells)
{
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;
int xCell, yCell;
//TODO implement cutoff height
xCell = (int) ((x - xMin) / cellResolution); //取整后默認按照cellResolution將點分配到ｃｅｌｌ
yCell = (int) ((y - yMin) / cellResolution);
if ((yCell * xCells + xCell) > (xCells * yCells))
std::cout << "x: " << x << ", y: " << y << ", xCell: " << xCell << ", yCell: " << yCell<< "\n";
double normal_value = acos(fabs(z));//值域 ０－－ｐｈｉ 地面fabs(z)應該是１ acos是０，最小值
if (normal_value > deviation) //根據acos函數的遞減性質，非地面點的值應該都比地面點大。可以設置deviation值，決定障礙物點的閾值
countGrid[yCell * xCells + xCell]++; //統計一個ｃｅｌｌ中垂直方向滿足條件的點數
//else
//countGrid[yCell * xCells + xCell]--;
}
}
// ---------------------------------
// -----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 && countGrid[i]>0)
ocGrid[i] = 0;
else if (countGrid[i] > buf)
ocGrid[i] = 100;
else if (countGrid[i] == 0)
ocGrid[i] = 0; // TODO Should be -1
}
//std::cout<<" genOccupancyGrid finshed!"<< std::endl;
}
// -----------------------------------
// -----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;
std::cout<<"updateGrid finshed!!"<< std::endl;
}
int main(int argc, char** argv)
{
/* Initialize ROS */
ros::init(argc, argv, "cloud_to_map_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<PointCloud>("/point_cloud_raw", 1, callback);
ros::Publisher pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 1);
/* Initialize Dynamic Reconfigure ＲＯＳ動態參數配置*/
dynamic_reconfigure::Server<cloud_to_map::cloud_to_map_nodeConfig> server;
dynamic_reconfigure::Server<cloud_to_map::cloud_to_map_nodeConfig>::CallbackType f;
f = boost::bind(&callbackReconfig, _1, _2);
//std::cout<<"*************************\n"; //test
server.setCallback(f);
/* Initialize Grid */
nav_msgs::OccupancyGridPtr grid(new nav_msgs::OccupancyGrid);
initGrid(grid);
/* Finish initializing ROS */
mutex.lock();
ros::Rate loop_rate(param.loopRate);
mutex.unlock();
/* Wait for first point cloud */
while(ros::ok() && newPointCloud == false)
{
ros::spinOnce();//ROS消息回調處理函數。它倆通常會出現在ROS的主循環中，程序需要不斷調用ros::spin() 或 ros::spinOnce()，
//兩者區別在於前者調用后不會再返回，也就是你的主程序到這兒就不往下執行了，而后者在調用后還可以繼續執行之后的程序。
//所接到的消息並不是立刻就被處理，而是必須要等到ros::spin()或ros::spinOnce()執行的時候才被調用，
loop_rate.sleep();
}
/* Begin processing point clouds */
while (ros::ok())
{
ros::spinOnce();//用后還可以繼續執行之后的程序。
boost::unique_lock<boost::mutex> lck(mutex);
if (newPointCloud || reconfig)
{
/* Update configuration status */
reconfig = false; //調用動態參數配置時 ＝true
newPointCloud = false; //在回調函數=true
/* Record start time */
uint32_t sec = ros::Time::now().sec; //秒
uint32_t nsec = ros::Time::now().nsec;//納秒
/* Calculate Surface Normals */
NormalCloud::Ptr cloud_normals(new NormalCloud);
calcSurfaceNormals(currentPC, cloud_normals);
/* Figure out size of matrix needed to store data. */
double xMax = 0, yMax = 0, xMin = 0, yMin = 0;
calcSize(&xMax, &yMax, &xMin, &yMin); //Calculate size of Occupancy Grid
//std::cout <<"size of matrix needed to store data:"<<"xMax: " << xMax << ", yMax: " << yMax << ", xMin: " << xMin << ", yMin: "<< yMin << "\n";
/* Determine resolution of grid (m/cell) */
double cellResolution = param.cellResolution;
int xCells = ((int) ((xMax - xMin) / cellResolution)) + 1;
int yCells = ((int) ((yMax - yMin) / cellResolution)) + 1;
std::cout <<"the number of cells: " <<xCells*yCells<<"\n";
/* Populate Map */
std::vector<int> countGrid(yCells * xCells);
populateMap(cloud_normals, countGrid, xMin, yMin, cellResolution, xCells, yCells);
/* Generate OccupancyGrid Data Vector */
std::vector<signed char> ocGrid(yCells * xCells); //存儲每個ｃｅｌｌ的值 ０或者１００
genOccupancyGrid(ocGrid, countGrid, xCells * yCells);
/* Update OccupancyGrid Object */
updateGrid(grid, cellResolution, xCells, yCells, xMin, yMin, &ocGrid);
/* Release lock */
lck.unlock();
/* Record end time */
uint32_t esec = ros::Time::now().sec;
uint32_t ensec = ros::Time::now().nsec;
std::cout << "Seconds: " << esec - sec << " NSeconds: " << ensec - nsec << "\n\n";
/* Publish Occupancy Grid */
pub.publish(grid);
}
loop_rate.sleep();
}
}
有了柵格地圖，就可以進行全局路徑規划了，完整的代碼可以在此處獲取；
總結
此案例其實就是一個有限有高度范圍內的點雲按照分辨率投影的案例，相較之下octomap在動態障礙物環境下優勢明顯。