Port "Quickstart in RViz" Tutorial

CMakeLists.txt

@@ -62,6 +62,7 @@ include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
 # add_subdirectory(doc/planning)
 # add_subdirectory(doc/planning_scene)
 # add_subdirectory(doc/planning_scene_ros_api)
+add_subdirectory(doc/quickstart_in_rviz)
 # add_subdirectory(doc/robot_model_and_robot_state)
 # add_subdirectory(doc/state_display)
 # add_subdirectory(doc/subframes)

doc/quickstart_in_rviz/CMakeLists.txt

@@ -0,0 +1 @@
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

doc/quickstart_in_rviz/launch/demo.launch.py

@@ -0,0 +1,139 @@
+import os
+import yaml
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+def load_file(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 file.read()
+    except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+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():
+
+    # Command-line arguments
+    tutorial_arg = DeclareLaunchArgument(
+        'rviz_tutorial', default_value='False', description='Tutorial flag')
+
+    # planning_context
+    robot_description_config = load_file('moveit_resources_panda_description', 'urdf/panda.urdf')
+    robot_description = {'robot_description' : robot_description_config}
+
+    robot_description_semantic_config = load_file('moveit_resources_panda_moveit_config', 'config/panda.srdf')
+    robot_description_semantic = {'robot_description_semantic' : robot_description_semantic_config}
+
+    kinematics_yaml = load_yaml('moveit_resources_panda_moveit_config', 'config/kinematics.yaml')
+    robot_description_kinematics = { 'robot_description_kinematics' : 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/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('moveit_resources_panda_moveit_config', 'config/ompl_planning.yaml')
+    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
+
+    # Trajectory Execution Functionality
+    controllers_yaml = load_yaml('run_move_group', 'config/controllers.yaml')
+    moveit_controllers = { 'moveit_simple_controller_manager' : controllers_yaml,
+                           'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'}
+
+    trajectory_execution = {'moveit_manage_controllers': True,
+                            'trajectory_execution.allowed_execution_duration_scaling': 1.2,
+                            'trajectory_execution.allowed_goal_duration_margin': 0.5,
+                            'trajectory_execution.allowed_start_tolerance': 0.01}
+
+    planning_scene_monitor_parameters = {"publish_planning_scene": True,
+                 "publish_geometry_updates": True,
+                 "publish_state_updates": True,
+                 "publish_transforms_updates": True}
+
+    # Start the actual move_group node/action server
+    run_move_group_node = Node(package='moveit_ros_move_group',
+                               executable='move_group',
+                               output='screen',
+                               parameters=[robot_description,
+                                           robot_description_semantic,
+                                           kinematics_yaml,
+                                           ompl_planning_pipeline_config,
+                                           trajectory_execution,
+                                           moveit_controllers,
+                                           planning_scene_monitor_parameters])
+
+    # RViz
+    tutorial_mode = LaunchConfiguration('rviz_tutorial')
+    rviz_base = os.path.join(get_package_share_directory('moveit2_tutorials'), 'launch')
+    rviz_full_config = os.path.join(rviz_base, 'panda_moveit_config_demo.rviz')
+    rviz_empty_config = os.path.join(rviz_base, 'panda_moveit_config_demo_empty.rviz')
+    rviz_node_tutorial = Node(package='rviz2',
+                     executable='rviz2',
+                     name='rviz2',
+                     output='log',
+                     arguments= ['-d', rviz_empty_config],
+                     parameters=[robot_description,
+                                 robot_description_semantic,
+                                 ompl_planning_pipeline_config,
+                                 kinematics_yaml],
+                    condition=IfCondition(tutorial_mode))
+    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],
+                    condition=UnlessCondition(tutorial_mode))
+
+    # Static TF
+    static_tf = Node(package='tf2_ros',
+                     executable='static_transform_publisher',
+                     name='static_transform_publisher',
+                     output='log',
+                     arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'panda_link0'])
+
+    # Publish TF
+    robot_state_publisher = Node(package='robot_state_publisher',
+                                 executable='robot_state_publisher',
+                                 name='robot_state_publisher',
+                                 output='both',
+                                 parameters=[robot_description])
+
+    # Fake joint driver
+    fake_joint_driver_node = Node(package='fake_joint_driver',
+                                  executable='fake_joint_driver_node',
+                                  parameters=[{'controller_name': 'panda_arm_controller'},
+                                              os.path.join(get_package_share_directory("run_move_group"), "config", "panda_controllers.yaml"),
+                                              os.path.join(get_package_share_directory("run_move_group"), "config", "start_positions.yaml"),
+                                              robot_description]
+                                  )
+
+    # Warehouse mongodb server
+    mongodb_server_node = Node(package='warehouse_ros_mongo',
+                               executable='mongo_wrapper_ros.py',
+                               parameters=[{'warehouse_port': 33829},
+                                           {'warehouse_host': 'localhost'},
+                                           {'warehouse_plugin': 'warehouse_ros_mongo::MongoDatabaseConnection'}],
+                               output='screen')
+
+    return LaunchDescription([ tutorial_arg, rviz_node, rviz_node_tutorial, static_tf, robot_state_publisher, run_move_group_node, fake_joint_driver_node, mongodb_server_node ])