从零开始调试ROS-PCL库(1)：初始化/导入及显示模型

1 测试PCL库的基本功能以及导入模型

通过sudo apt install ros-melodic-desktop full安装的ros包含了PCL库，因此不需要额外安装。但是由于PCL的编程通常是使用C++来实现的，因此相比之前使用的简单的Python需要额外学习一下关于头文件的使用/CMakeList.txt文件的编写以及package.xml文件的编写。

一个特别好的教程:知乎网友总结的教程汇总

1.1 复习通过C++发布话题

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>

// These are some normal format with topic publishing.
int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1);
    ros::Rate loop_rate(10);

    int count = 0;
    while (ros::ok())
    {
        std_msgs::String msg;
        std::stringstream ss;
        ss << "hello world" << count;
        msg.data = ss.str();

        ROS_INFO("%s", msg.data.c_str());

        chatter_pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }
}

1.2 获取pcd格式点云文件

通过realsense D435i直接获取的点云文件格式为ply，总是在程序中导入失败，因此根据《点云库PCL：从入门到精通中》的插入pcd格式点云的方式进行尝试，成功了，就先使用pcd格式吧，后续再改进。
先在realsense-viewer中导出ply文件；
再将ply文件使用matlab打开；
再使用pcwrite函数保存pcd文件；
最后将pcd文件在ros中导入

1.3 ros-pcl导入pcd文件

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
//which contains the required definitions to load and store point clouds to PCD and other file formats.

int main(int argc,char **argv){
    std::string model_path = "/home/jjj/brickbot_ws/src/pcl_test/config/points_data/";
    std::string model_name = "pcd_05.pcd";

    ros::init(argc,argv,"UandBdetect");
    ros::NodeHandle nh;
    ros::Publisher pcl_pub=nh.advertise<sensor_msgs::PointCloud2> ("pcl_output",1);
    pcl::PointCloud<pcl::PointXYZ> cloud;
    sensor_msgs::PointCloud2 output;
    // pcl::io::loadPCDFile("pcd_05.pcd", cloud);//修改自己pcd文件所在路径
    pcl::io::loadPCDFile<pcl::PointXYZ>(model_path + model_name, cloud);

    //Convert the cloud to ROS message
    pcl::toROSMsg(cloud, output);
    output.header.frame_id="odom";
    //this has been done in order to be able to visualize our PointCloud2 message on the RViz visualizer

    //！！！这一步需要注意，是后面rviz的 fixed_frame  !!!敲黑板，画重点。
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        /* code for loop body */
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

在rostopic list中可以看到发布的点云话题，成功了。
这里使用

    std::string model_path = "/home/jjj/brickbot_ws/src/pcl_test/config/points_data/";
    std::string model_name = "pcd_05.pcd";

来定义模型位置和模型名称。

2 测试ros-pcl显示模型

把程序中关于ROS发布topic的部分注释掉，暂时用不到。现在就只把ros当作一个编译工具来用。

关于点云的显示问题，pcl/visualization/cloud_viewer.h类似乎不能用于多线程，也就是不能用于ros？

2.1 测试独立于ROS运行pcl文件

创建pcl_test.cpp

#include <iostream>
#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::CloudViewer viewer ("test");
    viewer.showCloud(point_cloud_ptr);
    while (!viewer.wasStopped()){ };
    return 0;
}

以及CMakeLists.txt

cmake_minimum_required(VERSION 2.6)
project(pcl_test)

find_package(PCL 1.2 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)

以及编译运行：

cmake .
make
./pcl_test

运行成功，生成由彩色点构成的圆环图案。

2.2 尝试将上边的代码在ros里运行

一开始出现编译功能包失败的问题，显示关键词未定义的引用。接下来开始排除问题：

1. 它不是ros兼容性的问题，一段程序应当在ros和外部都能运行，初步判断是编译的问题；
2. 它不是gcc和g++版本的问题，不像有些人写的攻略一样。
3. 也不像有些人说的是ros版本的问题。

主要还是CMakeLists.txt的内容格式问题。以下附上调试成功的一个CMakeLists.txt：

cmake_minimum_required(VERSION 2.8.3)
project(pcl_test)


## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS 
  roscpp
  rospy
  std_msgs
  sensor_msgs
  genmsg
  pcl_ros
  pcl_conversions
  message_generation
  geometry_msgs
  tf
)

find_package(PCL REQUIRED)

## Declare ROS messages and services
add_message_files(FILES Num.msg)
add_service_files(FILES AddTwoInts.srv)

## Generate added messages and services
generate_messages(DEPENDENCIES std_msgs)

## Declare a catkin package
catkin_package(
  CATKIN_DEPENDS roscpp rospy std_msgs
)

## Build talker and listener
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${PCL_INCLUDE_DIRS}
)

add_executable(test src/test.cpp)
target_link_libraries(test ${catkin_LIBRARIES} ${PCL_LIBRARIES})
add_dependencies(test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

add_executable(pcl_sample src/pcl_sample.cpp)
target_link_libraries(pcl_sample ${catkin_LIBRARIES} ${PCL_LIBRARIES})
add_dependencies(pcl_sample ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

首先是最后两大段，对应每个cpp文件；其中的add_dependencies一行实测是可以去掉的，不知道是什么原因。要注意的是第二行需要加入pcl的关键词部分**${PCL_LIBRARIES}**。

其次在另一大段include_directories段落也需要加上**${PCL_LIBRARIES}**关键词。

最后就是在find_package段落加入pcl相关的包。

现在用catkin_make编译就成功了。

2.3 导入/显示自己的模型

/* 显示点云  */
pcl::visualization::CloudViewer viewer ("mytest");
viewer.showCloud (myCloud);
while (!viewer.wasStopped()){ };
/* 显示点云  */

2.4 所有使用到的头文件

显示点云用到的头文件：

#include <pcl/visualization/cloud_viewer.h>

调用ros以及发布点云topic用到的头文件：

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>

读取pcd点云用到的头文件：

#include <pcl/io/pcd_io.h>