本文主要是介绍3D激光SLAM点云地图pcd转导航可用的2D栅格地图，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
本文旨在帮助读者将激光点云地图转为2D栅格地图，以便完成路径规划与导航。本方法将pcd转为pgm的原理是将接收到的点云信息以"/map"话题的形式发布，用map_server来接收"/map"话题，保存2D栅格地图！
废话不多说，直接开始！
一、安装pcd2pgm
#创建工作空间
mkdir -p ~/pcd2pgm_ws/src
cd ~/pcd2pgm_ws/src
catkin_init_workspace#克隆代码
git clone https://github.com/hujiax380/pcd2pgm.git————————————————————————————————
针对评论区各位的问题，修改了部分内容，用下面代码覆盖test.cpp
需要修改pcd文件的路径以及名称
#include <ros/ros.h>#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetMap.h>#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>#include <pcl/point_types.h>std::string file_directory;
std::string file_name;
std::string pcd_file;std::string map_topic_name;const std::string pcd_format = ".pcd";nav_msgs::OccupancyGrid map_topic_msg;double map_resolution = 0.05;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_Radius(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_cloud(new pcl::PointCloud<pcl::PointXYZ>);void SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, nav_msgs::OccupancyGrid& msg);int main(int argc, char** argv)
{ros::init(argc, argv, "pcl_filters");ros::NodeHandle nh;ros::NodeHandle private_nh("~");ros::Rate loop_rate(1.0);private_nh.param("file_directory", file_directory, std::string("/home/ubuntu/"));//此处需要修改为自己pcd文件的路径ROS_INFO("*** file_directory = %s ***\n", file_directory.c_str());private_nh.param("file_name", file_name, std::string("pcd_name"));//此处"pcd_name"需要修改为自己的pcd文件名，无需.pcdROS_INFO("*** file_name = %s ***\n", file_name.c_str());pcd_file = file_directory + file_name + pcd_format;ROS_INFO("*** pcd_file = %s ***\n", pcd_file.c_str());private_nh.param("map_resolution", map_resolution, 0.05);private_nh.param("map_topic_name", map_topic_name, std::string("map"));ros::Publisher map_topic_pub = nh.advertise<nav_msgs::OccupancyGrid>(map_topic_name, 1);if (pcl::io::loadPCDFile<pcl::PointXYZ> (pcd_file, *pcd_cloud) == -1){PCL_ERROR ("Couldn't read file: %s \n", pcd_file.c_str());return (-1);}std::cout << "输入点云点数：" << pcd_cloud->points.size() << std::endl;SetMapTopicMsg(pcd_cloud, map_topic_msg);while(ros::ok()){map_topic_pub.publish(map_topic_msg);loop_rate.sleep();ros::spinOnce();}return 0;
}void SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, nav_msgs::OccupancyGrid& msg)
{msg.header.seq = 0;msg.header.stamp = ros::Time::now();msg.header.frame_id = "map";msg.info.map_load_time = ros::Time::now();msg.info.resolution = map_resolution;double x_min, x_max, y_min, y_max; //这里是投影到xy平面，如果要投到xz/yz，这里以及后面的xy对应的数据改为你想投影的平面if(cloud->points.empty()){ROS_WARN("pcd is empty!\n");return;}for(int i = 0; i < cloud->points.size() - 1; i++){if(i == 0){x_min = x_max = cloud->points[i].x;y_min = y_max = cloud->points[i].y;}double x = cloud->points[i].x;double y = cloud->points[i].y;if(x < x_min) x_min = x;if(x > x_max) x_max = x;if(y < y_min) y_min = y;if(y > y_max) y_max = y;}msg.info.origin.position.x = x_min;msg.info.origin.position.y = y_min;msg.info.origin.position.z = 0.0;msg.info.origin.orientation.x = 0.0;msg.info.origin.orientation.y = 0.0;msg.info.origin.orientation.z = 0.0;msg.info.origin.orientation.w = 1.0;msg.info.width = int((x_max - x_min) / map_resolution);msg.info.height = int((y_max - y_min) / map_resolution);msg.data.resize(msg.info.width * msg.info.height);msg.data.assign(msg.info.width * msg.info.height, 0);ROS_INFO("data size = %d\n", msg.data.size());for(int iter = 0; iter < cloud->points.size(); iter++){int i = int((cloud->points[iter].x - x_min) / map_resolution);if(i < 0 || i >= msg.info.width) continue;int j = int((cloud->points[iter].y - y_min) / map_resolution);if(j < 0 || j >= msg.info.height - 1) continue;msg.data[i + j * msg.info.width] = 100;}
}修改之后完成编译：
cd ~/pcd2pgm_ws
catkin_make二、转换地图
编译成功后：
roscore#另起终端
rosrun pcd2pgm pcd2topic终端显示pcd文件信息：
另起终端启动map_server
#另起终端，保存地图
rosrun map_server map_saver如此就生成可用作2D导航的yaml与pgm文件！
本文主要参考：pcd转pgm/3d点云转2d灰度图
这篇关于3D激光SLAM点云地图pcd转导航可用的2D栅格地图的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
