ubuntu16 rviz查看pcd点云文件,并使用octomap生成二维栅格地图_ocomap 将三维点云pcd转成二维栅格地图

1、代码

1-1 pcd_to_rviz.cpp

#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl_conversions//pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <iostream>

int main(int argc, char **argv)
{
    ros::init(argc,argv,"UanBdet");
    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_output",1);
    
    pcl::PointCloud<pcl::PointXYZRGB> cloud;
    sensor_msgs::PointCloud2 output;

    pcl::io::loadPCDFile("/home/zxy/map/result.pcd",cloud);
    //读取文件绝对路径.
    //对点云坐标做变换，绕x轴旋转90度，将z轴指向上方
    //octomap_server，它的坐标系是定义z轴向上的，得到栅格地图是默认投影到xy平面
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.rotate(Eigen::AngleAxisf(M_PI/2, Eigen::Vector3f(1,0,0)));
    pcl::transformPointCloud(cloud, cloud, transform);
    	
    pcl::toROSMsg(cloud,output);
    output.header.frame_id = "odom1";   
    ros::Rate loop_rate(1);
    while(ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

1-2 CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(pcd_to_rviz)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)

catkin_package()

find_package(PCL 1.7 REQUIRED)

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

add_executable(pcd_to_rviz src/pcd_to_rviz.cpp)
target_link_libraries(pcd_to_rviz 
	${PCL_LIBRARIES}
	${catkin_LIBRARIES})
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

1-3 package.xml

  <build_depend>libpcl-all-dev</build_depend>
  <exec_depend>libpcl-all</exec_depend>
1
2

2、编译运行

2-1 编译

catkin_make -DCATKIN_WHITELIST_PACKAGES="pcd_to_rviz"
//只编译该功能包
source devel/setup.bash
1
2
3

2-2 运行pcd_to_rviz

rosrun pcd_to_rviz pcd_to_rviz

rostopic list
//点云发布话题/pcl_output
1
2
3
4

2-3 运行rviz

rviz
1

2-3-1 修改坐标系 Fixed Frame

2-3-2 Add 添加PointCloud2

2-3-3 结果图

3 octomap

3-1 安装octomap插件

sudo apt-get install ros-kinetic-octomap-ros
sudo apt-get install ros-kinetic-octomap-msgs
sudo apt-get install ros-kinetic-octomap-server

sudo apt-get install ros-kinetic-octomap-rviz-plugins
1
2
3
4
5

3-2 launch文件

octomap.launch

<launch>
    <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
	<param name="resolution" value="0.1" />
	<param name="frame_id" type="string" value="odom1" />
	<param name="sensor_model/max_range" value="1000.0" />
	<param name="latch" value="true" />
	<param name="pointcloud_max_x" value="1000.0" />
	<param name="pointcloud_min_x" value="-300.0" />
	<param name="pointcloud_max_y" value="1000.0" />
	<param name="pointcloud_min_y" value="-300.0" />
	<param name="pointcloud_max_z" value="0.5" />
	<param name="pointcloud_min_z" value="-3.0" />
	<remap from="cloud_in" to="/pcl_output" />
    </node>
</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

启动octomap

roslaunch octomap.launch
1

3-3 修改octomap的topic

4 编译问题

pcd_conversionsConfig.cmake

sudo apt-get install ros-kinetic-pcl-conversions
1

5 参考文章

××× 1 ×××
××× 2 ×××
××× 3 ×××
××× 4 ×××
××× 5 ×××