Merge branch 'develop'

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_sim_omni)
+[![License](https://img.shields.io/github/license/qcr/benchbot_sim_omni)](./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.

run

@@ -0,0 +1,143 @@
+#!/usr/bin/env bash
+
+################################################################################
+########################### Configuration & helpers ############################
+################################################################################
+
+set -euo pipefail
+IFS=$'\n\t'
+abs_path=$(readlink -f $0)
+
+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.
+
+    -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
+            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
+
+            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:
+        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=
+python_sh_path=
+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 ;;
+    -p|--python-sh-path)
+      python_sh_path="$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
+  # TODO handle object labels
+  printf "ERROR: Object labels option is currently unsupported.\n"
+  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"
+  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
+  fi
+  printf "Guessed: $python_sh_path\n"
+fi
+
+# Run our simulator script through Isaac Sim's python.sh Python environment
+# 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" "$(dirname "$abs_path")/run.py"

run.py

@@ -0,0 +1,134 @@
+import numpy as np
+import os
+import signal
+import sys
+import time
+
+from omni.isaac.kit import SimulationApp
+
+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')
+robot_path = os.environ.get('BENCHBOT_ROBOT_PATH')
+start_pose = os.environ.get('BENCHBOT_START_POSE')
+
+
+def finish(sim_context, kit):
+    print("BENCHBOT: Exit requested. Finishing ...")
+    sim_context.stop()
+    kit.close()
+    sys.exit()  # TODO figure out why this seg faults
+
+
+def disable_component(prop_path):
+    print("DISABLING '%s.enabled'" % 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:
+        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)
+
+    if map_path is None or not os.path.exists(map_path):
+        print("ERROR: map_path '%s' does not exist." % map_path)
+        quit()
+    if robot_path is None or not os.path.exists(robot_path):
+        print("ERROR: robot_path '%s' does not exist." % robot_path)
+        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
+    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
+    from pxr import Sdf
+    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])
+
+    # 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):
+        k.update()
+
+    # Wait until we find a ROS master
+    # TODO dynamically incorporate address to look for master...
+    print("BENCHBOT: Waiting for ROS master on TODO ...")
+    chk = False
+    while not chk:
+        time.sleep(1)
+        _, chk = execute("RosBridgeRosMasterCheck")
+    print("BENCHBOT: Found ROS master.")
+
+    # Start the simulation, quitting on SIGINT
+    sc = SimulationContext()
+    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