From 7229891e8f2cc91fe9f1fb0298efa08425c21119 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Sat, 12 Oct 2024 02:16:23 +0800
Subject: [PATCH] Adds the ability to set the update rate of the simulation
 (#264)

* Adds the ability to set the update rate of the simulation

This commit adds the ability to set the update rate of simulation to be
faster than real time via the `update_rate` option.

For instance if you want a 5x real time factor simulation running via
CLI simply add the flags:

```
ros2 launch rmf_demos_gz office.launch.xml update_rate:=5000
```
Note by default this sets gazebo's internal step size. Usually we have
1000 steps per second of simulation, hence if we want 5x RTF we need to
set update_rate:=5000.

Signed-off-by: Arjo Chakravarty <arjoc@google.com>

* Rename parameter and fix update rate

Signed-off-by: Arjo Chakravarty <arjoc@google.com>

---------

Signed-off-by: Arjo Chakravarty <arjoc@google.com>
---
 rmf_demos_gz/launch/airport_terminal.launch.xml          | 2 ++
 rmf_demos_gz/launch/battle_royale.launch.xml             | 2 ++
 rmf_demos_gz/launch/campus.launch.xml                    | 2 ++
 rmf_demos_gz/launch/clinic.launch.xml                    | 2 ++
 rmf_demos_gz/launch/hotel.launch.xml                     | 2 ++
 rmf_demos_gz/launch/office.launch.xml                    | 2 ++
 rmf_demos_gz/launch/office_mock_traffic_light.launch.xml | 1 +
 rmf_demos_gz/launch/simulation.launch.xml                | 3 ++-
 rmf_demos_gz/launch/triple_H.launch.xml                  | 2 ++
 9 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/rmf_demos_gz/launch/airport_terminal.launch.xml b/rmf_demos_gz/launch/airport_terminal.launch.xml
index 319882e8..48a9eaaa 100644
--- a/rmf_demos_gz/launch/airport_terminal.launch.xml
+++ b/rmf_demos_gz/launch/airport_terminal.launch.xml
@@ -4,6 +4,7 @@
   <arg name="use_sim_time" default="true"/>
   <arg name="failover_mode" default="false"/>
   <arg name="gazebo_version" default='8'/>
+  <arg name="sim_update_rate" default='100'/>
 
   <!-- Common launch -->
   <include file="$(find-pkg-share rmf_demos)/airport_terminal.launch.xml">
@@ -15,6 +16,7 @@
   <include file="$(find-pkg-share rmf_demos_gz)/simulation.launch.xml">
     <arg name="map_name" value="airport_terminal" />
     <arg name="gazebo_version" value="$(var gazebo_version)" />
+    <arg name="sim_update_rate" value="$(var sim_update_rate)"/>
   </include>
 
 </launch>
diff --git a/rmf_demos_gz/launch/battle_royale.launch.xml b/rmf_demos_gz/launch/battle_royale.launch.xml
index f96c8d55..62d3d165 100644
--- a/rmf_demos_gz/launch/battle_royale.launch.xml
+++ b/rmf_demos_gz/launch/battle_royale.launch.xml
@@ -4,6 +4,7 @@
   <arg name="use_sim_time" default="true"/>
   <arg name="gazebo_version" default='8'/>
   <arg name="use_traffic_light" default="false"/>
+  <arg name="sim_update_rate" default='100'/>
 
   <!-- Common launch -->
   <include file="$(find-pkg-share rmf_demos)/battle_royale.launch.xml">
@@ -15,6 +16,7 @@
   <include file="$(find-pkg-share rmf_demos_gz)/simulation.launch.xml">
     <arg name="map_name" value="battle_royale" />
     <arg name="gazebo_version" value="$(var gazebo_version)" />
+    <arg name="sim_update_rate" value="$(var sim_update_rate)"/>
   </include>
 
 </launch>
diff --git a/rmf_demos_gz/launch/campus.launch.xml b/rmf_demos_gz/launch/campus.launch.xml
index ba140a6c..a759621c 100644
--- a/rmf_demos_gz/launch/campus.launch.xml
+++ b/rmf_demos_gz/launch/campus.launch.xml
@@ -4,6 +4,7 @@
   <arg name="use_sim_time" default="true"/>
   <arg name="failover_mode" default="false"/>
   <arg name="gazebo_version" default='8'/>
+  <arg name="sim_update_rate" default='100'/>
 
   <!-- Common launch -->
   <include file="$(find-pkg-share rmf_demos)/campus.launch.xml">
@@ -15,6 +16,7 @@
   <include file="$(find-pkg-share rmf_demos_gz)/simulation.launch.xml">
     <arg name="map_name" value="campus" />
     <arg name="gazebo_version" value="$(var gazebo_version)" />
+    <arg name="sim_update_rate" value="$(var sim_update_rate)"/>
   </include>
 
 </launch>
diff --git a/rmf_demos_gz/launch/clinic.launch.xml b/rmf_demos_gz/launch/clinic.launch.xml
index 59dc73e0..ae7cbe5a 100644
--- a/rmf_demos_gz/launch/clinic.launch.xml
+++ b/rmf_demos_gz/launch/clinic.launch.xml
@@ -4,6 +4,7 @@
   <arg name="use_sim_time" default="true"/>
   <arg name="failover_mode" default="false"/>
   <arg name="gazebo_version" default='8'/>
+  <arg name="sim_update_rate" default='100'/>
 
   <!-- Common launch -->
   <include file="$(find-pkg-share rmf_demos)/clinic.launch.xml">
@@ -15,6 +16,7 @@
   <include file="$(find-pkg-share rmf_demos_gz)/simulation.launch.xml">
     <arg name="map_name" value="clinic" />
     <arg name="gazebo_version" value="$(var gazebo_version)" />
+    <arg name="sim_update_rate" value="$(var sim_update_rate)"/>
   </include>
 
 </launch>
diff --git a/rmf_demos_gz/launch/hotel.launch.xml b/rmf_demos_gz/launch/hotel.launch.xml
index 193fa0d2..507e8956 100644
--- a/rmf_demos_gz/launch/hotel.launch.xml
+++ b/rmf_demos_gz/launch/hotel.launch.xml
@@ -4,6 +4,7 @@
   <arg name="use_sim_time" default="true"/>
   <arg name="failover_mode" default="false"/>
   <arg name="gazebo_version" default='8'/>
+  <arg name="sim_update_rate" default='100'/>
 
   <!-- Common launch -->
   <include file="$(find-pkg-share rmf_demos)/hotel.launch.xml">
@@ -15,5 +16,6 @@
   <include file="$(find-pkg-share rmf_demos_gz)/simulation.launch.xml">
     <arg name="map_name" value="hotel" />
     <arg name="gazebo_version" value="$(var gazebo_version)" />
+    <arg name="sim_update_rate" value="$(var sim_update_rate)"/>
   </include>
 </launch>
diff --git a/rmf_demos_gz/launch/office.launch.xml b/rmf_demos_gz/launch/office.launch.xml
index 73e38a7a..fab9c832 100644
--- a/rmf_demos_gz/launch/office.launch.xml
+++ b/rmf_demos_gz/launch/office.launch.xml
@@ -4,6 +4,7 @@
   <arg name="use_sim_time" default="true"/>
   <arg name="failover_mode" default="false"/>
   <arg name="gazebo_version" default='8'/>
+  <arg name="sim_update_rate" default='100'/>
 
   <!-- Common launch -->
   <include file="$(find-pkg-share rmf_demos)/office.launch.xml">
@@ -15,6 +16,7 @@
   <include file="$(find-pkg-share rmf_demos_gz)/simulation.launch.xml">
     <arg name="map_name" value="office" />
     <arg name="gazebo_version" value="$(var gazebo_version)" />
+    <arg name="sim_update_rate" value="$(var sim_update_rate)"/>
   </include>
 
 </launch>
diff --git a/rmf_demos_gz/launch/office_mock_traffic_light.launch.xml b/rmf_demos_gz/launch/office_mock_traffic_light.launch.xml
index 18200d87..07663f16 100644
--- a/rmf_demos_gz/launch/office_mock_traffic_light.launch.xml
+++ b/rmf_demos_gz/launch/office_mock_traffic_light.launch.xml
@@ -3,6 +3,7 @@
 <launch>
   <arg name="use_sim_time" default="true"/>
   <arg name="gazebo_version" default='8'/>
+  <arg name="sim_update_rate" default='100'/>
 
   <!-- Common launch -->
   <include file="$(find-pkg-share rmf_demos)/office_mock_traffic_light.launch.xml">
diff --git a/rmf_demos_gz/launch/simulation.launch.xml b/rmf_demos_gz/launch/simulation.launch.xml
index 8a017450..45c59056 100644
--- a/rmf_demos_gz/launch/simulation.launch.xml
+++ b/rmf_demos_gz/launch/simulation.launch.xml
@@ -5,6 +5,7 @@
   <arg name="map_name" description="Name of the rmf_demos map to simulate" />
   <arg name="use_crowdsim" default='0'/>
   <arg name="gazebo_version" default='8'/>
+  <arg name="sim_update_rate" default='100'/>
 
   <let name="world_path" value="$(find-pkg-share $(var map_package))/maps/$(var map_name)/$(var map_name).world" />
   <let name="model_path" value="$(find-pkg-share $(var map_package))/maps/$(var map_name)/models:$(env HOME)/.gazebo/models" />
@@ -17,7 +18,7 @@
   <let name="gz_headless" unless="$(var headless)" value="" />
 
   <!-- TODO(luca) Remove the manual concatenation of GZ_SIM_RESOURCE_PATH and just use environment hooks -->
-  <executable cmd="gz sim --force-version $(var gazebo_version) $(var gz_headless) -r -v 3 $(var world_path)" output="both">
+  <executable cmd="gz sim --force-version $(var gazebo_version) $(var gz_headless) -r -v 3 $(var world_path) -z $(var sim_update_rate)" output="both">
     <env name="GZ_SIM_RESOURCE_PATH" value="$(env GZ_SIM_RESOURCE_PATH):$(var model_path)" />
     <env name="MENGE_RESOURCE_PATH" value="$(var menge_resource_path)"/>
   </executable>
diff --git a/rmf_demos_gz/launch/triple_H.launch.xml b/rmf_demos_gz/launch/triple_H.launch.xml
index 9e462458..f0d99d6f 100644
--- a/rmf_demos_gz/launch/triple_H.launch.xml
+++ b/rmf_demos_gz/launch/triple_H.launch.xml
@@ -3,6 +3,7 @@
 <launch>
   <arg name="use_sim_time" default="true"/>
   <arg name="gazebo_version" default='8'/>
+  <arg name="sim_update_rate" default='100'/>
 
   <!-- Common launch -->
   <include file="$(find-pkg-share rmf_demos)/triple_H.launch.xml">
@@ -13,6 +14,7 @@
   <include file="$(find-pkg-share rmf_demos_gz)/simulation.launch.xml">
     <arg name="map_name" value="triple_H" />
     <arg name="gazebo_version" value="$(var gazebo_version)" />
+    <arg name="sim_update_rate" value="$(var sim_update_rate)"/>
   </include>
 
 </launch>