関数を実装するには、次の手順に分けて説明します。
- pcl を使用して、平面、円柱、球などのオブジェクトを識別します。
- pcl は、特定されたモデルの位置情報をトピックの形式で送信します。
- ロボットは信号を受信して処理し、深度情報を使用して追跡を実現します。
PCL の基盤はまったくありません。。。断片的な理解しかありません。このデモをできるだけ早く実装したいので、上司のアプリケーションで変更できるかどうかを確認するだけです。もちろん、時間があれば勉強するように手配する必要があります。後で、このフラグを設定しました。
1. 1つ目はpclの識別ですが、methyIDragonの識別チュートリアルを使用します。
GitHub - methylDragon/pcl-ros-tutorial: 点群ライブラリのかなり詳細なチュートリアル (ROS 統合ノート付き!)点群ライブラリのかなり詳細なチュートリアル (ROS 統合ノート付き!) - GitHub - methylDragon /pcl-ros-tutorial: 点群ライブラリのかなり詳細なチュートリアル (ROS 統合ノート付き!) https://github.com/methylDragon/pcl-ros-tutorialのPCLリファレンス (ROS.md)。この PCLdemo 認識の非常に詳細な紹介と応用です。
この記事は主にプロジェクトの実行に関するものですが、将来的には PCL の落とし穴を補う方法も学ぶことができると思います。
git をダウンロードした直後にコンパイルすると失敗します。このデモはチュートリアルとして使用されていると思いますが、minimal_pcl の minimum_pub_sub.cpp に空白部分があり、学生が自分で書くとコンパイルが失敗します。
私は主にdemo_codeを使用するので、minimal_pclパッケージを削除してからコンパイルすると、正常にコンパイルされます。
2. 次に、PCL ベースのシリンダー認識を実行します (デモではガゼボ シミュレーション)
ROS.md を使用した PCL リファレンスを読むことで確認できます(詳細については、例: シリンダーのセグメンテーションを参照してください)。
pcl_tester パッケージ内の 2 つの起動ファイル、pcl_tester.launch とシリンダー_segmentation.launch を開始します。
roslaunch pcl_tester pcl_tester.launch
1) pcl_tester.launch には主に次のノードが含まれます
- ガゼボの開始 (複数のボックスとシリンダー、シミュレーション環境を含む)
- rviz (ビジュアルディスプレイ)
- 静的変換パブリッシャー (位置変換を公開するノードとして 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 には主に次のノードが含まれます
- ノードレットを使用してノードを統合し、データをより速く転送する
- pcl/VoxelGrid: VoxelGrid フィルターを実行して NaN をクリーンアップし、データをダウンサンプリングします。
- pcl/NormalEstimation : 点法線を推定します
- pcl/SACSegmentationFromNormal s:床面をセグメント化します。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:円柱をセグメント化します、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: 外れ値フィルターを実行してデータをクリーンアップします
- 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>
エフェクトはチュートリアルの画像と同じで、ガゼボでシミュレートされ、Rviz で視覚的にシミュレーションされます。ガゼボ上で円柱やその他のオブジェクトを移動すると、それに対応する変化が RViz 上で発生します。
もちろん、さらに知る必要があるのは、PCL が正常に認識された後、PCL がどのように表示され、どのようなトピックが公開されるかということです。
サブスクリプションによって表示された点群トピックを閉じて、1 つずつ開いて表示します。
1 つ目は CameraCloud (カメラ点群データ)、受け入れられるトピックは /camera/ Depth/point です
VoxeIGrid、表示されるトピックは /voxel_grid/output です
NonFloorPoints、表示されるトピックは /extract_plane_indices/output です
UnfiltererdCylinder、表示されるトピックは /extract_cylinder_indices/output です
CylinderPoints、表示されるのは /outlier_filter/output です
また、cylinder_segmentation.launch ファイルを読むことで、少し理解することができます。コードの実行シーケンスは、起動ノードのシーケンスとほぼ同じです。ただし、ファイル内では、入力で指定されたトピックのみが表示され、トピック名が表示されません。推測はできましたが、十分な精度が得られなかったため、rqt ツールを使用してノード間の通信を表示しようとしました。
rqt_graph
しかし残念ながら、ノードレットを使用しているため、トピック間の通信は隠蔽されています。
ノード間のコミュニケーションを明確に理解するために、それを示すマインド マップが作成され、ノードの出力トピックはより重要なもののみが取り上げられます。
トピックの詳細なリストについては、デモの開始後に ROS コマンドを使用してください。
rostopic list
次に、ブレインマップに基づいて、各ノードの出力トピックの 1 つを要約することができます。実際には、これはノード名と出力であり、各ノードがどのトピックを取得したいのかも明確にわかります。
話は戻りますが、認識に成功した後は、認識成功後の物体の位置情報を取得する必要があります。
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 深度カメラはこのトピックを取得できるため、深度カメラを開いてからシリンダー_segmentation.launch を開始するかどうかを判断します。ファイルは実現できるでしょうか?
まずカメラを起動してください
$ roslaunch camera_driver_transfer astrapro.launch
次にシリンダー認識デモを開始します
$ roslaunch pcl_tester cylinder_segmentation.launch
rviz に表示すると明らかな認識効果はありません (rviz はシリンダーを開くことに注意してください...pcl_tester の rviz の rviz)
次に、トピックから逆算して、トピック内の情報を 1 つずつ確認します。
$ rostopic echo /extract_cylinder_indices/output
$ rostopic echo /extract_plane_indices/output
$ rostopic echo /cylinder_segmentation/model
モデルデータの取得
埋める穴