├── README.md
└── pcd2pgm
    ├── CMakeLists.txt
    ├── launch
        └── run.launch
    ├── package.xml
    └── src
        └── pcd2pgm.cpp


/README.md:
--------------------------------------------------------------------------------
1 | # pcd2pgm_package
2 | 点云pcd文件转二维栅格地图
3 | 


--------------------------------------------------------------------------------
/pcd2pgm/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(pcd2pgm)
 3 | 
 4 | ## is used, also find other catkin packages
 5 | find_package(catkin REQUIRED COMPONENTS
 6 |   nav_msgs
 7 |   pcl_ros
 8 |   roscpp
 9 |   rospy
10 |   std_msgs
11 |   geometry_msgs
12 |   sensor_msgs
13 | )
14 | 
15 | catkin_package(
16 | #  INCLUDE_DIRS include
17 | #  LIBRARIES pcd2pgm
18 | #  CATKIN_DEPENDS nav_msgs pcl roscpp rospy std_msgs
19 | #  DEPENDS system_lib
20 | )
21 | 
22 | 
23 | include_directories(
24 | # include
25 |   ${catkin_INCLUDE_DIRS}
26 | )
27 | 
28 | 
29 | find_package(PCL REQUIRED)
30 | 
31 | include_directories(
32 |   include
33 | 		${PCL_INCLUDE_DIRS})
34 | 
35 | 
36 | add_executable(pcd2pgm src/pcd2pgm.cpp)
37 | target_link_libraries(pcd2pgm ${catkin_LIBRARIES} ${PCL_LIBRARIES}   )
38 | 
39 | 
40 | 


--------------------------------------------------------------------------------
/pcd2pgm/launch/run.launch:
--------------------------------------------------------------------------------
 1 | <!-- -->
 2 | <launch>
 3 | <node pkg="pcd2pgm" name="pcd2pgm" type="pcd2pgm" output="screen">
 4 | <!-- 存放pcd文件的路径-->
 5 | <param name="file_directory" value= "/home/robot/map/" />
 6 | <!-- pcd文件名称-->
 7 | <param name="file_name" value= "map" />
 8 | <!-- 选取的范围　最小的高度-->
 9 | <param name="thre_z_min" value= "0.1" />
10 | <!-- 选取的范围　最大的高度-->
11 | <param name="thre_z_max" value= "1.5" />
12 | <!--0 选取高度范围内的，１选取高度范围外的-->
13 | <param name="flag_pass_through" value= "0" />
14 | <!-- 半径滤波的半径-->
15 | <param name="thre_radius" value= "0.5" />
16 | <!-- 半径滤波的要求点数个数-->
17 | <param name="thres_point_count" value= "10" />
18 | <!-- 存储的栅格map的分辨率-->
19 | <param name="map_resolution" value= "0.05" />
20 | <!-- 转换后发布的二维地图的topic，默认使用map即可，可使用map_server保存-->
21 | <param name="map_topic_name" value= "map" />
22 | </node>
23 | 
24 | </launch>
25 | 


--------------------------------------------------------------------------------
/pcd2pgm/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>pcd2pgm</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The pcd2pgm package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="MotherRussia@facker.com">facker</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/pcd_to_topic</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>geometry_msgs</build_depend>
53 |   <build_depend>nav_msgs</build_depend>
54 |   <build_depend>pcl_ros</build_depend>
55 |   <build_depend>roscpp</build_depend>
56 |   <build_depend>sensor_msgs</build_depend>
57 |   <build_depend>std_msgs</build_depend>
58 |   <build_export_depend>geometry_msgs</build_export_depend>
59 |   <build_export_depend>nav_msgs</build_export_depend>
60 |   <build_export_depend>pcl_ros</build_export_depend>
61 |   <build_export_depend>roscpp</build_export_depend>
62 |   <build_export_depend>sensor_msgs</build_export_depend>
63 |   <build_export_depend>std_msgs</build_export_depend>
64 |   <exec_depend>geometry_msgs</exec_depend>
65 |   <exec_depend>nav_msgs</exec_depend>
66 |   <exec_depend>pcl_ros</exec_depend>
67 |   <exec_depend>roscpp</exec_depend>
68 |   <exec_depend>sensor_msgs</exec_depend>
69 |   <exec_depend>std_msgs</exec_depend>
70 | 
71 | 
72 |   <!-- The export tag contains other, unspecified, tags -->
73 |   <export>
74 |     <!-- Other tools can request additional information be placed here -->
75 | 
76 |   </export>
77 | </package>
78 | 
79 | 


--------------------------------------------------------------------------------
/pcd2pgm/src/pcd2pgm.cpp:
--------------------------------------------------------------------------------
  1 | #include <ros/ros.h>
  2 | 
  3 | #include <nav_msgs/GetMap.h>
  4 | #include <nav_msgs/OccupancyGrid.h>
  5 | 
  6 | #include <pcl/io/pcd_io.h>
  7 | #include <pcl_conversions/pcl_conversions.h>
  8 | #include <sensor_msgs/PointCloud2.h>
  9 | 
 10 | #include <pcl/filters/conditional_removal.h>         //条件滤波器头文件
 11 | #include <pcl/filters/passthrough.h>                 //直通滤波器头文件
 12 | #include <pcl/filters/radius_outlier_removal.h>      //半径滤波器头文件
 13 | #include <pcl/filters/statistical_outlier_removal.h> //统计滤波器头文件
 14 | #include <pcl/filters/voxel_grid.h>                  //体素滤波器头文件
 15 | #include <pcl/point_types.h>
 16 | 
 17 | std::string file_directory;
 18 | std::string file_name;
 19 | std::string pcd_file;
 20 | 
 21 | std::string map_topic_name;
 22 | 
 23 | const std::string pcd_format = ".pcd";
 24 | 
 25 | nav_msgs::OccupancyGrid map_topic_msg;
 26 | //最小和最大高度
 27 | double thre_z_min = 0.3;
 28 | double thre_z_max = 2.0;
 29 | int flag_pass_through = 0;
 30 | double map_resolution = 0.05;
 31 | double thre_radius = 0.1;
 32 | //半径滤波的点数阈值
 33 | int thres_point_count = 10;
 34 | 
 35 | //直通滤波后数据指针
 36 | pcl::PointCloud<pcl::PointXYZ>::Ptr
 37 |     cloud_after_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);
 38 | //半径滤波后数据指针
 39 | pcl::PointCloud<pcl::PointXYZ>::Ptr
 40 |     cloud_after_Radius(new pcl::PointCloud<pcl::PointXYZ>);
 41 | pcl::PointCloud<pcl::PointXYZ>::Ptr
 42 |     pcd_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 43 | 
 44 | //直通滤波
 45 | void PassThroughFilter(const double &thre_low, const double &thre_high,
 46 |                        const bool &flag_in);
 47 | //半径滤波
 48 | void RadiusOutlierFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &pcd_cloud,
 49 |                          const double &radius, const int &thre_count);
 50 | //转换为栅格地图数据并发布
 51 | void SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
 52 |                     nav_msgs::OccupancyGrid &msg);
 53 | 
 54 | int main(int argc, char **argv) {
 55 |   ros::init(argc, argv, "pcl_filters");
 56 |   ros::NodeHandle nh;
 57 |   ros::NodeHandle private_nh("~");
 58 | 
 59 |   ros::Rate loop_rate(1.0);
 60 | 
 61 |   private_nh.param("file_directory", file_directory, std::string("/home/"));
 62 | 
 63 |   private_nh.param("file_name", file_name, std::string("map"));
 64 | 
 65 |   pcd_file = file_directory + file_name + pcd_format;
 66 | 
 67 |   private_nh.param("thre_z_min", thre_z_min, 0.2);
 68 |   private_nh.param("thre_z_max", thre_z_max, 2.0);
 69 |   private_nh.param("flag_pass_through", flag_pass_through, 0);
 70 |   private_nh.param("thre_radius", thre_radius, 0.5);
 71 |   private_nh.param("map_resolution", map_resolution, 0.05);
 72 |   private_nh.param("thres_point_count", thres_point_count, 10);
 73 |   private_nh.param("map_topic_name", map_topic_name, std::string("map"));
 74 | 
 75 |   ros::Publisher map_topic_pub =
 76 |       nh.advertise<nav_msgs::OccupancyGrid>(map_topic_name, 1);
 77 | 
 78 |   // 下载pcd文件
 79 |   if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_file, *pcd_cloud) == -1) {
 80 |     PCL_ERROR("Couldn't read file: %s \n", pcd_file.c_str());
 81 |     return (-1);
 82 |   }
 83 | 
 84 |   std::cout << "初始点云数据点数：" << pcd_cloud->points.size() << std::endl;
 85 |   //对数据进行直通滤波
 86 |   PassThroughFilter(thre_z_min, thre_z_max, bool(flag_pass_through));
 87 |   //对数据进行半径滤波
 88 |   RadiusOutlierFilter(cloud_after_PassThrough, thre_radius, thres_point_count);
 89 |   //转换为栅格地图数据并发布
 90 |   SetMapTopicMsg(cloud_after_Radius, map_topic_msg);
 91 |   // SetMapTopicMsg(cloud_after_PassThrough, map_topic_msg);
 92 | 
 93 |   while (ros::ok()) {
 94 |     map_topic_pub.publish(map_topic_msg);
 95 | 
 96 |     loop_rate.sleep();
 97 | 
 98 |     ros::spinOnce();
 99 |   }
100 | 
101 |   return 0;
102 | }
103 | 
104 | //直通滤波器对点云进行过滤，获取设定高度范围内的数据
105 | void PassThroughFilter(const double &thre_low, const double &thre_high,
106 |                        const bool &flag_in) {
107 |   // 创建滤波器对象
108 |   pcl::PassThrough<pcl::PointXYZ> passthrough;
109 |   //输入点云
110 |   passthrough.setInputCloud(pcd_cloud);
111 |   //设置对z轴进行操作
112 |   passthrough.setFilterFieldName("z");
113 |   //设置滤波范围
114 |   passthrough.setFilterLimits(thre_low, thre_high);
115 |   // true表示保留滤波范围外，false表示保留范围内
116 |   passthrough.setFilterLimitsNegative(flag_in);
117 |   //执行滤波并存储
118 |   passthrough.filter(*cloud_after_PassThrough);
119 |   // test 保存滤波后的点云到文件
120 |   pcl::io::savePCDFile<pcl::PointXYZ>(file_directory + "map_filter.pcd",
121 |                                       *cloud_after_PassThrough);
122 |   std::cout << "直通滤波后点云数据点数："
123 |             << cloud_after_PassThrough->points.size() << std::endl;
124 | }
125 | 
126 | //半径滤波
127 | void RadiusOutlierFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &pcd_cloud0,
128 |                          const double &radius, const int &thre_count) {
129 |   //创建滤波器
130 |   pcl::RadiusOutlierRemoval<pcl::PointXYZ> radiusoutlier;
131 |   //设置输入点云
132 |   radiusoutlier.setInputCloud(pcd_cloud0);
133 |   //设置半径,在该范围内找临近点
134 |   radiusoutlier.setRadiusSearch(radius);
135 |   //设置查询点的邻域点集数，小于该阈值的删除
136 |   radiusoutlier.setMinNeighborsInRadius(thre_count);
137 |   radiusoutlier.filter(*cloud_after_Radius);
138 |   // test 保存滤波后的点云到文件
139 |   pcl::io::savePCDFile<pcl::PointXYZ>(file_directory + "map_radius_filter.pcd",
140 |                                       *cloud_after_Radius);
141 |   std::cout << "半径滤波后点云数据点数：" << cloud_after_Radius->points.size()
142 |             << std::endl;
143 | }
144 | 
145 | //转换为栅格地图数据并发布
146 | void SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
147 |                     nav_msgs::OccupancyGrid &msg) {
148 |   msg.header.seq = 0;
149 |   msg.header.stamp = ros::Time::now();
150 |   msg.header.frame_id = "map";
151 | 
152 |   msg.info.map_load_time = ros::Time::now();
153 |   msg.info.resolution = map_resolution;
154 | 
155 |   double x_min, x_max, y_min, y_max;
156 |   double z_max_grey_rate = 0.05;
157 |   double z_min_grey_rate = 0.95;
158 |   //? ? ??
159 |   double k_line =
160 |       (z_max_grey_rate - z_min_grey_rate) / (thre_z_max - thre_z_min);
161 |   double b_line =
162 |       (thre_z_max * z_min_grey_rate - thre_z_min * z_max_grey_rate) /
163 |       (thre_z_max - thre_z_min);
164 | 
165 |   if (cloud->points.empty()) {
166 |     ROS_WARN("pcd is empty!\n");
167 |     return;
168 |   }
169 | 
170 |   for (int i = 0; i < cloud->points.size() - 1; i++) {
171 |     if (i == 0) {
172 |       x_min = x_max = cloud->points[i].x;
173 |       y_min = y_max = cloud->points[i].y;
174 |     }
175 | 
176 |     double x = cloud->points[i].x;
177 |     double y = cloud->points[i].y;
178 | 
179 |     if (x < x_min)
180 |       x_min = x;
181 |     if (x > x_max)
182 |       x_max = x;
183 | 
184 |     if (y < y_min)
185 |       y_min = y;
186 |     if (y > y_max)
187 |       y_max = y;
188 |   }
189 |   // origin的确定
190 |   msg.info.origin.position.x = x_min;
191 |   msg.info.origin.position.y = y_min;
192 |   msg.info.origin.position.z = 0.0;
193 |   msg.info.origin.orientation.x = 0.0;
194 |   msg.info.origin.orientation.y = 0.0;
195 |   msg.info.origin.orientation.z = 0.0;
196 |   msg.info.origin.orientation.w = 1.0;
197 |   //设置栅格地图大小
198 |   msg.info.width = int((x_max - x_min) / map_resolution);
199 |   msg.info.height = int((y_max - y_min) / map_resolution);
200 |   //实际地图中某点坐标为(x,y)，对应栅格地图中坐标为[x*map.info.width+y]
201 |   msg.data.resize(msg.info.width * msg.info.height);
202 |   msg.data.assign(msg.info.width * msg.info.height, 0);
203 | 
204 |   ROS_INFO("data size = %d\n", msg.data.size());
205 | 
206 |   for (int iter = 0; iter < cloud->points.size(); iter++) {
207 |     int i = int((cloud->points[iter].x - x_min) / map_resolution);
208 |     if (i < 0 || i >= msg.info.width)
209 |       continue;
210 | 
211 |     int j = int((cloud->points[iter].y - y_min) / map_resolution);
212 |     if (j < 0 || j >= msg.info.height - 1)
213 |       continue;
214 |     // 栅格地图的占有概率[0,100]，这里设置为占据
215 |     msg.data[i + j * msg.info.width] = 100;
216 |     //    msg.data[i + j * msg.info.width] = int(255 * (cloud->points[iter].z *
217 |     //    k_line + b_line)) % 255;
218 |   }
219 | }
220 | 


--------------------------------------------------------------------------------