Simulation Interfaces

Simulation Interfaces Test Script

In this module, we will analyze a script that leverages service interfaces to orchestrate a multi-robot workflow. We will walk through the code in detail, uncommenting sections as we go, then finally run it.

Setup

1. In Isaac Sim, open the warehouse test scene from the course assets folder: Starting_Point/warehouse_test_scene.usd

2. In your code-editor, open the script in /sim_control_script_and_carter_ws/learning_simulation_control/workshop_learning_script.py

The carter1_sequence

We are now working on the carter1_sequence function on our workshop script.

This sequence includes the following steps:

1. Drive the Nav2-controlled Carter to the table near a cube.

2. Spawn and position a Limo pusher if needed.

3. Limo “nudges” the cube off the table.

4. Reset and randomize the cube location, repeat the routine again.

Let’s take a closer look at the code together. Search for the function carter1_sequence

The robot is initially placed at [-4, -4, 0] on the warehouse map:

# Set default initial pose if not provided
if initial_pose is None:
    initial_pose = [(-4.0, -4.0, 0.0), yaw_to_quaternion(0.0)]

The drop off zone is located at [-3.5, -4, 0], like you have seen in the Isaac Sim scene. We also set up the prim paths for the table and limo robot. Note the namespace for the limo robot. One limo robot is setup for each carter sequence.

    # dropOff_pose position
    # Format: [(x, y, z), quaternion_tuple]
    carter1_dropOff_pose = [(-3.5, -4.0, 0.0), yaw_to_quaternion(-1.5708)]

    demo_table_path = "/World/Demo_table_1/thor_table"
    demo_cube_path = "/World/Demo_table_1/nvidia_cube"

    limo_namespace = f"{robot_name}_limo"
    limo_name = f"/World/{limo_namespace}"

We are now initializing the main object of this workshop, the TestContext, that will be analyzed in detail after the carter1_sequence initialization.

test_robot_context = TestContext(robot_name, initial_pose)

This section initializes the robot context and reads the table position.

# Grab the table position
success, table_state = await test_robot_context.get_entity_state(demo_table_path)
if not success:
    print(f"{robot_name}: Failed to get table state: {table_state}")
    return f"{robot_name} failed: Could not get table state"
print(f"{robot_name}: Table at {table_state.pose.position.x:.2f}, {table_state.pose.position.y:.2f}")

table_x = table_state.pose.position.x
table_y = table_state.pose.position.y
table_z = table_state.pose.position.z

limo_rest_quat_theta = yaw_to_quaternion(0.0)
limo_rest_pose = [(table_x-0.3, table_y-0.5, table_z+0.15), limo_rest_quat_theta]

Find the code under WORKSHOP 01 and uncomment that section.

The TestContext class

Let’s analyze another key component of this script, the TestContext class.

TestContext is a small ROS 2 helper class that wraps the simulation service clients (get/set entity state, spawn, play/pause sim) and exposes convenient async methods that return (success, payload_or_error).

The clients are created in (e.g., “get_entity_state”, “spawn_entity”, “set_simulation_state”, etc.). Below you can see the list of all services this node uses to spawn, and control the simulation state.

self.get_entity_service = self.node.create_client(GetEntityState, "get_entity_state")
self.set_entity_service = self.node.create_client(SetEntityState, "set_entity_state")
self.spawn_entity_service = self.node.create_client(SpawnEntity, "spawn_entity")
self.set_simulation_state_service = self.node.create_client(SetSimulationState, "set_simulation_state")
self.reset_simulation_service = self.node.create_client(ResetSimulation, "reset_simulation")
self.get_entity_info_service = self.node.create_client(GetEntityInfo, "get_entity_info")
self.publish_cmd_vel = None # to be set when the robot is spawned

Find the code under WORKSHOP TC-01 and uncomment that section.

The get_entity_info method

Queries metadata about an entity via the GetEntityInfo service. This metadata could be:

1. Does the entity exist?

2. What type is it? Note, in Isaac Sim 5.1 we only return CATEGORY_OBJECT so far.

   Returns (True, info) on success or (False, error_message) on failure.

   Internally: builds a GetEntityInfo.Request, calls the async client, spins until the future completes, and checks Result.RESULT_OK.

Tip

To learn more about the unique simulation interface Result, see the msg type here.

req = GetEntityInfo.Request()
req.entity = entity_path
future = self.get_entity_info_service.call_async(req)

while not future.done():
    rclpy.spin_once(self.node, timeout_sec=0.1)
    await asyncio.sleep(0.01)

if future.result() is not None:
    result = future.result()
    if result.result.result == Result.RESULT_OK:
        return True, result.info
    else:
        error_msg = result.result.error_message or f"Failed to get entity info (code: {result.result.result})"
        print(f"Error getting entity info for {entity_path}: {error_msg}")
        return False, error_msg

Find the code under WORKSHOP TC-02 and uncomment that section.

The get_entity_state method

Reads the current EntityState (pose + twist) for the given path via GetEntityState. Returns (True, state) or (False, error_message)

req = GetEntityState.Request()
req.entity = entity_path
future = self.get_entity_service.call_async(req)

while not future.done():
    rclpy.spin_once(self.node, timeout_sec=0.1)
    await asyncio.sleep(0.01)

if future.result() is not None:
    result = future.result()
    if result.result.result == Result.RESULT_OK:
        return True, result.state
    else:
        error_msg = result.result.error_message or f"Failed to get entity state (code: {result.result.result})"
        print(f"Error getting entity state for {entity_path}: {error_msg}")
        return False, error_msg

Find the code under WORKSHOP TC-03 and uncomment that section.

The set_simulation_state method

Using the SetSimulationState service, this method switches the simulation between states, such as

• SimulationState.STATE_PLAYING

• SimulationState.STATE_STOPPED

  Returns (True, None) on success or (False, error_message) on failure.

req = SetSimulationState.Request()
req.state.state = state
future = self.set_simulation_state_service.call_async(req)

while not future.done():
    rclpy.spin_once(self.node, timeout_sec=0.1)
    await asyncio.sleep(0.01)

if future.result() is not None:
    result = future.result()
    if result.result.result == Result.RESULT_OK:
        return True, None
    else:
        error_msg = result.result.error_message or f"Failed to set simulation state (code: {result.result.result})"
        print(f"Error setting simulation state: {error_msg}")
        return False, error_msg

Find the code under WORKSHOP TC-04 and uncomment that section.

The spawn_entity method

Spawns a new entity from a fully-formed SpawnEntity.Request. Returns (True, full_response, result_code) on success, or (False, error_message, result_code) on failure. (Convenience builder below: create_spawn_entity_request.)

future = self.spawn_entity_service.call_async(spawn_request)

while not future.done():
    rclpy.spin_once(self.node, timeout_sec=0.1)
    await asyncio.sleep(0.01)

result = future.result()
if result:
    if result.result.result == Result.RESULT_OK:
        print(f"Successfully spawned entity: {spawn_request.name}")
        return True, result, result.result.result
    else:
        error_msg = result.result.error_message or f"Failed to spawn entity (code: {result.result.result})"
        print(f"Error spawning entity {spawn_request.name}: {error_msg}")
        return False, error_msg, result.result.result

Find the code under WORKSHOP TC-05 and uncomment that section.

The create_spawn_entity_request method

Convenience builder that returns a correctly structured SpawnEntity.Request (name, USD URI, PoseStamped with frame, position, quaternion, optional namespace). Handy to avoid boilerplate and to keep pose fields as plain tuples.

spawn_request.name = name
spawn_request.uri = uri
spawn_request.allow_renaming = allow_renaming

# Create pose stamped
spawn_request.initial_pose = PoseStamped()
spawn_request.initial_pose.header.frame_id = frame_id
spawn_request.initial_pose.pose.position.x = float(position[0])
spawn_request.initial_pose.pose.position.y = float(position[1])
spawn_request.initial_pose.pose.position.z = float(position[2])
spawn_request.initial_pose.pose.orientation.w = float(orientation[0])
spawn_request.initial_pose.pose.orientation.x = float(orientation[1])
spawn_request.initial_pose.pose.orientation.y = float(orientation[2])
spawn_request.initial_pose.pose.orientation.z = float(orientation[3])

# Set namespace if provided
if entity_namespace:
    spawn_request.entity_namespace = entity_namespace

Find the code under WORKSHOP TC-06 and uncomment that section.

The set_entity_state method

Writes an entity’s pose and (optionally) a linear/angular velocity using SetEntityState. You can either pass a ready EntityState message or let the method build one from tuples. Returns (True, None) on success or (False, error_message)

spawn_request.name = name
spawn_request.uri = uri
spawn_request.allow_renaming = allow_renaming

# Create pose stamped
spawn_request.initial_pose = PoseStamped()
spawn_request.initial_pose.header.frame_id = frame_id
spawn_request.initial_pose.pose.position.x = float(position[0])
spawn_request.initial_pose.pose.position.y = float(position[1])
spawn_request.initial_pose.pose.position.z = float(position[2])
spawn_request.initial_pose.pose.orientation.w = float(orientation[0])
spawn_request.initial_pose.pose.orientation.x = float(orientation[1])
spawn_request.initial_pose.pose.orientation.y = float(orientation[2])
spawn_request.initial_pose.pose.orientation.z = float(orientation[3])

# Set namespace if provided
if entity_namespace:
    spawn_request.entity_namespace = entity_namespace

Find the code under WORKSHOP TC-07 and uncomment that section.

Implementing the robot sequence

Now we are inside the main, looping function for the carter1 sequence simulation. The routine runs two full trials to demonstrate repeatability and reset logic.

Overview

This code details how to fetch and monitor the cube’s pose, compute navigation waypoints, control the Limo robot to push the cube, reset both entities, and randomize cube position for iterative tabletop trials. If this fails, the sequence aborts with a message.

Navigation waypoint

Compute a navigation waypoint aligned with the table/cube and send the goal to the navigation planner.

# Check entity state
success, cube_state = await test_robot_context.get_entity_state(f"{demo_cube_path}")
if not success:
    print(f"{robot_name}: Failed to get cube state: {cube_state}")
    return f"{robot_name} failed: Could not get cube state"

print(f"{robot_name}: Cube at {cube_state.pose.position.x:.2f}, {cube_state.pose.position.y:.2f}")

cube_x = cube_state.pose.position.x
cube_y = cube_state.pose.position.y
cube_z = cube_state.pose.position.z
nav_x = cube_x - 0.15
nav_y = table_y - 1.05
nav_z = 0.0

nav_pose =[(nav_x, nav_y, nav_z), yaw_to_quaternion(1.5708)]

Find the code under WORKSHOP 02 and uncomment that section.

Start slightly behind the cube; push straight toward the table edge.

limo_start_x = cube_x
limo_start_y = cube_y + 0.25
limo_start_z = table_z + 0.15
limo_initial_pose = [(limo_start_x, limo_start_y, limo_start_z), yaw_to_quaternion(-1.5708)]

limo_goal_x = cube_x
limo_goal_y = table_y - 0.82
limo_goal_z = limo_start_z
limo_goal_pose = [(limo_goal_x, limo_goal_y, limo_goal_z), yaw_to_quaternion(-1.5708)]

Ensure the Limo exists, place it, command velocity to push, monitor the cube, stop, reset, and randomize for the subsequent trial.

• Ensure Limo exists; spawn if missing (shows how to use create_spawn_entity_request):

# Check does limo exist?
success, limo_error = await test_robot_context.get_entity_info(limo_name)
if not success:
    print(f"{robot_name}: {limo_error}")
    print(f"{robot_name}: Spawning limo")

    # Spawn limo
    success, spawn_result, spawn_result_code = await test_robot_context.spawn_entity(
        test_robot_context.create_spawn_entity_request(
            name=limo_name,
            uri=os.path.join(ROSCON25_ASSET_PATH, "limo/limo_ROS.usd"),
            position=limo_rest_pose[0],
            orientation=limo_rest_pose[1],
            allow_renaming=False,
            entity_namespace=limo_namespace
        )
    )

• Place the Limo at the initial pose (service-level teleport vs. nav)’

# move the limo to the limo initial pose
success, error_msg = await test_robot_context.set_entity_state(
    f"{limo_name}/chassis_link", 
    position=limo_initial_pose[0], 
    orientation=limo_initial_pose[1]
)

• Push: publish a short burst of cmd_vel in the Limo namespace.

• Create the publisher once (lazy): {limo_namespace}/cmd_vel.

  • Send three quick messages at ~20 Hz with linear.x = 0.2.

# Send cmd_vel to limo
cmd_vel = Twist()
cmd_vel.linear.x = 0.4
if test_robot_context.publish_cmd_vel is None:
# Instantly create a publisher for limo's namespace and publish cmd_vel
    test_robot_context.publish_cmd_vel = test_robot_context.node.create_publisher(
        Twist, f"/{limo_namespace}/cmd_vel", 10
    )

for i in range(3):
    test_robot_context.publish_cmd_vel.publish(cmd_vel)
    await asyncio.sleep(0.05)

• Monitor the cube until either:

  • it falls off (z < 0.5), or

  • it reaches the goal region near (limo_goal_x, limo_goal_y) (tight XY threshold)

  • A 60-iteration cap prevents infinite loops.

# Wait until cube reaches end of table
for i in range(60): #set a max number of iterations:
success, cube_state = await test_robot_context.get_entity_state(f"{demo_cube_path}")
if success:
    # If cube is at same position as carter robot with a +/- 0.25 margin or cube is below 0.5 meters, break
    if (cube_state.pose.position.z < 0.5) or (abs(cube_state.pose.position.x - limo_goal_x) < 0.05 and abs(cube_state.pose.position.y - limo_goal_y) < 0.05):
        print(f"{robot_name}: Cube at {cube_state.pose.position.x:.2f}, {cube_state.pose.position.y:.2f}, {cube_state.pose.position.z:.2f}")
        print(f"{robot_name}: Cube reached goal position or dropped off the table")
        break

    else:
        print(f"{robot_name}: Cube at {cube_state.pose.position.x:.2f}, {cube_state.pose.position.y:.2f}, {cube_state.pose.position.z:.2f}")
    await asyncio.sleep(0.1)

else:
    print(f"{robot_name}: Failed to get cube state: {cube_state}")
    break

• Stop the Limo (send zeros a few times) and reset the Limo to a “rest” staging pose behind the table (service-level set).

# Set robot velocity to 0
cmd_vel = Twist()
cmd_vel.linear.x = 0.0
cmd_vel.angular.z = 0.0

for i in range(3):
    test_robot_context.publish_cmd_vel.publish(cmd_vel)
    await asyncio.sleep(0.1)

# wait for block to fall off the table 
for i in range(10):
    success, cube_state = await test_robot_context.get_entity_state(f"{demo_cube_path}")
    if success:
        if cube_state.pose.position.z < 0.5:
            print(f"{robot_name}: Cube at {cube_state.pose.position.x:.2f}, {cube_state.pose.position.y:.2f}")
            break
    await asyncio.sleep(0.2)

# Set limo to rest pose
rest_position = limo_rest_pose[0]  # (x, y, z)
rest_orientation = limo_rest_pose[1]  # (w, x, y, z)
success, error_msg = await test_robot_context.set_entity_state(
    f"{limo_name}/chassis_link", 
    position=rest_position, 
    orientation=rest_orientation
)
if not success:
    print(f"{robot_name}: Failed to set limo to rest pose: {error_msg}")
print(f"{robot_name}: Limo set to rest pose")
   

• Carter drop-off move: navigate Carter to carter1_dropOff_pose and wait for completion

# Move to third position
test_robot_context.send_nav_goal(carter1_dropOff_pose[0], carter1_dropOff_pose[1])
result = await test_robot_context.wait_for_nav_complete()

• Randomize the cube for the next trial: sample a position on the tabletop (radius 0.3 around the table center) and teleport the cube there via set_entity_state.

# Spawn a random position on the table
random_cube_position = get_random_position(
    (table_state.pose.position.x, table_state.pose.position.y, table_state.pose.position.z),
    radius=0.3,
    theta=0.0
)

# Move cube to the random position by setting the entity state
success, error_msg = await test_robot_context.set_entity_state(
    f"{demo_cube_path}",
    position=random_cube_position[0],
    orientation=random_cube_position[1]
)
if not success:
    print(f"{robot_name}: Failed to set cube to random position: {error_msg}")
print(f"{robot_name}: Cube set to random position")

test_robot_context.destroy()

Find the code under WORKSHOP 03 and uncomment that section

Spawn the World in Isaac Sim

req = LoadWorld.Request()
req.uri = os.path.join(ROSCON25_ASSET_PATH, "Starting_Point/Empty_Warehouse_Scene.usd")
future = load_world_client.call_async(req)
rclpy.spin_until_future_complete(node, future)

if future.result() and future.result().result.result == Result.RESULT_OK:
    print("World loaded successfully")
else:
    print("Failed to load world")

Find the code under WORKSHOP 04 and uncomment that section.

This section builds a fresh SpawnEntity.Request with:

• full path: /World/Demo_table_1/thor_table,

• frame_id : world

• position: (-4.72, 0.688, 1.19)

• quaternion: identity

• allow_renaming=False

  Calls spawn + checks result

table_req = SpawnEntity.Request()
table_req.name = "/World/Demo_table_1/thor_table"
table_req.uri = os.path.join(ROSCON25_ASSET_PATH, "thor_table/thor_table.usd")
table_req.allow_renaming = False
table_req.initial_pose = PoseStamped()
table_req.initial_pose.header.frame_id = "world"
table_req.initial_pose.pose.position.x = float(-4.72)
table_req.initial_pose.pose.position.y = float(0.688)
table_req.initial_pose.pose.position.z = float(1.19)
table_req.initial_pose.pose.orientation.w = float(1.0)
table_req.initial_pose.pose.orientation.x = float(0.0)
table_req.initial_pose.pose.orientation.y = float(0.0)
table_req.initial_pose.pose.orientation.z = float(0.0)
future = spawn_entity_client.call_async(table_req)
rclpy.spin_until_future_complete(node, future)

if future.result() and future.result().result.result == Result.RESULT_OK:
    print("Table spawned successfully")
else:
    print("Failed to spawn table")

Find the code WORKSHOP 05 and uncomment. This section spawns the nvidia cube at /World/Demo_table_1/nvidia_cube with:

• full path: /World/Demo_table_1/nvidia_cube,

• frame_id : world

• Position (partially reused): (-4.72+0.15, 0.688, 1.29)

• Quaternion (reused)

• allow_renaming=True (because we can spawn multiple cubes without having to worry about naming manually)

The spawn pose is set in world coordinates, and the result is confirmed and printed.

table_req.name = "/World/Demo_table_1/nvidia_cube"
table_req.uri = os.path.join(ROSCON25_ASSET_PATH, "nvidia_cube/nvidia_cube.usd")
table_req.allow_renaming = True
table_req.initial_pose.header.frame_id = "world"
table_req.initial_pose.pose.position.x += 0.15
table_req.initial_pose.pose.position.z = float(1.29)
future = spawn_entity_client.call_async(table_req)
rclpy.spin_until_future_complete(node, future)

if future.result() and future.result().result.result == Result.RESULT_OK:
    print("Cube spawned successfully")
else:
    print("Failed to spawn cube")

Find the code WORKSHOP 06 and uncomment.

This section spawns Nova Carter at /World/nova_carter_ROS_1 with:

• entity_namespace=”carter1”

• full path: /World/nova_carter_ROS_1,

• frame_id : world

• Position: (-4.0, -4.0, 0.0)

• Quaternion: yaw of +90° using yaw_to_quaternion(1.5708).

• allow_renaming=False (because we only intend to one Nova Carter spawned at this time) The spawn pose is set in world coordinates, and the result is confirmed and printed.

carter1_req = SpawnEntity.Request()
carter1_req.name = "/World/nova_carter_ROS_1"
carter1_req.uri = os.path.join(ROSCON25_ASSET_PATH, "Starting_Point/nova_carter_ROS.usd")
carter1_req.entity_namespace = "carter1"
carter1_req.allow_renaming = False
carter1_req.initial_pose = PoseStamped()
carter1_req.initial_pose.header.frame_id = "world"
carter1_req.initial_pose.pose.position.x = float(-4.0)
carter1_req.initial_pose.pose.position.y = float(-4.0)
carter1_req.initial_pose.pose.position.z = float(0.0)
quat = yaw_to_quaternion(1.5708)
carter1_req.initial_pose.pose.orientation.w = float(quat[0])
carter1_req.initial_pose.pose.orientation.x = float(quat[1])
carter1_req.initial_pose.pose.orientation.y = float(quat[2])
carter1_req.initial_pose.pose.orientation.z = float(quat[3])
future = spawn_entity_client.call_async(carter1_req)
rclpy.spin_until_future_complete(node, future)

if future.result() and future.result().result.result == Result.RESULT_OK:
    print("Carter1 spawned successfully")
else:
    print("Failed to spawn Carter1")

Find the code WORKSHOP 07 and uncomment.

Now, in this section, we will set the simulation state to SimulationState.STATE_PLAYING, spin ROS functions, and introduce a small delay to let simulation physics stacks settle.

req = SetSimulationState.Request()
req.state.state = SimulationState.STATE_PLAYING
future = set_state_client.call_async(req)
rclpy.spin_until_future_complete(node, future)

if future.result() and future.result().result.result == Result.RESULT_OK:
    print("Simulation playing successfully")
else:
    print("Failed to play simulation")

Find the code WORKSHOP 08 and uncomment.

When we stop the simulation, we set the state to SimulationState.STATE_STOPPED, spin ROS functions, and introduce a small delay to let simulation settle. This is essentially a clean pause before teardown.

req = SetSimulationState.Request()
req.state.state = SimulationState.STATE_STOPPED
future = set_state_client.call_async(req)
rclpy.spin_until_future_complete(node, future)

if future.result() and future.result().result.result == Result.RESULT_OK:
    print("Simulation is now stopped")
else:
    print("Failed to stop simulation")

Find the code WORKSHOP 09 and uncomment.

After that, we send a UnloadWorld.Request(), spin and wait for Isaac Sim to unload the world.

req = UnloadWorld.Request()
future = unload_world_client.call_async(req)
rclpy.spin_until_future_complete(node, future)

if future.result() and future.result().result.result == Result.RESULT_OK:
    print("World unloaded successfully")
else:
    print("Failed to unload world")

Find the code WORKSHOP 10 and uncomment.

Testing the Script

Now that we’ve analyzed the code in our script, let’s try it out.

Follow the instructions below to run the Nav2 stack:

Running the Nav Stack With the Test Script

1. Open a new terminal and source your ROS workspace:

source sim_control_script_and_carter_ws/ros_ws/install/setup.bash

Ensure that the ROS_DOMAIN_ID environment variable is set.

export ROS_DOMAIN_ID=42 # Replace number with your own station number

2. Start Nav2 for multi robot simulation for both Nova Carters by running the following command:

ros2 launch carter_navigation multiple_robot_carter_navigation_warehouse.launch.py

3. From a ROS-sourced terminal set your ROS_DOMAIN_ID and run the script:

export ROS_DOMAIN_ID=42 # Replace number with your own station number
# cd into the course materials folder
python3 sim_control_script_and_carter_ws/learning_simulation_control/workshop_learning_script.py

Tip

If you need, the full output script from this module is found in the course materials undersim_control_script_and_carter_ws/learning_simulation_control/workshop_learning_script_final.py.

You may also run a similar command as above to run this script here:

export ROS_DOMAIN_ID=42 # Replace number with your own station number
# cd into the course materials folder
python3 sim_control_script_and_carter_ws/learning_simulation_control/workshop_learning_script_final.py

Tip

The backup robot file is located in the course assets under /Backup/nova_carter_ROS.usd.

If you need to use the backup robot USD file, copy and replace it into the Starting Point folder at: Starting_Point/nova_carter_ROS.usd.

Tip

If the Nav stack seems stuck, simply stop both the Nav2 stack and the current script command (CTRL+C) and re-run.