[Project Implementation]: PCL recognizes objects and ROS robots track them (exploration and implementation)

To implement the function, I will divide it into the following steps:

1. Use pcl to identify objects such as planes, cylinders, spheres, etc.
2. pcl sends the identified model location information in the form of topics
3. The robot receives the signal and processes it, using depth information to achieve tracking.

There is no PCL foundation at all. . . I only have a sporadic understanding of it. Since I want to implement this demo as soon as possible, I can only see if it can be changed in the application of the boss. Of course, I must arrange to study it if I have time later, so I set this flag.

1. The first is pcl identification. I use the identification tutorial of 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 in thePCLReference with ROS.md, there is a very detailed introduction and application of this PCLdemo recognition

This article is mainly about project implementation. I think I can also learn to make up for the pitfalls of PCL in the future.

Compiling directly after git is downloaded will not be successful. I guess this demo is used as a tutorial. There will be a blank part in minimal_pub_sub.cpp in minimal_pcl for students to write by themselves, resulting in unsuccessful compilation.

Since I mainly use demo_code, I delete the minimal_pcl package and then compile it, and it compiles successfully.

2. Next, perform PCL-based cylinder recognition (gazebo simulation in demo)

You can find out by reading PCL Reference with ROS.md (see Example: Cylinder Segmentation for details )

       Start two launch files, pcl_tester.launch& cylinder_segmentation.launch in the pcl_tester package

roslaunch pcl_tester pcl_tester.launch

           1) pcl_tester.launch mainly contains the following nodes

• Start gazebo (including several boxes & cylinders, simulation environment)
• rviz (visual display)
• Static transform publishers (as node publishing position transformation 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 mainly contains the following nodes

• Use nodelet to integrate nodes to transfer data faster
• 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>

The effect will be the same as the tutorial picture, simulated by gazebo and visual simulation by Rviz. When we move the cylinder or other objects on gazebo, there will be corresponding changes on rviz.

Of course, what we need to know more is, after PCL is successfully recognized, how will it be displayed and what topics will be published?

Close the point cloud topics displayed by the subscription, and then open them one by one to view them.

The first is CameraCloud (camera point cloud data), the accepted topic is /camera/depth/point

 VoxeIGrid, the displayed topic is /voxel_grid/output

 NonFloorPoints, the displayed topic is /extract_plane_indices/output

 UnfiltererdCylinder, the displayed topic is /extract_cylinder_indices/output

 CylinderPoints, what is shown is /outlier_filter/output

 You can also get a little idea by reading the cylinder_segmentation.launch file. The code execution sequence is roughly the same as the launch node sequence. However, in the file, we can only see the topic specified by input, but lack the topic name and information of output. Although it can We guessed it, but it was not accurate enough. For this reason, we tried to use the rqt tool to view the communication between nodes.

rqt_graph

But unfortunately, due to the use of nodelet, the communication between topics is hidden.

 In order to clearly understand the communication between nodes, a mind map was drawn to show it. The output topics of the nodes only take the more important ones.

 For a detailed list of topics, use the ROS command after starting the demo.

rostopic list

 Then based on the brain map, one of the output topics of each node can be summarized. In fact, it is the node name plus output. It can also be clearly seen which topic each node wants to obtain.

Back to the topic, after we successfully recognize, what we need more is to obtain the position information of the object after successful recognition.

Also use ROS commands to obtain

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

The topic name is /cylinder_segmentation/model

The data type is Type: pcl_msgs/ModelCoefficients

The value obtained from the data has 7 digits. It is guessed that it should be coordinates and pose (quaternion)

Then, next we only need to use the depth camera to obtain the position information of the recognized object, and then subscribe to the tracking code to achieve the effect of robot tracking.

3. PCL-based object recognition (depth camera)

It is observed that the voxel_grid node initially subscribes to the topic /camera/depth/points, which is depth point cloud data, and the astrapro depth camera can obtain this topic, so whether to open the depth camera and then start the cylinder_segmentation.launch file will Can it be realized?

Start the camera first

$ roslaunch camera_driver_transfer astrapro.launch

Then start the cylinder recognition demo

$ roslaunch pcl_tester cylinder_segmentation.launch

There is no obvious recognition effect when displayed on rviz (note that rviz opens the cylinder...rviz of rviz in pcl_tester)

 Then work backwards from the topic and check the information in the topic one by one.

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

$ rostopic echo /cylinder_segmentation/model

 Get model data

---

Hole to be filled