OMPL planning working on occasion, race during startup suspected

src/ign_world/launch/moveit_launch.py

@@ -50,10 +50,6 @@ def load_yaml(package_name, file_path):
 
 
 def generate_launch_description():
-    robot_name = "iisy"
-    pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
-    world_path = os.path.join(FindPackageShare(package="gaz_simulation").find("gaz_simulation"), "world/test2.xml")
-    urdf_path = os.path.join(FindPackageShare(package="iisy_config").find("iisy_config"), "urdf/iisy.urdf")
 
     robot_description_config = load_xacro('iisy_config', 'urdf/iisy.urdf')
     robot_description = {'robot_description': robot_description_config}
@@ -64,13 +60,11 @@ def generate_launch_description():
     kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml')
     robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml}
 
-    robot_controllers = PathJoinSubstitution(
-        [
-            FindPackageShare("iisy_config"),
-            "config",
-            "iisy_controllers.yaml",
-        ]
-    )
+    moveit_controllers_yaml = load_yaml("iisy_config", "config/iisy_moveit_controllers.yaml")
+    moveit_controllers = {
+        "moveit_simple_controller_manager": moveit_controllers_yaml,
+        "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
+    }
 
     ompl_planning_pipeline_config = {
         'move_group': {
@@ -91,13 +85,6 @@ def generate_launch_description():
         )
     }
 
-    controllers_yaml = load_yaml('iisy_config', 'config/iisy_moveit_controllers.yml')
-
-    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,
@@ -127,13 +114,6 @@ def generate_launch_description():
         output='screen'
     )
 
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        output='screen',
-        parameters=[robot_description]
-    )
-
     rviz = Node(
         package='rviz2',
         executable='rviz2',
@@ -159,50 +139,21 @@ def generate_launch_description():
 
     pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
 
-    gz_sim = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
-        launch_arguments={'gz_args': '-r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(),
-    )
 
     return LaunchDescription([
         SetParameter(name="use_sim_time",value=True),
 
-        #SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'),
-        #SetEnvironmentVariable(name='DISPLAY', value=':0'),
-
-        #IncludeLaunchDescription(
-        #    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
-        #    launch_arguments={'world': world_path}.items()
-        #),
-
-        gz_sim,
-
-        #Node(package='ros_gz_bridge',
-        #    executable='parameter_bridge',
-        #    name='jointstate_bridge',
-        #    arguments=["/world/default/model/iisy/joint_state"]
-        #),
-
-        #UnsetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE'),
-
-        #Start Gazebo client
-        #IncludeLaunchDescription(
-        #    PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
-        #),
-        
-        #RegisterEventHandler(
-        #    event_handler=OnProcessExit(
-        #        target_action=spawn_robot,
-        #        on_exit=gzclient,
-        #    )
-        #),
-
+        Node(
+            package='ros_actor_action_server',
+            executable='ros_actor_action_server'),
 
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
+            launch_arguments={'gz_args': '-r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(),
+        ),
 
         Node(
             package='tf2_ros',
-            namespace='gaz_simulation',
             executable='static_transform_publisher',
             name='lidar_1_broadcaster',
             arguments=[
@@ -218,7 +169,6 @@ def generate_launch_description():
 
         Node(
             package='tf2_ros',
-            namespace='gaz_simulation',
             executable='static_transform_publisher',
             name='lidar_2_broadcaster',
             arguments=[
@@ -247,13 +197,6 @@ def generate_launch_description():
             ]
         ),
 
-        #Node(
-        #    package="controller_manager",
-        #    executable="ros2_control_node",
-        #    parameters=[robot_description, robot_controllers],
-        #    output="both",
-        #),
-
         Node(
             package='robot_state_publisher',
             executable='robot_state_publisher',
@@ -261,21 +204,11 @@ def generate_launch_description():
             parameters=[robot_description]
         ),
 
-        # Node(
-        #    package='controller_manager',
-        #    name='gazebo_controller_spawner',
-        #    executable='spawner',
-        #    output='screen',
-        #    parameters=[
-        #        controllers_yaml,
-        #    ]
-        # ),
-
         Node(
             package='moveit_ros_move_group',
             executable='move_group',
              # prefix='xterm -fs 10 -e gdb --ex run --args',
-            output='screen',
+            output='both',
             parameters=[
                 robot_description,
                 robot_description_semantic,
@@ -290,7 +223,6 @@ def generate_launch_description():
                 # octomap_updater_config
             ]
         ),
-        #robot_state_publisher,
         spawn_robot,
         rviz,