rviz中显示的点云与网格垂直，将保存的pcd文件转为点云在rviz中显示，并使用octomap_server将点云地图转化为八叉树地图和占据栅格地图

问题：点云与网格垂直

用lego-loam建图时用rosbag录制相关点云的话题，建图结束后用rosbag play将.bag包在rviz中显示，但是由于该话题的点云发布的frame_id=/camera_init，但是rviz中默认的坐标系是base_link，并且camera_init与base_link有旋转关系，因此导致点云在rviz中显示时与rviz网格线呈垂直关系，虽然rviz可以将默认显示的xy平面改成xz平面，让点云显示正常，但是此时不能在水平状态下左右旋转点云地图。

背景

ubuntu18.04+melodic
lego-loam订阅话题/laser_cloud_surround后保存了四个.pcd文件：

pcd保存的路径在utility.h文件中设置

解决方法：对点云坐标做变换，绕x轴旋转90度，将z轴指向上方

从PCD创建PointCloud2点云，然后再在rviz中显示

将pcd转成点云，在RVIZ中显示点云图

创建pcl_xy2xz.cpp文件：

#include  
#include
#include  
#include  
#include
#include
      
int main (int argc, char **argv)
{  
    ros::init (argc, argv, "lego_loam");  
    ros::NodeHandle nh;  
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("/lego_loam_with_c16/output", 10);  //待订阅的点云话题
    
    pcl::PointCloud<pcl::PointXYZ> cloud1,cloud2;  
    sensor_msgs::PointCloud2 output;  
    pcl::io::loadPCDFile ("/home/gyl/wheeltec_bag/lego-loam pcl/nosmog1/finalCloud.pcd", cloud1);	//自己的pcd路径

    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

    //绕x轴旋转一个theta角
    transform_2.rotate(Eigen::AngleAxisf(1.570795, Eigen::Vector3f::UnitX()));

    //执行变换
    //pcl::PointCloud::Ptr pPointCloudOut(new pcl::PointCloud());
    pcl::transformPointCloud(cloud1, cloud2, transform_2);

    pcl::toROSMsg(cloud2,output);// 转换成ROS下的数据类型 最终通过topic发布

    output.header.stamp=ros::Time::now();
    output.header.frame_id  ="/camera_init_xz";	//点云所在的坐标系，frame_id

    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
37
38
39
40

catkin_make编译，结果报错：

CMakeFiles/urdf01_rviz_node.dir/src/pcl_xy2xz.cpp.o：在函数‘void pcl::createMapping<pcl::PointXYZ>(std::vector<pcl::PCLPointField, std::allocator<pcl::PCLPointField> > const&, std::vector<pcl::detail::FieldMapping, std::allocator<pcl::detail::FieldMapping> >&)’中：
/usr/include/pcl-1.8/pcl/conversions.h:108：对‘pcl::console::print(pcl::console::VERBOSITY_LEVEL, char const*, ...)’未定义的引用
/usr/include/pcl-1.8/pcl/conversions.h:108：对‘pcl::console::print(pcl::console::VERBOSITY_LEVEL, char const*, ...)’未定义的引用
/usr/include/pcl-1.8/pcl/conversions.h:108：对‘pcl::console::print(pcl::console::VERBOSITY_LEVEL, char const*, ...)’未定义的引用
CMakeFiles/urdf01_rviz_node.dir/src/pcl_xy2xz.cpp.o：在函数‘main’中：
/usr/include/pcl-1.8/pcl/io/pcd_io.h:56：对‘vtable for pcl::PCDReader’未定义的引用
/usr/include/pcl-1.8/pcl/io/pcd_io.h:208：对‘pcl::PCDReader::read(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, pcl::PCLPointCloud2&, Eigen::Matrix<float, 4, 1, 0, 4, 1>&, Eigen::Quaternion<float, 0>&, int&, int)’未定义的引用

collect2: error: ld returned 1 exit status
urdf01_rviz/CMakeFiles/urdf01_rviz_node.dir/build.make:127: recipe for target '/home/gyl/hello_w/devel/lib/urdf01_rviz/urdf01_rviz_node' failed
make[2]: *** [/home/gyl/hello_w/devel/lib/urdf01_rviz/urdf01_rviz_node] Error 1
CMakeFiles/Makefile2:5439: recipe for target 'urdf01_rviz/CMakeFiles/urdf01_rviz_node.dir/all' failed
make[1]: *** [urdf01_rviz/CMakeFiles/urdf01_rviz_node.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8 -l8" failed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

原因就是CMakeLists.txt中没有配置好相关信息：

......

find_package(catkin REQUIRED COMPONENTS
  pcl_ros
  pcl_conversions
)
find_package(PCL REQUIRED QUIET)

catkin_package(
  DEPENDS PCL
)

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

link_directories(
	include
	${PCL_LIBRARY_DIRS}
)

add_executable(自己的项目名称 src/pcl_xy2xz.cpp)
target_link_libraries(自己的项目名称 ${catkin_LIBRARIES} ${PCL_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

重新catkin_make编译就成功了。

创建launch

<launch>
	<!-- 运行创建的pcl_xy2xz.cpp文件 -->
    <node pkg="urdf01_rviz" type="pcl_xy2xz" name="pcl_xy2xz" output="screen" respawn="false"/>
	
	<!-- 使用octomap_server将点云地图转化为八叉树地图和占据栅格地图 -->
    <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
        <!-- resolution in meters per pixel -->
        <param name="resolution" value="0.1" />
        <!-- name of the fixed frame, needs to be "/map" for SLAM -->
        <param name="frame_id" type="string" value="/camera_init_xz" />
        <!-- max range / depth resolution of the kinect in meter -->
        <param name="sensor_model/max_range" value="50.0" />
        <param name="latch" value="true" />
        <!-- max/min height for occupancy map, should be in meters -->
        <param name="pointcloud_max_z" value="1000" />
        <param name="pointcloud_min_z" value="-1000" />
        <param name="ground_filter_angle" value="3.14" />
        <!-- topic from where pointcloud2 messages are subscribed -->
        <remap from="cloud_in" to="/lego_loam_with_c16/output" />
    </node>
	
	<!-- 启动rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />

</launch>

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

rviz显示

点击add 按钮添加 “PointCloud2模块”
设置topic为 “/lego_loam_with_c16/output”
设置FixedFram为 “camera_init_xz”

点云显示：

八叉树地图显示：

参考博客：

1. 【激光SLAM】Lego_loam使用教程
2. Octomap 在ROS环境下实时显示
3. 使用octomap_server将点云地图转化为八叉树地图和占据栅格地图