点群検出サンプル追加 (#161)

* 点群検出ノード追加

* pick_and_place_tf追加

* RealSenseのパラメータ名修正

* プレーシング位置調整

* 把持判定条件の整理

* 初期化動作修正

* 把持角度調整

* RVizの表示を調整

* RVizの表示を調整

* 使用するライブラリを整理

* スタイル修正

* Update README

* 左右判別用enum作成、パラメータの定数化

* 点群が空のときの判定処理を修正

* 配信する点群を予めreserveする

* スタイル修正

* 点群の取得範囲を修正

* 平面検出時の判定条件を修正

* コメント修正

sciurus17_examples/CMakeLists.txt

@@ -24,6 +24,8 @@ find_package(control_msgs REQUIRED)
 find_package(image_geometry REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(OpenCV REQUIRED COMPONENTS core)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(rclcpp_action REQUIRED)
@@ -115,6 +117,8 @@ set(executable_list
   waist_control
   pick_and_place_right_arm_waist
   pick_and_place_left_arm
+  pick_and_place_tf
+  point_cloud_detection
 )
 foreach(loop_var IN LISTS executable_list)
   add_executable(${loop_var} src/${loop_var}.cpp)
@@ -126,6 +130,8 @@ foreach(loop_var IN LISTS executable_list)
     image_geometry
     moveit_ros_planning_interface
     OpenCV
+    pcl_ros
+    pcl_conversions
     rclcpp
     tf2_geometry_msgs
     control_msgs

sciurus17_examples/README.md

@@ -19,6 +19,7 @@
     - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm)
     - [head\_camera\_tracking](#head_camera_tracking)
     - [chest\_camera\_tracking](#chest_camera_tracking)
+    - [point\_cloud\_detection](#point_cloud_detection)
 
 ## 準備（実機を使う場合）
 
@@ -93,6 +94,7 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_
 - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm)
 - [head\_camera\_tracking](#head_camera_tracking)
 - [chest\_camera\_tracking](#chest_camera_tracking)
+- [point\_cloud\_detection](#point_cloud_detection)
 
 実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。
 
@@ -203,3 +205,21 @@ ros2 launch sciurus17_examples chest_camera_tracking.launch.py
 [back to example list](#examples)
 
 ---
+
+### point_cloud_detection
+
+点群から物体を検出して掴むコード例です。
+
+検出された物体位置はtfのフレームとして配信されます。
+tfの`frame_id`は検出された順に`target_0`、`target_1`、`target_2`…に設定されます。
+掴む対象はSciurus17前方の0.3 mの範囲にある`target_0`に設定されています。
+物体検出には[Point Cloud Library](https://pointclouds.org/)を使用しています。
+
+次のコマンドを実行します
+```sh
+ros2 launch sciurus17_examples camera_example.launch.py example:='point_cloud_detection'
+```
+
+[back to example list](#examples)
+
+---

sciurus17_examples/launch/camera_example.launch.py

@@ -0,0 +1,89 @@
+# Copyright 2024 RT Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from sciurus17_description.robot_description_loader import RobotDescriptionLoader
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import SetParameter
+from launch_ros.actions import Node
+import yaml
+
+
+def load_file(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return file.read()
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+    description_loader = RobotDescriptionLoader()
+
+    robot_description_semantic_config = load_file(
+        'sciurus17_moveit_config', 'config/sciurus17.srdf')
+    robot_description_semantic = {
+        'robot_description_semantic': robot_description_semantic_config}
+
+    kinematics_yaml = load_yaml('sciurus17_moveit_config', 'config/kinematics.yaml')
+
+    declare_example_name = DeclareLaunchArgument(
+        'example', default_value='point_cloud_detection',
+        description=('Set an example executable name: '
+                     '[point_cloud_detection]')
+    )
+
+    declare_use_sim_time = DeclareLaunchArgument(
+        'use_sim_time', default_value='false',
+        description=('Set true when using the gazebo simulator.')
+    )
+
+    picking_node = Node(name="pick_and_place_tf",
+                        package='sciurus17_examples',
+                        executable='pick_and_place_tf',
+                        output='screen',
+                        parameters=[{'robot_description': description_loader.load()},
+                                    robot_description_semantic,
+                                    kinematics_yaml])
+
+    detection_node = Node(name=[LaunchConfiguration('example'), '_node'],
+                          package='sciurus17_examples',
+                          executable=LaunchConfiguration('example'),
+                          output='screen')
+
+    return LaunchDescription([
+        declare_use_sim_time,
+        SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+        picking_node,
+        declare_example_name,
+        detection_node
+    ])

sciurus17_examples/package.xml

@@ -24,6 +24,7 @@
   <depend>image_geometry</depend>
   <depend>libopencv-dev</depend>
   <depend>moveit_ros_planning_interface</depend>
+  <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2_geometry_msgs</depend>