Added rviz.launch.py

my_panda_gazebo/launch/simulation.launch.py

@@ -151,6 +151,25 @@ def generate_launch_description():
         }.items(),
     )
 
+    moveit_rviz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    get_package_share_directory("my_panda_moveit_config"),
+                    "launch",
+                    "rviz.launch.py",
+                ]
+            )
+        ),
+        launch_arguments={
+            "load_gripper": load_gripper,
+            "robot_ip": robot_ip,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "use_sim": use_sim,
+        }.items()
+    )
+
     return LaunchDescription(
         [   declare_load_gripper_arg,
             declare_robot_ip_arg,
@@ -160,6 +179,7 @@ def generate_launch_description():
             gz_sim, ## Spawn empty world in ignition-gazebo.
             gz_spawn_entity, ## Spawn Franka Emika Robot on the table.
             bringup_launch,
+            moveit_rviz_launch,
             SetParameter(name="use_sim_time", value=True),
         ]
     )

my_panda_moveit_config/launch/moveit.launch.py

@@ -66,7 +66,7 @@ def generate_launch_description():
     robot_description_config = Command(
         [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper,
          ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware,
-         ' fake_sensor_commands:=', fake_sensor_commands])
+         ' fake_sensor_commands:=', fake_sensor_commands, ' use_sim:=', use_sim])
 
     robot_description = {'robot_description': robot_description_config}
 
@@ -234,7 +234,7 @@ def generate_launch_description():
     use_sim_arg = DeclareLaunchArgument(
         use_sim_parameter_name,
         default_value='false',
-        description="Use sim  for simulation".format(
+        description="Use sim for simulation".format(
             use_sim_parameter_name))
 
     gripper_launch_file = IncludeLaunchDescription(

my_panda_moveit_config/launch/rviz.launch.py

@@ -0,0 +1,132 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, Command, FindExecutable
+
+from launch_ros.actions import Node, SetParameter
+from launch_ros.substitutions import FindPackageShare
+import yaml
+
+
+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():
+
+    robot_ip_parameter_name = 'robot_ip'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    load_gripper_parameter_name = 'load_gripper'
+    use_sim_parameter_name = 'use_sim'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
+    use_sim = LaunchConfiguration(use_sim_parameter_name)
+
+    # planning_context
+    franka_xacro_file = os.path.join(get_package_share_directory('my_panda_description'), 'robots',
+                                     'panda_arm.urdf.xacro')
+    robot_description_config = Command(
+        [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper,
+         ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware,
+         ' fake_sensor_commands:=', fake_sensor_commands, ' use_sim:=', use_sim])
+
+    robot_description = {'robot_description': robot_description_config}
+
+    franka_semantic_xacro_file = os.path.join(get_package_share_directory('my_panda_moveit_config'),
+                                              'srdf',
+                                              'panda_arm.srdf.xacro')
+    robot_description_semantic_config = Command(
+        [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=', load_gripper]
+    )
+    robot_description_semantic = {
+        'robot_description_semantic': robot_description_semantic_config
+    }
+
+    kinematics_yaml = load_yaml(
+        'my_panda_moveit_config', 'config/kinematics.yaml'
+    )
+
+    # Planning Functionality
+    ompl_planning_pipeline_config = {
+        'move_group': {
+            'planning_plugin': 'ompl_interface/OMPLPlanner',
+            'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization '
+                                'default_planner_request_adapters/ResolveConstraintFrames '
+                                'default_planner_request_adapters/FixWorkspaceBounds '
+                                'default_planner_request_adapters/FixStartStateBounds '
+                                'default_planner_request_adapters/FixStartStateCollision '
+                                'default_planner_request_adapters/FixStartStatePathConstraints',
+            'start_state_max_bounds_error': 0.1,
+        }
+    }
+    ompl_planning_yaml = load_yaml(
+        'my_panda_moveit_config', 'config/ompl_planning.yaml'
+    )
+    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
+
+    # RViz
+    rviz_base = os.path.join(get_package_share_directory('my_panda_moveit_config'), 'rviz')
+    rviz_full_config = os.path.join(rviz_base, 'moveit.rviz')
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='log',
+        arguments=['-d', rviz_full_config],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            ompl_planning_pipeline_config,
+            kinematics_yaml,
+        ],
+    )
+
+    robot_arg = DeclareLaunchArgument(
+        robot_ip_parameter_name,
+        description='Hostname or IP address of the robot.')
+
+    use_fake_hardware_arg = DeclareLaunchArgument(
+        use_fake_hardware_parameter_name,
+        default_value='false',
+        description='Use fake hardware')
+
+    fake_sensor_commands_arg = DeclareLaunchArgument(
+        fake_sensor_commands_parameter_name,
+        default_value='false',
+        description="Fake sensor commands. Only valid when '{}' is true".format(
+            use_fake_hardware_parameter_name))
+
+    load_gripper_arg = DeclareLaunchArgument(
+        load_gripper_parameter_name,
+        default_value='false',
+        description="Load panda flange gripper".format(
+            load_gripper_parameter_name))
+
+    use_sim_arg = DeclareLaunchArgument(
+        use_sim_parameter_name,
+        default_value='false',
+        description="Use sim for simulation".format(
+            use_sim_parameter_name))
+    actions = [
+        robot_arg,
+        use_fake_hardware_arg,
+        fake_sensor_commands_arg,
+        load_gripper_arg,
+        use_sim_arg,
+        SetParameter(name="use_sim_time", value=use_sim),
+        rviz_node,
+    ]
+
+    return LaunchDescription(actions)