MoveitConfigsBuilderを使いつつ、rvizの設定ファイルを適切に設定する要修正

crane_plus_examples/launch/demo.launch.py

@@ -48,26 +48,32 @@ 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'),
                 '/launch/run_move_group.launch.py']),
             condition=IfCondition(LaunchConfiguration('use_camera')),
             launch_arguments={
                 'loaded_description': description,
-                'rviz_config_file': rviz_config_file
+                'rviz_config': rviz_config_file_camera
             }.items()
         )
 

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