From 58b51ec33ae7a3268b84c19bdeaeeab634b9980d Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Mon, 31 Jan 2022 11:26:37 +1000
Subject: [PATCH 01/14] Add boilerplate README.md

---
 README.md | 26 ++++++++++++++++++++++++++
 1 file changed, 26 insertions(+)
 create mode 100644 README.md

diff --git a/README.md b/README.md
new file mode 100644
index 0000000..f17b8e3
--- /dev/null
+++ b/README.md
@@ -0,0 +1,26 @@
+**NOTE: this software is part of the BenchBot software stack, and not intended to be run in isolation. For a working BenchBot system, please install the BenchBot software stack by following the instructions [here](https://github.com/qcr/benchbot).**
+
+# BenchBot Simulator for Omniverse powered Isaac Sim
+
+[![BenchBot project](https://img.shields.io/badge/collection-BenchBot-%231a2857)](http://benchbot.org)
+[![QUT Centre for Robotics Open Source](https://github.com/qcr/qcr.github.io/raw/master/misc/badge.svg)](https://qcr.github.io)
+![Primary language](https://img.shields.io/github/languages/top/qcr/benchbot_simulator)
+[![License](https://img.shields.io/github/license/qcr/benchbot_simulator)](./LICENSE.txt)
+
+TODO summary image
+
+TODO description
+
+## Installing the simulator
+
+**Please see the note at the top of the page; installation of this Simulator in isolation is generally not what you want!**
+
+TODO instructions
+
+## Running the simulator
+
+TODO instructions
+
+## Using this simulator with the BenchBot Robot Controller
+
+The [BenchBot Robot Controller](https://github.com/qcr/benchbot_robot_controller) is a wrapping ROS / HTTP hybrid script that manages running robots and their required subprocesses. See the `carter_sim.yaml` configuration in the [BenchBot Supervisor](https://github.com/qcr/benchbot_supervisor) for an example configuration of how to run BenchBot Simulator through the Robot Controller.

From 29ff32930fd71ed449eb36bc3aff50af8dc4d213 Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Mon, 31 Jan 2022 14:06:44 +1000
Subject: [PATCH 02/14] Add basic bash wrapper for running simulator

---
 run | 113 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 113 insertions(+)
 create mode 100755 run

diff --git a/run b/run
new file mode 100755
index 0000000..28e7ed9
--- /dev/null
+++ b/run
@@ -0,0 +1,113 @@
+#!/usr/bin/env bash
+
+################################################################################
+########################### Configuration & helpers ############################
+################################################################################
+
+set -euo pipefail
+IFS=$'\n\t'
+abs_path=$(readlink -f $0)
+pushd $(dirname $abs_path) > /dev/null
+
+DEFAULT_POSE="1,0,0,0,0,0,0"
+
+usage_text="$(basename "$abs_path") -- BenchBot simulator wrapper for Omniverse-powered Isaac Sim
+
+USAGE:
+
+    Run the simulator:
+        $(basename "$abs_path") \
+          --map-path /path/to/map.usd \
+          --robot-path /path/to/robot.usd \
+          --start-pose 1,0,0,0,0,0,0
+        $(basename "$abs_path") -m /path/to/map.usd -r /path/to/robot.usd
+
+    Print this help information:
+        $(basename "$abs_path") [-h|--help]
+
+OPTION DETAILS:
+
+    -h, --help             
+            Show this help menu.
+
+    -m,--map-path
+            Path to the map USD file representing the environment. This flag is
+            required.
+
+    -o,--object-labels
+            Currently unimplemented, but will be added in the future. Will
+            throw an error if this argument is used.
+
+    -r,--robot-path
+            Path to the USD file used for the robot. This robot will be placed
+            at the specified start pose, in the map specified by the USD file
+            at map path. This flag is required.
+
+    -s,--start-pose
+            Start pose as a comma separated 7-tuple of the form:
+            
+                    quat_w,quat_x,quat_y,quat_z,pos_x,pos_y,pos_z
+
+            This flag is optional, with the following default used when it
+            isn't provided:
+
+                    $DEFAULT_POSE
+
+FURTHER DETAILS:
+    
+    Please contact the authors of BenchBot for support or to report bugs:
+        b.talbot@qut.edu.au
+    "
+
+################################################################################
+################################# Main script ##################################
+################################################################################
+
+# Safely parse options
+_args="help,map-path:,object-labels:,robot-path:,start-pose:"
+parse_out=$(getopt -o hm:o:r:s: --long $_args -n "$(basename "$abs_path")" \
+  -- "$@")
+if [ $? != 0 ]; then exit 1; fi
+eval set -- "$parse_out"
+map_path=
+object_labels=
+robot_path=
+start_pose="$DEFAULT_POSE"
+while true; do
+  case "$1" in
+    -h|--help)
+      echo "$usage_text"; exit 0 ;;
+    -m|--map-path)
+      map_path="$2"; shift 2 ;;
+    -o|--object-labels)
+      object_labels="$2"; shift 2 ;;
+    -r|--robot-path)
+      robot_path="$2"; shift 2 ;;
+    -s|--start-pose)
+      start_pose="$2"; shift 2 ;;
+    --)
+      shift ; break ;;
+    *)
+      echo "$(basename "$abs_path"): option '$1' is unknown"; shift ; exit 1 ;;
+  esac 
+done
+
+# Handle error conditions
+if [ -z "$map_path" ]; then
+  printf "ERROR: Path to map USD is required via --map-path option.\n"
+  exit 1
+fi
+if [ -z "$robot_path" ]; then
+  printf "ERROR: Path to robot USD is required via --robot-path option.\n"
+  exit 1
+fi
+if [ ! -z "$object_labels" ]; then
+  printf "ERROR: Object labels option is currently unsupported.\n"
+  exit 1
+fi
+
+# Run our simulator script through Isaac Sim's python.sh Python environment
+echo "$start_pose"
+echo "$map_path"
+echo "$robot_path"
+echo "$object_labels"

From 72a444a7644b814164da317fdee41074bfb5b89a Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Mon, 31 Jan 2022 14:27:38 +1000
Subject: [PATCH 03/14] Add flag for linking with python.sh environment

---
 run | 33 ++++++++++++++++++++++++++++-----
 1 file changed, 28 insertions(+), 5 deletions(-)

diff --git a/run b/run
index 28e7ed9..9bd14d4 100755
--- a/run
+++ b/run
@@ -7,7 +7,6 @@
 set -euo pipefail
 IFS=$'\n\t'
 abs_path=$(readlink -f $0)
-pushd $(dirname $abs_path) > /dev/null
 
 DEFAULT_POSE="1,0,0,0,0,0,0"
 
@@ -38,6 +37,11 @@ OPTION DETAILS:
             Currently unimplemented, but will be added in the future. Will
             throw an error if this argument is used.
 
+    -p,--python-sh-path
+            Path to the 'python.sh' environment script included with your Isaac
+            Sim installation. Will recursively search for the script in the
+            current directory if this flag is not provided.
+
     -r,--robot-path
             Path to the USD file used for the robot. This robot will be placed
             at the specified start pose, in the map specified by the USD file
@@ -71,6 +75,7 @@ if [ $? != 0 ]; then exit 1; fi
 eval set -- "$parse_out"
 map_path=
 object_labels=
+python_sh_path=
 robot_path=
 start_pose="$DEFAULT_POSE"
 while true; do
@@ -81,6 +86,8 @@ while true; do
       map_path="$2"; shift 2 ;;
     -o|--object-labels)
       object_labels="$2"; shift 2 ;;
+    -p|--python-sh-path)
+      python_sh_path="$2"; shift 2 ;;
     -r|--robot-path)
       robot_path="$2"; shift 2 ;;
     -s|--start-pose)
@@ -102,12 +109,28 @@ if [ -z "$robot_path" ]; then
   exit 1
 fi
 if [ ! -z "$object_labels" ]; then
+  # TODO handle object labels
   printf "ERROR: Object labels option is currently unsupported.\n"
   exit 1
 fi
 
+# Derive any missing values
+if [ -z "$python_sh_path" ]; then
+  printf "No python_sh_path provided. Guessing ... \n"
+  python_sh_path="$(find . -name 'python.sh' 2>/dev/null | head -n 1 | xargs \
+    || true)"
+  if [ -z "$python_sh_path" ]; then
+    printf "ERROR: Failed to auto-find python.sh file. Exiting.\n"
+    exit 1
+  fi
+  printf "Guessed: $python_sh_path\n"
+fi
+
 # Run our simulator script through Isaac Sim's python.sh Python environment
-echo "$start_pose"
-echo "$map_path"
-echo "$robot_path"
-echo "$object_labels"
+# NOTE: we have to use environment variables due to bug in how python.sh
+# handles arguments
+# TODO support object labels argument
+BENCHBOT_MAP_PATH="$map_path" \
+  BENCHBOT_ROBOT_PATH="$robot_path" \
+  BENCHBOT_START_POSE="$start_pose" \
+  "$python_sh_path" 

From 9c42e79f3ab81e5efca9ad888eb4582ed8f46598 Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Mon, 31 Jan 2022 14:30:19 +1000
Subject: [PATCH 04/14] Defer to run.py stub for scripted simulation

---
 run    | 2 +-
 run.py | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)
 create mode 100644 run.py

diff --git a/run b/run
index 9bd14d4..0a8d271 100755
--- a/run
+++ b/run
@@ -133,4 +133,4 @@ fi
 BENCHBOT_MAP_PATH="$map_path" \
   BENCHBOT_ROBOT_PATH="$robot_path" \
   BENCHBOT_START_POSE="$start_pose" \
-  "$python_sh_path" 
+  "$python_sh_path" "$(dirname "$abs_path")/run.py"
diff --git a/run.py b/run.py
new file mode 100644
index 0000000..e5bbd6f
--- /dev/null
+++ b/run.py
@@ -0,0 +1 @@
+print("Hello benchbot")

From 011271fd903a72af461f6074e994bee989f0c97c Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Mon, 31 Jan 2022 17:46:41 +1000
Subject: [PATCH 05/14] Expand run.py stub to handle input arguments

---
 run.py | 31 ++++++++++++++++++++++++++++++-
 1 file changed, 30 insertions(+), 1 deletion(-)

diff --git a/run.py b/run.py
index e5bbd6f..8f1a0bd 100644
--- a/run.py
+++ b/run.py
@@ -1 +1,30 @@
-print("Hello benchbot")
+import os
+
+map_path = os.environ.get('BENCHBOT_MAP_PATH')
+robot_path = os.environ.get('BENCHBOT_ROBOT_PATH')
+start_pose = os.environ.get('BENCHBOT_START_POSE')
+
+if __name__ == '__main__':
+    # Handle input arguments (env vars due to python.sh bug)
+    if start_pose != None:
+        start_pose = [float(x) for x in start_pose.split(',')]
+
+    print("Starting Omniverse-powered Isaac Sim simulation with settings:")
+    print("\tmap_path:\t%s" % map_path)
+    print("\trobot_path:\t%s" % robot_path)
+    print("\tstart_pose:\t%s" % start_pose)
+
+    err = False
+    if any(x is None for x in [map_path, robot_path, start_pose]):
+        print("ERROR: one of map_path, robot_path, or start_pose was none.")
+        err = True
+    if map_path is None or not os.path.exists(map_path):
+        print("ERROR: map_path '%s' does not exist." % map_path)
+        err = True
+    if robot_path is None or not os.path.exists(robot_path):
+        print("ERROR: robot_path '%s' does not exist." % robot_path)
+        err = True
+    if err:
+        quit()
+
+    # Start the simulator

From 256182f04fe0f27c3e18f8a0a4e2214e0f8fe580 Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Mon, 31 Jan 2022 18:13:08 +1000
Subject: [PATCH 06/14] Port simulation-from-code example to run.py

---
 run.py | 77 ++++++++++++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 69 insertions(+), 8 deletions(-)

diff --git a/run.py b/run.py
index 8f1a0bd..4b990d9 100644
--- a/run.py
+++ b/run.py
@@ -1,30 +1,91 @@
+import numpy as np
 import os
+import signal
+import time
+
+from omni.isaac.kit import SimulationApp
+
+ROBOT_NAME = 'robot'
+ROBOT_PRIM_PATH = '/%s' % ROBOT_NAME
+UPDATE_DELAY_SECS = 3.0
 
 map_path = os.environ.get('BENCHBOT_MAP_PATH')
 robot_path = os.environ.get('BENCHBOT_ROBOT_PATH')
 start_pose = os.environ.get('BENCHBOT_START_POSE')
 
+done = False
+
+
+def sig_handler(_, __):
+    global done
+    done = True
+
+
 if __name__ == '__main__':
     # Handle input arguments (env vars due to python.sh bug)
     if start_pose != None:
-        start_pose = [float(x) for x in start_pose.split(',')]
+        start_pose = np.array([float(x) for x in start_pose.split(',')])
 
     print("Starting Omniverse-powered Isaac Sim simulation with settings:")
     print("\tmap_path:\t%s" % map_path)
     print("\trobot_path:\t%s" % robot_path)
     print("\tstart_pose:\t%s" % start_pose)
 
-    err = False
-    if any(x is None for x in [map_path, robot_path, start_pose]):
-        print("ERROR: one of map_path, robot_path, or start_pose was none.")
-        err = True
     if map_path is None or not os.path.exists(map_path):
         print("ERROR: map_path '%s' does not exist." % map_path)
-        err = True
+        quit()
     if robot_path is None or not os.path.exists(robot_path):
         print("ERROR: robot_path '%s' does not exist." % robot_path)
-        err = True
-    if err:
+        quit()
+    if start_pose is None or len(start_pose) != 7:
+        print("ERROR: Start pose is not 7 comma-separated values.")
         quit()
 
     # Start the simulator
+    signal.signal(signal.SIGINT, sig_handler)
+    k = SimulationApp({
+        "renderer": "RayTracedLighting",
+        "headless": False,
+        "open_usd": map_path
+    })
+
+    # Import all required modules, and configure application
+    from omni.isaac.core import SimulationContext
+    from omni.isaac.core.robots import Robot
+    from omni.isaac.core.utils.extensions import enable_extension
+    from omni.isaac.core.utils.stage import add_reference_to_stage
+    from omni.kit.commands import execute
+    from omni.kit.viewport import get_default_viewport_window
+    enable_extension("omni.isaac.ros_bridge")
+
+    # Configure the interface
+    vp = get_default_viewport_window()
+    # vp.set_active_camera('/%s/carter_view' % ROBOT_PRIM_PATH)  seems to get overridden??
+
+    # Add robot to the environment at the requested pose
+    add_reference_to_stage(usd_path=robot_path, prim_path=ROBOT_PRIM_PATH)
+    r = Robot(prim_path=ROBOT_PRIM_PATH, name=ROBOT_NAME)
+    r.set_world_pose(position=start_pose[4::] * 100,
+                     orientation=start_pose[:4])
+
+    # Random number of updates for UI to catch up with things???
+    a = time.time()
+    while (time.time() - a < UPDATE_DELAY_SECS):
+        k.update()
+
+    # Wait until we find a ROS master
+    # TODO dynamically incorporate address to look for master...
+    print("Waiting for ROS master on TODO ...")
+    chk = False
+    while not chk:
+        time.sleep(1)
+        _, chk = execute("RosBridgeRosMasterCheck")
+    print("Found ROS master.")
+
+    # Start the simulation, quitting on SIGINT
+    sc = SimulationContext()
+    sc.play()
+    while not done:
+        sc.step()
+    sc.stop()
+    k.close()

From 2adc2c854b210027b2c71829c8fc2a79598a0cd3 Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Mon, 31 Jan 2022 18:18:27 +1000
Subject: [PATCH 07/14] Increase verbosity of run.py output

---
 run.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/run.py b/run.py
index 4b990d9..cf06cc0 100644
--- a/run.py
+++ b/run.py
@@ -75,17 +75,19 @@ def sig_handler(_, __):
 
     # Wait until we find a ROS master
     # TODO dynamically incorporate address to look for master...
-    print("Waiting for ROS master on TODO ...")
+    print("BENCHBOT: Waiting for ROS master on TODO ...")
     chk = False
     while not chk:
         time.sleep(1)
         _, chk = execute("RosBridgeRosMasterCheck")
-    print("Found ROS master.")
+    print("BENCHBOT: Found ROS master.")
 
     # Start the simulation, quitting on SIGINT
     sc = SimulationContext()
     sc.play()
+    print("BENCHBOT: Running simulation ...")
     while not done:
         sc.step()
+    print("BENCHBOT: Exit requested. Finishing ...")
     sc.stop()
     k.close()

From 743203e0ece6eb4132c5038d56d3b3fd610ee0de Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Mon, 31 Jan 2022 18:34:26 +1000
Subject: [PATCH 08/14] Tidy up SIGINT handling & exiting in run.py

---
 run.py | 18 ++++++++----------
 1 file changed, 8 insertions(+), 10 deletions(-)

diff --git a/run.py b/run.py
index cf06cc0..f7e601d 100644
--- a/run.py
+++ b/run.py
@@ -1,6 +1,7 @@
 import numpy as np
 import os
 import signal
+import sys
 import time
 
 from omni.isaac.kit import SimulationApp
@@ -13,12 +14,12 @@
 robot_path = os.environ.get('BENCHBOT_ROBOT_PATH')
 start_pose = os.environ.get('BENCHBOT_START_POSE')
 
-done = False
 
-
-def sig_handler(_, __):
-    global done
-    done = True
+def finish(sim_context, kit):
+    print("BENCHBOT: Exit requested. Finishing ...")
+    sim_context.stop()
+    kit.close()
+    sys.exit()  # TODO figure out why this seg faults
 
 
 if __name__ == '__main__':
@@ -42,7 +43,6 @@ def sig_handler(_, __):
         quit()
 
     # Start the simulator
-    signal.signal(signal.SIGINT, sig_handler)
     k = SimulationApp({
         "renderer": "RayTracedLighting",
         "headless": False,
@@ -86,8 +86,6 @@ def sig_handler(_, __):
     sc = SimulationContext()
     sc.play()
     print("BENCHBOT: Running simulation ...")
-    while not done:
+    signal.signal(signal.SIGINT, lambda _, __: finish(sc, k))
+    while True:
         sc.step()
-    print("BENCHBOT: Exit requested. Finishing ...")
-    sc.stop()
-    k.close()

From b85694bc7b03575d53cbfb78b3544ce6a40b5217 Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Fri, 4 Feb 2022 10:22:43 +1000
Subject: [PATCH 09/14] Fix badge URLs

---
 README.md | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/README.md b/README.md
index f17b8e3..1815c52 100644
--- a/README.md
+++ b/README.md
@@ -4,8 +4,8 @@
 
 [![BenchBot project](https://img.shields.io/badge/collection-BenchBot-%231a2857)](http://benchbot.org)
 [![QUT Centre for Robotics Open Source](https://github.com/qcr/qcr.github.io/raw/master/misc/badge.svg)](https://qcr.github.io)
-![Primary language](https://img.shields.io/github/languages/top/qcr/benchbot_simulator)
-[![License](https://img.shields.io/github/license/qcr/benchbot_simulator)](./LICENSE.txt)
+![Primary language](https://img.shields.io/github/languages/top/qcr/benchbot_sim_omni)
+[![License](https://img.shields.io/github/license/qcr/benchbot_sim_omni)](./LICENSE.txt)
 
 TODO summary image
 

From 59e7f59e74a1789b264912a02f135a6fef63ad6e Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Thu, 10 Feb 2022 21:17:01 +1000
Subject: [PATCH 10/14] Use dumb proxy of depth-first search for python.sh

---
 run | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/run b/run
index 0a8d271..9ac781e 100755
--- a/run
+++ b/run
@@ -117,8 +117,9 @@ fi
 # Derive any missing values
 if [ -z "$python_sh_path" ]; then
   printf "No python_sh_path provided. Guessing ... \n"
-  python_sh_path="$(find . -name 'python.sh' 2>/dev/null | head -n 1 | xargs \
-    || true)"
+  python_sh_path="$(find . -name 'python.sh' 2>/dev/null | \
+    awk '{ print length, $0 }' | sort -n | cut -d' ' -f2 | \
+    head -n 1 | xargs || true)"
   if [ -z "$python_sh_path" ]; then
     printf "ERROR: Failed to auto-find python.sh file. Exiting.\n"
     exit 1

From 5fab2c58332e276c06db266a1585330531c0d1be Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Fri, 11 Feb 2022 09:05:07 +1000
Subject: [PATCH 11/14] Disable ROS components before starting sim

---
 run.py | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/run.py b/run.py
index f7e601d..bb947d9 100644
--- a/run.py
+++ b/run.py
@@ -8,6 +8,14 @@
 
 ROBOT_NAME = 'robot'
 ROBOT_PRIM_PATH = '/%s' % ROBOT_NAME
+ROBOT_COMPONENTS = {
+    'clock': '/ROS_Clock',
+    'diff_base': '/%s/ROS_DifferentialBase' % ROBOT_PRIM_PATH,
+    'lidar': '/%s/ROS_Lidar' % ROBOT_PRIM_PATH,
+    'rgbd': '/%s/ROS_Camera_Stereo_Left' % ROBOT_PRIM_PATH,
+    'tf_sensors': '/%s/ROS_Carter_Sensors_Broadcaster' % ROBOT_PRIM_PATH,
+    'tf': '/%s/ROS_Carter_Broadcaster' % ROBOT_PRIM_PATH
+}
 UPDATE_DELAY_SECS = 3.0
 
 map_path = os.environ.get('BENCHBOT_MAP_PATH')
@@ -22,6 +30,17 @@ def finish(sim_context, kit):
     sys.exit()  # TODO figure out why this seg faults
 
 
+def disable_component(prop_path):
+    execute("ChangeProperty",
+            prop_path=Sdf.Path("%s.enabled" % prop_path),
+            value=False,
+            prev=None)
+
+
+def tick_component(prop_path):
+    execute("RosBridgeTickComponent", path=prop_path)
+
+
 if __name__ == '__main__':
     # Handle input arguments (env vars due to python.sh bug)
     if start_pose != None:
@@ -56,6 +75,7 @@ def finish(sim_context, kit):
     from omni.isaac.core.utils.stage import add_reference_to_stage
     from omni.kit.commands import execute
     from omni.kit.viewport import get_default_viewport_window
+    from pxr import Sdf
     enable_extension("omni.isaac.ros_bridge")
 
     # Configure the interface
@@ -68,6 +88,11 @@ def finish(sim_context, kit):
     r.set_world_pose(position=start_pose[4::] * 100,
                      orientation=start_pose[:4])
 
+    # Disable auto-publishing of all robot components (we'll manually publish
+    # at varying frequencies instead)
+    for p in ROBOT_COMPONENTS.values():
+        disable_component(p)
+
     # Random number of updates for UI to catch up with things???
     a = time.time()
     while (time.time() - a < UPDATE_DELAY_SECS):

From f9a3b2d07c0fc6ec742f62499e0f9e3295099c59 Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Fri, 11 Feb 2022 09:40:04 +1000
Subject: [PATCH 12/14] Fix double slash in component paths

---
 run.py | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/run.py b/run.py
index bb947d9..c2d4f51 100644
--- a/run.py
+++ b/run.py
@@ -10,11 +10,11 @@
 ROBOT_PRIM_PATH = '/%s' % ROBOT_NAME
 ROBOT_COMPONENTS = {
     'clock': '/ROS_Clock',
-    'diff_base': '/%s/ROS_DifferentialBase' % ROBOT_PRIM_PATH,
-    'lidar': '/%s/ROS_Lidar' % ROBOT_PRIM_PATH,
-    'rgbd': '/%s/ROS_Camera_Stereo_Left' % ROBOT_PRIM_PATH,
-    'tf_sensors': '/%s/ROS_Carter_Sensors_Broadcaster' % ROBOT_PRIM_PATH,
-    'tf': '/%s/ROS_Carter_Broadcaster' % ROBOT_PRIM_PATH
+    'diff_base': '%s/ROS_DifferentialBase' % ROBOT_PRIM_PATH,
+    'lidar': '%s/ROS_Lidar' % ROBOT_PRIM_PATH,
+    'rgbd': '%s/ROS_Camera_Stereo_Left' % ROBOT_PRIM_PATH,
+    'tf_sensors': '%s/ROS_Carter_Sensors_Broadcaster' % ROBOT_PRIM_PATH,
+    'tf': '%s/ROS_Carter_Broadcaster' % ROBOT_PRIM_PATH
 }
 UPDATE_DELAY_SECS = 3.0
 
@@ -31,6 +31,7 @@ def finish(sim_context, kit):
 
 
 def disable_component(prop_path):
+    print("DISABLING '%s.enabled'" % prop_path)
     execute("ChangeProperty",
             prop_path=Sdf.Path("%s.enabled" % prop_path),
             value=False,

From 36dcb1d8c02f6de5f68eccf1ba300e3c18c9bc31 Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Fri, 11 Feb 2022 11:08:56 +1000
Subject: [PATCH 13/14] Slow down publish rate of high-bandwidth sensors

---
 run.py | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/run.py b/run.py
index c2d4f51..120c6cf 100644
--- a/run.py
+++ b/run.py
@@ -113,5 +113,22 @@ def tick_component(prop_path):
     sc.play()
     print("BENCHBOT: Running simulation ...")
     signal.signal(signal.SIGINT, lambda _, __: finish(sc, k))
+    i = 0
     while True:
         sc.step()
+
+        # Tick at 60Hz
+        tick_component(ROBOT_COMPONENTS['clock'])
+
+        # Tick at 30Hz
+        if i % 2 == 0:
+            tick_component(ROBOT_COMPONENTS['diff_base'])
+            tick_component(ROBOT_COMPONENTS['lidar'])
+            tick_component(ROBOT_COMPONENTS['tf'])
+            tick_component(ROBOT_COMPONENTS['tf_sensors'])
+
+        # Tick at 10Hz
+        if i % 6 == 0:
+            tick_component(ROBOT_COMPONENTS['rgbd'])
+
+        i += 1

From b88dab636675d96de270fc254b9854e81abc076a Mon Sep 17 00:00:00 2001
From: Ben Talbot <b.talbot@qut.edu.au>
Date: Tue, 15 Feb 2022 19:25:58 +1000
Subject: [PATCH 14/14] Add support for square brackets around start_pose

---
 run | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/run b/run
index 9ac781e..cd3c3b3 100755
--- a/run
+++ b/run
@@ -57,6 +57,9 @@ OPTION DETAILS:
 
                     $DEFAULT_POSE
 
+            Note: starting or closing square brackets are also supported, they
+            will simply be stripped from the input.
+
 FURTHER DETAILS:
     
     Please contact the authors of BenchBot for support or to report bugs:
@@ -114,6 +117,9 @@ if [ ! -z "$object_labels" ]; then
   exit 1
 fi
 
+# Make available corrections to input arguments
+start_pose="$(echo "$start_pose" | sed 's/ //g; s/^\[//; s/\]$//')"
+
 # Derive any missing values
 if [ -z "$python_sh_path" ]; then
   printf "No python_sh_path provided. Guessing ... \n"