From 30b83f49662859d0968a4d499e17eb61fa5164f9 Mon Sep 17 00:00:00 2001
From: mizonon <mizoguchi@rt-net.jp>
Date: Fri, 29 Nov 2024 19:10:27 +0900
Subject: [PATCH] =?UTF-8?q?MoveitConfigsBuilder=E3=82=92=E4=BD=BF=E3=81=84?=
 =?UTF-8?q?=E3=81=A4=E3=81=A4=E3=80=81rviz=E3=81=AE=E8=A8=AD=E5=AE=9A?=
 =?UTF-8?q?=E3=83=95=E3=82=A1=E3=82=A4=E3=83=AB=E3=82=92=E9=81=A9=E5=88=87?=
 =?UTF-8?q?=E3=81=AB=E8=A8=AD=E5=AE=9A=E3=81=99=E3=82=8B=E8=A6=81=E4=BF=AE?=
 =?UTF-8?q?=E6=AD=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 crane_plus_examples/launch/demo.launch.py          | 14 ++++++++++----
 .../launch/run_move_group.launch.py                |  9 +++++++++
 2 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py
index 12a7dbe..5e164b0 100644
--- a/crane_plus_examples/launch/demo.launch.py
+++ b/crane_plus_examples/launch/demo.launch.py
@@ -48,18 +48,24 @@ def generate_launch_description():
     description_loader.use_camera = LaunchConfiguration('use_camera')
     description = description_loader.load()
 
+    rviz_config_file_move_group = get_package_share_directory(
+        'crane_plus_moveit_config'
+        ) + '/config/moveit.rviz'
+    rviz_config_file_camera = get_package_share_directory(
+        'crane_plus_examples'
+        ) + '/launch/camera_example.rviz'
+
     move_group = IncludeLaunchDescription(
             PythonLaunchDescriptionSource([
                 get_package_share_directory('crane_plus_moveit_config'),
                 '/launch/run_move_group.launch.py']),
             condition=UnlessCondition(LaunchConfiguration('use_camera')),
             launch_arguments={
-                'loaded_description': description
+                'loaded_description': description,
+                'rviz_config': rviz_config_file_move_group
             }.items()
         )
 
-    rviz_config_file = get_package_share_directory(
-        'crane_plus_examples') + '/launch/camera_example.rviz'
     move_group_camera = IncludeLaunchDescription(
             PythonLaunchDescriptionSource([
                 get_package_share_directory('crane_plus_moveit_config'),
@@ -67,7 +73,7 @@ def generate_launch_description():
             condition=IfCondition(LaunchConfiguration('use_camera')),
             launch_arguments={
                 'loaded_description': description,
-                'rviz_config_file': rviz_config_file
+                'rviz_config': rviz_config_file_camera
             }.items()
         )
 
diff --git a/crane_plus_moveit_config/launch/run_move_group.launch.py b/crane_plus_moveit_config/launch/run_move_group.launch.py
index a72bd21..3b7d728 100755
--- a/crane_plus_moveit_config/launch/run_move_group.launch.py
+++ b/crane_plus_moveit_config/launch/run_move_group.launch.py
@@ -18,6 +18,7 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
 from moveit_configs_utils import MoveItConfigsBuilder
 from moveit_configs_utils.launches import generate_move_group_launch
 from moveit_configs_utils.launches import generate_moveit_rviz_launch
@@ -51,6 +52,8 @@ def generate_launch_description():
             description='Set the path to rviz configuration file.',
         )
     )
+    
+    rviz_config = LaunchConfiguration('rviz_config')
 
     moveit_config = (
         MoveItConfigsBuilder('crane_plus')
@@ -74,6 +77,12 @@ def generate_launch_description():
     ld.add_entity(generate_move_group_launch(moveit_config))
 
     # RViz
+    rviz_entities = generate_moveit_rviz_launch(moveit_config).entities
+    for entity in rviz_entities:
+        if isinstance(entity, Node):
+            entity.cmd.extend(['--ros-args', '--params-file', rviz_config])
+            ld.add_entity(entity)
+
     ld.add_entity(generate_moveit_rviz_launch(moveit_config))
 
     # Static TF