Added HQ world

andino_isaac/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.8)
+project(andino_isaac)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    launch
+    tools
+    isaac_worlds
+    andino_isaac_description
+  DESTINATION
+    share/${PROJECT_NAME}/
+)
+
+ament_package()

andino_isaac/launch/andino_isaac.launch.py

@@ -0,0 +1,48 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, ExecuteProcess
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+
+def generate_launch_description():
+    # Arguments
+    world_name = LaunchConfiguration('world_name')
+    robot_name = LaunchConfiguration('robot_name')
+    world_name_arg = DeclareLaunchArgument(
+        'world_name',
+        default_value='plain_world.usda',
+        description='Name of the world to launch',
+    )
+    robot_name_arg = DeclareLaunchArgument(
+        'robot_name',
+        default_value='just_andino.usda',
+        description='Name of the robot to spawn',
+    )
+
+    # Paths to places
+    pkg_andino_isaac_path = get_package_share_directory('andino_isaac')
+    user_home_path = os.path.expanduser("~")
+    isaac_install_path = os.path.join(user_home_path, ".local/share/ov/pkg/isaac_sim-2023.1.0")
+    isaac_python_launcher_path = os.path.join(isaac_install_path, "python.sh")
+    isaac_custom_launch_script = os.path.join(pkg_andino_isaac_path, "tools", "isaac_launch_script.py")
+    full_path_to_world = PathJoinSubstitution([pkg_andino_isaac_path, 'isaac_worlds', world_name])
+    full_path_to_robot = PathJoinSubstitution([pkg_andino_isaac_path, 'andino_isaac_description', robot_name])
+
+    # Environment variables
+    prev_ld_library_path = os.environ.get("LD_LIBRARY_PATH", "")
+    ld_library_path_env_var = prev_ld_library_path + ":" + user_home_path + "/.local/share/ov/pkg/isaac_sim-2023.1.0/exts/omni.isaac.ros2_bridge/humble/lib"
+    rmw_implementation_env_var = 'rmw_fastrtps_cpp'
+
+    return LaunchDescription([
+        SetEnvironmentVariable(name='RMW_IMPLEMENTATION', value=rmw_implementation_env_var),
+        SetEnvironmentVariable(name='LD_LIBRARY_PATH', value=ld_library_path_env_var),
+        world_name_arg,
+        robot_name_arg,
+
+        ExecuteProcess(
+            cmd=[isaac_python_launcher_path, ' ', isaac_custom_launch_script, ' --world_file ', full_path_to_world, ' --robot_file ', full_path_to_robot],
+            shell=True,
+            output="screen"
+        )
+    ])

andino_isaac/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>andino_isaac</name>
+  <version>0.1.0</version>
+  <description>The andino_isaac package</description>
+  <maintainer email="[email protected]">Juan Carosella</maintainer>
+  <license file="LICENSE">BSD Clause 3</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>ros2launch</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

andino_isaac/tools/isaac_launch_script.py

@@ -0,0 +1,62 @@
+import argparse
+import carb
+from omni.isaac.kit import SimulationApp
+
+# This sample loads a usd stage and starts simulation
+CONFIG = {"width": 1280, "height": 720, "sync_loads": True, "headless": False, "renderer": "RayTracedLighting"}
+
+# Set up command line arguments
+parser = argparse.ArgumentParser("Simulation loader argument parser")
+parser.add_argument("--world_file", help="Full path to the world file")
+parser.add_argument("--robot_file", help="Full path to the robot file")
+args, unknown = parser.parse_known_args()
+
+# Start the omniverse application
+simulation_app = SimulationApp(launch_config=CONFIG)
+
+# Now that the simulation app is open, continue to load the extension, world and robot
+# These imports need to be here as they depend on SimulationApp
+from omni.isaac.core import World
+from omni.isaac.core.utils.extensions import enable_extension
+from omni.isaac.core.utils.stage import open_stage, is_stage_loading, add_reference_to_stage
+
+# Enable the ros2_bridge extension. Environment variables must be set
+if not enable_extension("omni.isaac.ros2_bridge"):
+	print("ROS2_BRIDGE extension could not be loaded, aborting startup")
+	simulation_app.close()
+
+# Open world
+try:
+	print("Loading world please wait")
+	open_stage(args.world_file)
+	# Wait two frames so that stage starts loading
+	simulation_app.update()
+	simulation_app.update()
+	while is_stage_loading():
+			simulation_app.update()
+	world_settings = {"physics_dt": 1.0 / 60.0, "stage_units_in_meters": 1.0, "rendering_dt": 1.0 / 60.0}
+	world = World(**world_settings)
+	print("World loaded")
+except ValueError:
+	print("Stage could not be loaded, check file path. Aborting startup")
+	simulation_app.close()
+
+# Load robot
+print("Loading robot")
+try:
+	add_reference_to_stage(usd_path=args.robot_file, prim_path="/andino")
+	print("Robot loaded")
+except FileNotFoundError:
+	print("Robot could not be loaded. Check robot file path or load it manually in the simulation")
+
+world.reset()
+
+while simulation_app.is_running():
+    world.step()
+
+    # Deal with pause/stop
+    if world.is_playing():
+        if world.current_time_step_index == 0:
+            world.reset()
+
+simulation_app.close()