Play sim automatically (#52)

* Play sim automatically

* Accept auto_start as a launch configuration to support previous behavior although default is true

clearpath_gz/launch/gz_sim.launch.py

@@ -19,7 +19,7 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
@@ -32,46 +32,64 @@
                           description='use_sim_time'),
     DeclareLaunchArgument('world', default_value='warehouse',
                           description='Gazebo World'),
+    DeclareLaunchArgument('auto_start', default_value='true',
+                          choices=['true', 'false'],
+                          description='Auto-start Gazebo simulation'),
 ]
 
 
-def generate_launch_description():
+def gz_launch(context, *args, **kwargs):
 
     # Directories
     pkg_clearpath_gz = get_package_share_directory(
         'clearpath_gz')
     pkg_ros_gz_sim = get_package_share_directory(
         'ros_gz_sim')
 
-    # Determine all ros packages that are sourced
-    packages_paths = [os.path.join(p, 'share') for p in os.getenv('AMENT_PREFIX_PATH').split(':')]
-
-    # Set ignition resource path to include all sourced ros packages
-    gz_sim_resource_path = SetEnvironmentVariable(
-        name='IGN_GAZEBO_RESOURCE_PATH',
-        value=[
-            os.path.join(pkg_clearpath_gz, 'worlds'),
-            ':' + ':'.join(packages_paths)])
-
     # Paths
     gz_sim_launch = PathJoinSubstitution(
         [pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'])
 
     gui_config = PathJoinSubstitution(
         [pkg_clearpath_gz, 'config', 'gui.config'])
 
+    auto_start_option = ''
+    auto_start = LaunchConfiguration('auto_start').perform(context)
+    if (auto_start == 'true'):
+        auto_start_option = ' -r'
+
     # Gazebo Simulator
     gz_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([gz_sim_launch]),
         launch_arguments=[
             ('gz_args', [LaunchConfiguration('world'),
                          '.sdf',
+                         auto_start_option,
                          ' -v 4',
                          ' --gui-config ',
                          gui_config])
         ]
     )
 
+    return [gz_sim]
+
+
+def generate_launch_description():
+
+    # Directories
+    pkg_clearpath_gz = get_package_share_directory(
+        'clearpath_gz')
+
+    # Determine all ros packages that are sourced
+    packages_paths = [os.path.join(p, 'share') for p in os.getenv('AMENT_PREFIX_PATH').split(':')]
+
+    # Set ignition resource path to include all sourced ros packages
+    gz_sim_resource_path = SetEnvironmentVariable(
+        name='IGN_GAZEBO_RESOURCE_PATH',
+        value=[
+            os.path.join(pkg_clearpath_gz, 'worlds'),
+            ':' + ':'.join(packages_paths)])
+
     # Clock bridge
     clock_bridge = Node(package='ros_gz_bridge',
                         executable='parameter_bridge',
@@ -84,6 +102,6 @@ def generate_launch_description():
     # Create launch description and add actions
     ld = LaunchDescription(ARGUMENTS)
     ld.add_action(gz_sim_resource_path)
-    ld.add_action(gz_sim)
+    ld.add_action(OpaqueFunction(function=gz_launch))
     ld.add_action(clock_bridge)
     return ld