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<ros/ros.h>  
#include<pcl/point_cloud.h>
#include<pcl_conversions/pcl_conversions.h>  
#include<sensor_msgs/PointCloud2.h>  
#include<pcl/common/transforms.h>
#include<pcl/io/pcd_io.h>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<pcl::PointXYZ>::Ptr pPointCloudOut(new pcl::PointCloud<pcl::PointXYZ>());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_idros::Rate loop_rate(1);  while (ros::ok())  {  pcl_pub.publish(output);  ros::spinOnce();  loop_rate.sleep();  }  return 0;  
} 

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

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

......find_package(catkin REQUIRED COMPONENTSpcl_rospcl_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})

重新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>

rviz显示

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

点云显示：

八叉树地图显示：

参考博客：

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