diff --git a/crane_x7_examples/README.md b/crane_x7_examples/README.md
index a01b8263..2fe5a84a 100644
--- 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,6 +41,7 @@ USB通信ポートの設定については`crane_x7_control`の
### 3. move_groupとcontrollerを起動する
+#### 標準モデルを使用する場合
次のコマンドでmove_group (`crane_x7_moveit_config`)と
controller (`crane_x7_control`)を起動します。
@@ -29,6 +49,13 @@ controller (`crane_x7_control`)を起動します。
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
index a612b601..b6dfb6ba 100644
--- 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
index e4819ee8..d1c8a628 100644
--- a/crane_x7_examples/package.xml
+++ b/crane_x7_examples/package.xml
@@ -19,6 +19,7 @@
geometry_msgs
moveit_ros_planning_interface
rclcpp
+ realsense2_camera
tf2_geometry_msgs
ament_lint_auto