The point cloud displayed in rviz is perpendicular to the grid. Convert the saved pcd file to a point cloud and display it in rviz, and use octomap_server to convert the point cloud map into an octree map and an occupied raster map.

Article directory

      • Problem: Point cloud is perpendicular to grid
      • background
      • Solution: Transform the point cloud coordinates, rotate 90 degrees around the x-axis, and point the z-axis upward.
        • Convert pcd to point cloud and display point cloud image in RVIZ
        • Create launch
      • rviz display

Problem: Point cloud is perpendicular to the grid

When using lego-loam to build a map, use rosbag to record the topic of the relevant point cloud. After the map is built, use rosbag play to display the .bag package in rviz. However, because the point cloud of the topic is released with frame_id=/camera_init, it is the default in rviz. The coordinate system is base_link, and camera_init has a rotation relationship with base_link, which causes the point cloud to be perpendicular to the rviz grid line when displayed in rviz, although rviz can change the default displayed xy plane to the xz plane to allow the point cloud to be displayed Normal, but the point cloud map cannot be rotated left or right in the horizontal state at this time.

Background

ubuntu18.04 + melodic
lego-loam saved four .pcd files after subscribing to the topic/laser_cloud_surround:


The path saved by pcd is set in the utility.h file

Solution: Transform the point cloud coordinates, rotate 90 degrees around the x-axis, and point the z-axis upward

Create PointCloud2 point cloud from PCD before displaying it in rviz

Convert pcd to point cloud and display point cloud image in RVIZ

Create pcl_xy2xz.cpp file:

#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); //Point cloud topic to be subscribed
    
    pcl::PointCloud<pcl::PointXYZ> cloud1,cloud2;
    sensor_msgs::PointCloud2 output;
    pcl::io::loadPCDFile ("/home/gyl/wheeltec_bag/lego-loam pcl/nosmog1/finalCloud.pcd", cloud1); //own pcd path

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

    //Rotate around the x-axis by a theta angle
    transform_2.rotate(Eigen::AngleAxisf(1.570795, Eigen::Vector3f::UnitX()));

    //Perform transformation
    //pcl::PointCloud<pcl::PointXYZ>::Ptr pPointCloudOut(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::transformPointCloud(cloud1, cloud2, transform_2);

    pcl::toROSMsg(cloud2,output);//Convert to data type under ROS and finally publish through topic

    output.header.stamp=ros::Time::now();
    output.header.frame_id ="/camera_init_xz"; //The coordinate system where the point cloud is located, frame_id

    ros::Rate loop_rate(1);
    while (ros::ok())
    {<!-- -->
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

catkin_make compiles, and the result is an error:

CMakeFiles/urdf01_rviz_node.dir/src/pcl_xy2xz.cpp.o: In function 'void pcl::createMapping<pcl::PointXYZ>(std::vector<pcl::PCLPointField, std::allocator<pcl:: PCLPointField> > const & amp;, std::vector<pcl::detail::FieldMapping, std::allocator<pcl::detail::FieldMapping> > & amp;)':
/usr/include/pcl-1.8/pcl/conversions.h:108: undefined reference to 'pcl::console::print(pcl::console::VERBOSITY_LEVEL, char const*, ...)'
/usr/include/pcl-1.8/pcl/conversions.h:108: undefined reference to 'pcl::console::print(pcl::console::VERBOSITY_LEVEL, char const*, ...)'
/usr/include/pcl-1.8/pcl/conversions.h:108: undefined reference to 'pcl::console::print(pcl::console::VERBOSITY_LEVEL, char const*, ...)'
CMakeFiles/urdf01_rviz_node.dir/src/pcl_xy2xz.cpp.o: In function 'main':
/usr/include/pcl-1.8/pcl/io/pcd_io.h:56: undefined reference to vtable for pcl::PCDReader’
/usr/include/pcl-1.8/pcl/io/pcd_io.h:208: for 'pcl::PCDReader::read(std::__cxx11::basic_string<char, std::char_traits<char>, std:: allocator<char> > const & amp;, pcl::PCLPointCloud2 & amp;, Eigen::Matrix<float, 4, 1, 0, 4, 1> & amp;, Eigen::Quaternion<float, 0> & amp ;, int & amp;, int)' undefined reference

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

The reason is that the relevant information is not configured in 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(own project name src/pcl_xy2xz.cpp)
target_link_libraries(own project name ${catkin_LIBRARIES} ${PCL_LIBRARIES})

Re-compile catkin_make and it will be successful.

Create launch
<launch>
<!-- Run the created pcl_xy2xz.cpp file -->
    <node pkg="urdf01_rviz" type="pcl_xy2xz" name="pcl_xy2xz" output="screen" respawn="false"/>
\t
<!-- Use octomap_server to convert point cloud maps into octree maps and occupied raster maps -->
    <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>
\t
<!-- Start rviz -->
    <node pkg="rviz" type="rviz" name="rviz" />

</launch>

rviz display

Click the add button to add “PointCloud2 module”
Set the topic to “/lego_loam_with_c16/output”
Set FixedFram to “camera_init_xz”

Point cloud display:

Please add a picture descriptionPlease add a picture description
The octree map shows:

Please add a picture descriptionPlease add a picture description

Reference blog:

  1. [Laser SLAM] Lego_loam usage tutorial
  2. Octomap real-time display in ROS environment
  3. Use octomap_server to convert point cloud maps into octree maps and occupied raster maps