diff --git a/README.md b/README.md new file mode 100644 index 0000000..1815c52 --- /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_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. diff --git a/run b/run new file mode 100755 index 0000000..cd3c3b3 --- /dev/null +++ b/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" diff --git a/run.py b/run.py new file mode 100644 index 0000000..120c6cf --- /dev/null +++ b/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