Ubuntu 22.04 - ROS2 + RS16无法正常显示点云

[yanjingang]

在Ubuntu 22.04上调试RS16的3D建图，发现ROS2无法正常显示点云，尝试使用官方的RSView工具也无法正常显示点云。
Ubuntu 22.04
VTK：9.1.0
PCL：1.12.1

排查过程：

1. 确认传感器正常

切换到Ubuntu 20.04系统，RSView、rs_driver_viewer、ROS包均可以正常显示点云。
Ubuntu 20.04
VTK：7.1.1
PCL：1.10.0

2.确认Ubuntu 22.04里数据包正常

切换到Ubuntu 22.04系统，配置网络后，tcpdump/wireshark抓包，6699/7788端口UDP消息包收发正常。但RSView依然无法显示点云，也获取不到Lidar信息。

3.确认防火墙

关闭ufw防火墙，或添加端口规则，问题依旧：
`// close ufw
sudo ufw disable
//or
sudo ufw allow 6699/udp
sudo ufw allow 7788/udp
sudo ufw reload

// check iptables
sudo iptables -L
Chain INPUT (policy ACCEPT)
target prot opt source destination
Chain FORWARD (policy ACCEPT)
target prot opt source destination
Chain OUTPUT (policy ACCEPT)
target prot opt source destination`

4.确认PCL+VTK可正常渲染点云

测试生成点云代码：
`vim pcl_test.cpp

#include
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/parse.h>

int main(int argc, char **argv)
{
std::cout << "Test PCL !!!" << std::endl;

pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
uint8_t r(255), g(15), b(15);
for (float z(-1.0); z <= 1.0; z += 0.05)
{
    for (float angle(0.0); angle <= 360.0; angle += 5.0)
    {
        pcl::PointXYZRGB point;
        point.x = 0.5 * cosf (pcl::deg2rad(angle));
        point.y = sinf (pcl::deg2rad(angle));
        point.z = z;
        uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
                        static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
        point.rgb = *reinterpret_cast<float*>(&rgb);
        point_cloud_ptr->points.push_back (point);
    }
    if (z < 0.0)
    {
        r -= 12;
        g += 12;
    }
    else
    {
        g -= 12;
        b += 12;
    }
}
point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
point_cloud_ptr->height = 1;

pcl::visualization::PCLVisualizer::Ptr RGBViewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(point_cloud_ptr); 
RGBViewer->setBackgroundColor(0,0,0);
RGBViewer->addPointCloud<pcl::PointXYZRGB> (point_cloud_ptr,rgb,"rgb cloud");
RGBViewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"rgb cloud");
RGBViewer->addCoordinateSystem(1.0);
RGBViewer->initCameraParameters();

while (!RGBViewer->wasStopped())
{
    RGBViewer->spin();
}
return 0;

}`

编译配置：
`vim CMakeLists.txt

cmake_minimum_required(VERSION 2.6)
project(pcl_test)

find_package(PCL REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable(pcl_test pcl_test.cpp)

target_link_libraries (pcl_test ${PCL_LIBRARIES})

install(TARGETS pcl_test RUNTIME DESTINATION bin)`

编译运行测试：

cd build
cmake ..
make
./pcl_test

可以正常显示点云

5.使用rs_driver无法查看点云

打开工具编译，用rs_driver_viewer查看点云

`cd ~/ros2_ws/src/rslidar_sdk/src/rs_driver
mkdir build && cd build

// build
cmake -DCOMPILE_TOOLS=ON ..
make

// run
./tool/rs_driver_viewer -type=RS16`

编译报pcl/io/io.h找不到pcl/common/io.h，按如下方式修改：
vim ~/ros2_ws/src/rs_driver/msg/pcl_point_cloud_msg.hpp // #include <pcl/io/io.h> 改为pcd_io.h #include <pcl/io/pcd_io.h>

编译后运行出core，core栈在spinOnce，查到是由于VTK9.1.0的一个bug导致，暂时改为spin规避：
`cd ~/ros2_ws/src/rslidar_sdk/src/rs_driver/build

// debug
gdb ./tool/rs_driver_viewer
(gdb) set args -type RS16
(gdb) r
(gdb) bt
#0 0x00007ffff38d8c10 in _XEventsQueued () from /lib/x86_64-linux-gnu/libX11.so.6
#1 0x00007ffff38c5291 in XPending () from /lib/x86_64-linux-gnu/libX11.so.6
#2 0x00007ffff5ac6b8f in vtkXRenderWindowInteractor::StartEventLoop() () from
/lib/x86_64-linux-gnu/libvtkRenderingUI-9.1.so.1
#3 0x00007ffff7ec1f8c in pcl::visualization::PCLVisualizer::spinOnce(int, bool) () from /lib/x86_64-linux-gnu/libpcl_visualization.so.1.12
#4 0x000055555555f333 in main ()

// fix
vim tool/rs_driver_viewer.cpp
// pcl_viewer->spinOnce(); 临时改为spin
pcl_viewer->spin();

// run
./tool/rs_driver_viewer -type=RS16`

运行后报ERRCODE_MSOPTIMEOUT，点云无法正常显示，但是tcpdump/wireshark抓包，6699/7788端口UDP消息包收发正常。
不确定是否是点云包解析过程出了问题（与Ubuntu 22.04的PCL/VTK的版本升级接口变化兼容性有关？）

感谢～！

[yanjingang]

问题已解决，方法参考：
https://blog.yanjingang.com/?p=9033