【项目实现】：PCL识别物体，ROS机器人进行跟踪（摸索与实现）

想要实现功能，我将分成以下几个步骤

1. 使用pcl识别物体，如平面，圆柱，球等等
2. pcl将识别出来的模型位置信息以话题的形式发送出来
3. 机器人接受信号，并进行处理，采用深度信息来实现跟踪

完全没有PCL基础。。。只有零星的了解过，既然要尽快实现这个demo，只能看看能不能在大佬的应用上更改了，当然后期有时间一定要安排上学习，立此flag了。

1.首先是pcl识别，我采用的是methyIDragon大佬的识别教程

GitHub - methylDragon/pcl-ros-tutorial: A fairly in-depth tutorial for the Point Cloud Library (with ROS integration notes!)A fairly in-depth tutorial for the Point Cloud Library (with ROS integration notes!) - GitHub - methylDragon/pcl-ros-tutorial: A fairly in-depth tutorial for the Point Cloud Library (with ROS integration notes!)https://github.com/methylDragon/pcl-ros-tutorial在引言的PCL Reference with ROS.md中，有很详细的这个PCLdemo识别的介绍与应用

这个文章主要还是项目实现上，我想之后也能学习补上PCL的坑吧

git下来之后直接进行编译会不成功，猜想此demo作为教程，在minimal_pcl中的minimal_pub_sub.cpp会有留空让学生自行编写的部分，导致编译不成功

由于我采用的主要是demo_code,所以将minimal_pcl包删除后再进行编译，能够编译成功

2.接下来进行基于PCL的圆柱体识别（demo中的gazebo仿真）

阅读PCL Reference with ROS.md可以知道（详细参看Example: Cylinder Segmentation）

       启动两个launch文件，pcl_tester包中的pcl_tester.launch& cylinder_segmentation.launch

roslaunch pcl_tester pcl_tester.launch

           1) pcl_tester.launch主要包含以下几个节点

• 启动gazebo(包含几个box&cylinder，仿真环境)
• rviz（可视化显示）
• Static transform publishers(作为节点发布位置转换 camera_transform_publisher&camera_image_transform_publisher)

<launch>
  <!-- Load URDF -->
  <param
    name="robot_description"
    command="$(find xacro)/xacro --inorder $(find pcl_tester_description)/urdf/depth_camera.urdf.xacro"/>

  <!-- Start Gazebo -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="$(find pcl_tester)/worlds/pcl_tester.world"/>
  </include>

  <!-- Spawn Camera into Gazebo -->
  <node
    name="camera_urdf_spawner"
    pkg="gazebo_ros"
    type="spawn_model"
    args="-param robot_description -urdf -model camera -z 0.25"/>

  <!-- Static transform publishers -->
  <node
    pkg="tf2_ros"
    type="static_transform_publisher" 
    name="camera_transform_publisher"
    args="0 0 0 0 0 0 1 world camera_link"/>

  <node
    pkg="tf2_ros"
    type="static_transform_publisher" 
    name="camera_image_transform_publisher"
    args="0 0 0 -0.5 -0.5 0.5 0.5 camera_link camera_image_link"/>

  <!-- RViz -->
  <node type="rviz" name="pcl_rviz" pkg="rviz" args="-d $(find pcl_tester)/rviz/pcl_tester.rviz"/>


  <!-- Spawn some junk -->
  <param name="box_description" command="$(find xacro)/xacro.py $(find pcl_tester)/objects/simple_box.urdf" />
  <param name="cylinder_description" command="$(find xacro)/xacro.py $(find pcl_tester)/objects/simple_cylinder.urdf" />

  <node name="spawn_box_1" pkg="gazebo_ros" type="spawn_model" args="-urdf -param box_description -x 2.23 -y -0.12 -z .2 -model box_model_1" respawn="false"/>
  <node name="spawn_box_2" pkg="gazebo_ros" type="spawn_model" args="-urdf -param box_description -x 0.84 -y -0.4 -z .2 -model box_model_2" respawn="false"/>
  <node name="spawn_box_3" pkg="gazebo_ros" type="spawn_model" args="-urdf -param box_description -x 1.0 -y 0.53 -Y 4 -z .2 -model box_model_3" respawn="false"/>
  <node name="spawn_box_4" pkg="gazebo_ros" type="spawn_model" args="-urdf -param box_description -x 1.571 -y 0.43 -Y 2 -R 3.14 -z .2 -model box_model_4" respawn="false"/>

  <node name="spawn_cylinder_1" pkg="gazebo_ros" type="spawn_model" args="-urdf -param cylinder_description -x 1.38 -y 0.35 -z .25 -model cylinder_model_1" respawn="false"/>
  <node name="spawn_cylinder_2" pkg="gazebo_ros" type="spawn_model" args="-urdf -param cylinder_description -x 1.48 -y -0.243 -z .25 -model cylinder_model_2" respawn="false"/>
  <node name="spawn_cylinder_3" pkg="gazebo_ros" type="spawn_model" args="-urdf -param cylinder_description -x 1.571 -y 0.43 -z .6 -model cylinder_model_3" respawn="false"/>
</launch>

roslaunch pcl_tester cylinder_segmentation.launch

2) cylinder_segmentation.launch主要包含以下几个节点

• 使用nodelet将节点整合，让传输数据更快速
• pcl/VoxelGrid: Run a VoxelGrid filter to clean NaNs and downsample the data
• pcl/NormalEstimation: Estimate point normals
• pcl/SACSegmentationFromNormals:Segment the floor plane,                                 model_type: 11                                                                                                                <remap from="~input" to="/voxel_grid/output" />                                                          <remap from="~normals" to="/normal_estimation/output" />
• pcl/ExtractIndices：                                                                                                                   一个是name="extract_plane_indices"                                                                       <remap from="~input" to="/voxel_grid/output"/>                                                                      <remap from="~indices" to="/planar_segmentation/inliers" />                                             接着一个是name="extract_plane_normal_indices"                                                                                   <remap from="~input" to="/normal_estimation/output" />                                                    <remap from="~indices" to="/planar_segmentation/inliers" />
• pcl/SACSegmentationFromNormals：Segment a cylinder ，                                    model_type: 5                                                                                                              <remap from="~input" to="/extract_plane_indices/output" />                                              <remap from="~normals" to="/extract_plane_normal_indices/output" />
•  pcl/ExtractIndices：name="extract_cylinder_indices"                                                            <remap from="~input" to="/extract_plane_indices/output" />                                                 <remap from="~indices" to="/cylinder_segmentation/inliers" />
• pcl/StatisticalOutlierRemoval： Run an outlier filter to clean the data
• RVIZ

<launch>
  <node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" />

  <!-- Run a VoxelGrid filter to clean NaNs and downsample the data -->
  <node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid pcl_manager" output="screen">
    <remap from="~input" to="/camera/depth/points" />
    <rosparam>
      filter_field_name: z
      filter_limit_min: 0.01
      filter_limit_max: 10
      filter_limit_negative: False
      leaf_size: 0.02
    </rosparam>
  </node>

  <!-- Estimate point normals -->
  <node pkg="nodelet" type="nodelet" name="normal_estimation" args="load pcl/NormalEstimation pcl_manager" output="screen">
    <remap from="~input" to="/voxel_grid/output" />
    <rosparam>
      # -[ Mandatory parameters
      k_search: 12
      radius_search: 0
      # Set the spatial locator. Possible values are: 0 (ANN), 1 (FLANN), 2 (organized)
      spatial_locator: 0
    </rosparam>
  </node>

  <!-- Segment the floor plane -->
  <node pkg="nodelet" type="nodelet" name="planar_segmentation" args="load pcl/SACSegmentationFromNormals pcl_manager" output="screen">
    <remap from="~input"   to="/voxel_grid/output" />
    <remap from="~normals" to="/normal_estimation/output" />
    <rosparam>
      # -[ Mandatory parameters

      # model_type:
      # 0: SACMODEL_PLANE
      # 1: SACMODEL_LINE
      # 2: SACMODEL_CIRCLE2D
      # 3: SACMODEL_CIRCLE3D
      # 4: SACMODEL_SPHERE
      # 5: SACMODEL_CYLINDER
      # 6: SACMODEL_CONE
      # 7: SACMODEL_TORUS
      # 8: SACMODEL_PARALLEL_LINE
      # 9: SACMODEL_PERPENDICULAR_PLANE
      # 10: SACMODEL_PARALLEL_LINES
      # 11: SACMODEL_NORMAL_PLANE
      # 12: SACMODEL_NORMAL_SPHERE
      # 13: SACMODEL_REGISTRATION
      # 14: SACMODEL_REGISTRATION_2D
      # 15: SACMODEL_PARALLEL_PLANE
      # 16: SACMODEL_NORMAL_PARALLEL_PLANE
      # 17: SACMODEL_STICK

      model_type: 11
      axis: [0.0,0.0,1.0]
      distance_threshold: 0.155
      max_iterations: 1000
      method_type: 0
      optimize_coefficients: true
      normal_distance_weight: 0.1
      eps_angle: 0.09
    </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet" name="extract_plane_indices" args="load pcl/ExtractIndices pcl_manager" output="screen">
    <remap from="~input"   to="/voxel_grid/output" />
    <remap from="~indices" to="/planar_segmentation/inliers" />
    <rosparam>
      negative: true
    </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet" name="extract_plane_normal_indices" args="load pcl/ExtractIndices pcl_manager" output="screen">
    <remap from="~input"   to="/normal_estimation/output" />
    <remap from="~indices" to="/planar_segmentation/inliers" />
    <rosparam>
      negative: true
    </rosparam>
  </node>

  <!-- Segment a cylinder -->
  <node pkg="nodelet" type="nodelet" name="cylinder_segmentation" args="load pcl/SACSegmentationFromNormals pcl_manager" output="screen">
    <remap from="~input"   to="/extract_plane_indices/output" />
    <remap from="~normals" to="/extract_plane_normal_indices/output" />
    <rosparam>
      model_type: 5
      distance_threshold: 0.1
      max_iterations: 5000
      method_type: 0
      optimize_coefficients: true
      normal_distance_weight: 0.45
      eps_angle: 0.0
      radius_min: 0.05
      radius_max: 0.15
      min_inliers: 0
    </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet" name="extract_cylinder_indices" args="load pcl/ExtractIndices pcl_manager" output="screen">
    <remap from="~input"   to="/extract_plane_indices/output" />
    <remap from="~indices" to="/cylinder_segmentation/inliers" />
    <rosparam>
      negative: false
    </rosparam>
  </node>

  <!-- Run an outlier filter to clean the data -->
  <node pkg="nodelet" type="nodelet" name="outlier_filter" args="load pcl/StatisticalOutlierRemoval pcl_manager" output="screen">
    <remap from="~input"   to="/extract_cylinder_indices/output" />
    <rosparam>
      mean_k: 75
      stddev: 0.1
    </rosparam>
  </node>

  <!-- RViz -->
  <node type="rviz" name="pcl_rviz" pkg="rviz" args="-d $(find pcl_tester)/rviz/cylinder_segmentation.rviz"/>
</launch>

效果就会和教程图上的一样，由gazebo仿真，Rviz可视化仿真，当我们在gazebo上移动圆柱体或其他物体时，rviz上也会有相对应的变化。

当然，我们更要知道的是，PCL成功识别之后，会怎么显示，会发布什么话题？

把订阅显示的点云话题关闭，在再逐个打开查看

首先是CameraCloud（相机点云数据），接受的topic是/camera/depth/point

 VoxeIGrid,显示的话题是/voxel_grid/output

 NonFloorPoints,显示的话题是/extract_plane_indices/output

 UnfiltererdCylinder,显示的话题是/extract_cylinder_indices/output

 CylinderPoints,显示的是/outlier_filter/output

 阅读cylinder_segmentation.launch文件，也可以略知一二，代码执行顺序大致如launch启动节点顺序一致，但是在文件中，我们只能看到input指定的话题，而缺少output的话题名称和信息，虽然能猜算出来，但却不够准确，为此，我们尝试使用rqt工具来查看节点间的通信情况。

rqt_graph

但是很可惜，由于使用了nodelet,话题间的通信都被隐藏在内

 为了能清晰地了解节点间的通信，画了一个脑图来展示，其中节点的输出话题只拿取比较重要的

 详细的话题列表在启动demo后用ROS命令来查看

rostopic list

 那么根据脑图，可以总结出每个节点的其中一个输出话题，其实也就是节点名称加output，每个节点要获取哪一个话题也可以很清晰地看出来

回到主题，我们识别成功后，更需要的是获取识别成功后的物体位置信息

同样使用ROS命令来获取

$ rostopic info /cylinder_segmentation/model 
$ rostopic echo /cylinder_segmentation/model 

话题名为/cylinder_segmentation/model

数据类型为Type: pcl_msgs/ModelCoefficients

数据获取到的值有7位数，猜想应该为坐标与位姿（四元数）

那么，接下来我们只要用深度摄像头获取识别到的物体的位置信息，再由跟踪代码订阅，就能实现机器人跟踪的效果

3.基于PCL的物体识别（深度摄像头）

观察到voxel_grid这个节点一开始订阅的话题是/camera/depth/points，即深度点云数据，而astrapro深度摄像头是能够获取到此话题的，所以是否将深度摄像头打开，再启动cylinder_segmentation.launch文件就可以实现了呢？

首先启动摄像头

$ roslaunch camera_driver_transfer astrapro.launch

接着启动圆柱识别demo

$ roslaunch pcl_tester cylinder_segmentation.launch

在rviz上显示时没有明显识别效果（注意rviz打开的是pcl_tester中rviz的cylinder...rviz）

 那么从话题中反推，逐一查看话题中的信息

$ rostopic echo /extract_cylinder_indices/output 
$ rostopic echo /extract_plane_indices/output 

$ rostopic echo /cylinder_segmentation/model

 获取到模型数据

---

待填坑