Refactor gzserver launch args, add --initial_sim_time

gazebo_ros/launch/gzserver.launch.py

@@ -39,27 +39,35 @@ def generate_launch_description():
         _boolean_command('lockstep'),
         _boolean_command('help'),
         _boolean_command('pause'),
-        _arg_command('physics'), LaunchConfiguration('physics'),
-        _arg_command('play'), LaunchConfiguration('play'),
+        # join with '=' (--initial_sim_time=[time]) so that old versions of
+        # gazebo will parse it all as a single argument and ignore the [time].
+        _arg_command('initial_sim_time', join_with='='),
+        _arg_command('physics'),
+        _arg_command('play'),
         _boolean_command('record'),
-        _arg_command('record_encoding'), LaunchConfiguration('record_encoding'),
-        _arg_command('record_path'), LaunchConfiguration('record_path'),
-        _arg_command('record_period'), LaunchConfiguration('record_period'),
-        _arg_command('record_filter'), LaunchConfiguration('record_filter'),
-        _arg_command('seed'), LaunchConfiguration('seed'),
-        _arg_command('iters'), LaunchConfiguration('iters'),
+        _arg_command('record_encoding'),
+        _arg_command('record_path'),
+        _arg_command('record_period'),
+        _arg_command('record_filter'),
+        _arg_command('seed'),
+        _arg_command('iters'),
         _boolean_command('minimal_comms'),
         _plugin_command('init'),
         _plugin_command('factory'),
         _plugin_command('force_system'),
         # Wait for (https://github.com/ros-simulation/gazebo_ros_pkgs/pull/941)
         # _plugin_command('force_system'), ' ',
-        _arg_command('profile'), LaunchConfiguration('profile'),
-        # convenience parameter for params file
-        _arg_command('ros-args', condition=LaunchConfiguration('params_file')),
-        _arg_command('params-file', condition=LaunchConfiguration('params_file')),
+        _arg_command('profile'),
+        # Support a yaml params_file:
+        #   If a params file is to be used, it needs to be preceded by --ros-args
+        #   and followed by a trailing --
+        # This provides the leading --ros-args if a params_file is specified.
+        _conditional_command('ros-args', LaunchConfiguration('params_file')),
+        # These two lines provide the --params-file [params_file] arguments
+        _conditional_command('params-file', LaunchConfiguration('params_file')),
         LaunchConfiguration('params_file'),
-        _arg_command('', condition=LaunchConfiguration('params_file')),
+        # This provides the trailing -- if a params_file is specified.
+        _conditional_command('', LaunchConfiguration('params_file')),
         LaunchConfiguration('extra_gazebo_args'),
     ]
 
@@ -107,6 +115,10 @@ def generate_launch_description():
             'help', default_value='false',
             description='Set "true" to produce gzserver help message.'
         ),
+        DeclareLaunchArgument(
+            'initial_sim_time', default_value='',
+            description='Specify the initial simulation time (seconds).'
+        ),
         DeclareLaunchArgument(
             'pause', default_value='false',
             description='Set "true" to start the server in a paused state.'
@@ -220,12 +232,16 @@ def _boolean_command(arg):
     return py_cmd
 
 
-# Add string commands if not empty
-def _arg_command(arg, condition=None):
-    if condition:
-        cmd = ['"--', arg, '" if "" != "', condition, '" else ""']
-    else:
-        cmd = ['"--', arg, '" if "" != "', LaunchConfiguration(arg), '" else ""']
+# Add string command and argument if not empty
+def _arg_command(arg, join_with=" "):
+    cmd = ['"--', arg, join_with, '" if "" != "', LaunchConfiguration(arg), '" else ""']
+    py_cmd = PythonExpression(cmd)
+    return (py_cmd, LaunchConfiguration(arg))
+
+
+# Add --command if condition not empty
+def _conditional_command(arg, condition):
+    cmd = ['"--', arg, '" if "" != "', condition, '" else ""']
     py_cmd = PythonExpression(cmd)
     return py_cmd