RealSense D435搭載モデルに対応 (#164)

* use_d435引数を追加 * 引数use_d435を追加 * スタイル修正 * スタイル修正 * 依存パッケージにrealsense2_cameraを追加 * Update README * Gazebo上でuse_d435の使用停止 * Update crane_x7_examples/README.md Co-authored-by: Shota Aoki <[email protected]> Co-authored-by: Shota Aoki <[email protected]>
rt-net · Nov 15, 2022 · c809856 · c809856
1 parent 2aa7d37
commit c809856
Show file tree

Hide file tree

Showing 3 changed files with 50 additions and 1 deletion.
diff --git a/crane_x7_examples/README.md b/crane_x7_examples/README.md
@@ -2,6 +2,25 @@
 
 このパッケージはCRANE-X7 ROS 2パッケージのサンプルコード集です。
 
+- [crane_x7_examples](#crane_x7_examples)
+  - [準備（実機を使う場合）](#準備実機を使う場合)
+    - [1. CRANE-X7本体をPCに接続する](#1-crane-x7本体をpcに接続する)
+    - [2. USB通信ポートの接続を確認する](#2-usb通信ポートの接続を確認する)
+    - [3. move_groupとcontrollerを起動する](#3-move_groupとcontrollerを起動する)
+      - [標準モデルを使用する場合](#標準モデルを使用する場合)
+      - [RealSense D435マウンタ搭載モデルを使用する場合](#realsense-d435マウンタ搭載モデルを使用する場合)
+  - [準備 (Gazeboを使う場合)](#準備-gazeboを使う場合)
+    - [1. move_groupとGazeboを起動する](#1-move_groupとgazeboを起動する)
+  - [サンプルプログラムを実行する](#サンプルプログラムを実行する)
+  - [Examples](#examples)
+    - [gripper_control](#gripper_control)
+    - [pose_groupstate](#pose_groupstate)
+    - [joint_values](#joint_values)
+    - [cartesian_path](#cartesian_path)
+      - [Videos](#videos)
+    - [pick_and_place](#pick_and_place)
+      - [Videos](#videos-1)
+
 ## 準備（実機を使う場合）
 
 ![crane_x7](https://rt-net.github.io/images/crane-x7/CRANE-X7-500x500.png)
@@ -22,13 +41,21 @@ USB通信ポートの設定については`crane_x7_control`の
 
 ### 3. move_groupとcontrollerを起動する
 
+#### 標準モデルを使用する場合
 次のコマンドでmove_group (`crane_x7_moveit_config`)と
 controller (`crane_x7_control`)を起動します。
 
 ```sh
 ros2 launch crane_x7_examples demo.launch.py port_name:=/dev/ttyUSB0
 ```
 
+#### RealSense D435マウンタ搭載モデルを使用する場合
+[RealSense D435マウンタ](https://github.com/rt-net/crane_x7_Hardware/blob/master/3d_print_parts/v1.0/CRANE-X7_HandA_RealSenseD435マウンタ.stl)を搭載している場合は次のコマンドを実行します。RealSense D435が起動し、camera_linkがロボットモデルに追加されます。
+
+```sh
+ros2 launch crane_x7_examples demo.launch.py port_name:=/dev/ttyUSB0 use_d435:=true
+```
+
 ## 準備 (Gazeboを使う場合)
 
 ![crane_x7_gazebo](https://rt-net.github.io/images/crane-x7/crane_x7_gazebo_ros2.png)

diff --git a/crane_x7_examples/launch/demo.launch.py b/crane_x7_examples/launch/demo.launch.py
@@ -19,6 +19,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.actions import IncludeLaunchDescription
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
 
@@ -36,6 +37,12 @@ def generate_launch_description():
         description='Set baudrate.'
     )
 
+    declare_use_d435 = DeclareLaunchArgument(
+        'use_d435',
+        default_value='false',
+        description='Use d435.'
+    )
+
     config_file_path = os.path.join(
         get_package_share_directory('crane_x7_control'),
         'config',
@@ -51,6 +58,7 @@ def generate_launch_description():
     description_loader = RobotDescriptionLoader()
     description_loader.port_name = LaunchConfiguration('port_name')
     description_loader.baudrate = LaunchConfiguration('baudrate')
+    description_loader.use_d435 = LaunchConfiguration('use_d435')
     description_loader.timeout_seconds = '1.0'
     description_loader.manipulator_config_file_path = config_file_path
     description_loader.manipulator_links_file_path = links_file_path
@@ -71,9 +79,22 @@ def generate_launch_description():
             launch_arguments={'loaded_description': description}.items()
         )
 
+    realsense_node = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                get_package_share_directory('realsense2_camera'),
+                '/launch/rs_launch.py']),
+            condition=IfCondition(LaunchConfiguration('use_d435')),
+            launch_arguments={
+                'device_type': 'd435',
+                'pointcloud.enable': 'true',
+            }.items()
+        )
+
     return LaunchDescription([
         declare_port_name,
         declare_baudrate,
+        declare_use_d435,
         move_group,
-        control_node
+        control_node,
+        realsense_node
     ])
diff --git a/crane_x7_examples/package.xml b/crane_x7_examples/package.xml
@@ -19,6 +19,7 @@
   <depend>geometry_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>rclcpp</depend>
+  <depend>realsense2_camera</depend>
   <depend>tf2_geometry_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>