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:
The octree map shows:
Reference blog:
- [Laser SLAM] Lego_loam usage tutorial
- Octomap real-time display in ROS environment
- Use octomap_server to convert point cloud maps into octree maps and occupied raster maps