想要实现功能,我将分成以下几个步骤
- 使用pcl识别物体,如平面,圆柱,球等等
- pcl将识别出来的模型位置信息以话题的形式发送出来
- 机器人接受信号,并进行处理,采用深度信息来实现跟踪
完全没有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
获取到模型数据
待填坑