From 03a285ccff96a5e488be4bc862caa11aa7bf400d Mon Sep 17 00:00:00 2001 From: Ayush Ghosh Date: Wed, 15 Oct 2025 22:18:15 -0700 Subject: [PATCH 1/7] Added initial warehouse_simulation_script --- README.md | 7 + warehouse_simulation_script.py | 459 +++++++++++++++++++++++++++++++++ 2 files changed, 466 insertions(+) create mode 100644 warehouse_simulation_script.py diff --git a/README.md b/README.md index 99f6593..592b696 100644 --- a/README.md +++ b/README.md @@ -11,5 +11,12 @@ https://github.com/RobotecAI/q_simulation_interfaces ### Tutorial +Install the demo assets folder to your local machine. + +Run the script: + +`python3 warehouse_simulation_script.py` + + ### Other resources ROSCon 2025 talk introducing the standard and resources diff --git a/warehouse_simulation_script.py b/warehouse_simulation_script.py new file mode 100644 index 0000000..c8354e6 --- /dev/null +++ b/warehouse_simulation_script.py @@ -0,0 +1,459 @@ +import rclpy +from simulation_interfaces.srv import GetEntityState, SetEntityState, SpawnEntity, DeleteEntity, SetSimulationState, ResetSimulation, GetEntityInfo, LoadWorld, UnloadWorld +from simulation_interfaces.msg import Result, SimulationState +from geometry_msgs.msg import Twist, PoseStamped, Pose, Point, Quaternion +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +import os +import numpy as np +import time + +DEMO_ASSET_PATH = os.getenv('DEMO_ASSET_PATH') + + +def yaw_to_quaternion(yaw): + """Convert a yaw angle (in radians) to a quaternion (w, x, y, z).""" + yaw = np.arctan2(np.sin(yaw), np.cos(yaw)) + half_yaw = yaw / 2.0 + q = np.array([ + np.cos(half_yaw), # w + 0.0, # x + 0.0, # y + np.sin(half_yaw) # z + ]) + q = q / np.linalg.norm(q) + return tuple(q) + +def main(): + rclpy.init() + + # Initialize main ROS node + node = rclpy.create_node("warehouse_simulation") + + # Create service clients + set_state_client = node.create_client(SetSimulationState, 'set_simulation_state') + reset_client = node.create_client(ResetSimulation, 'reset_simulation') + load_world_client = node.create_client(LoadWorld, 'load_world') + unload_world_client = node.create_client(UnloadWorld, 'unload_world') + spawn_entity_client = node.create_client(SpawnEntity, 'spawn_entity') + get_entity_state_client = node.create_client(GetEntityState, 'get_entity_state') + set_entity_state_client = node.create_client(SetEntityState, 'set_entity_state') + get_entity_info_client = node.create_client(GetEntityInfo, 'get_entity_info') + + # Create publishers for robot control + dingo_cmd_vel_pub = None + ur10_joint_pub = None + + try: + # Wait for services + print("Waiting for simulation services...") + while not set_state_client.wait_for_service(timeout_sec=1.0): + print("set_simulation_state service not available, waiting...") + while not load_world_client.wait_for_service(timeout_sec=1.0): + print("load_world service not available, waiting...") + while not spawn_entity_client.wait_for_service(timeout_sec=1.0): + print("spawn_entity service not available, waiting...") + + print("All services ready!") + + # Load warehouse world + print("Loading warehouse world...") + req = LoadWorld.Request() + req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_warehouse_with_forklifts/warehouse_with_forklifts.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("Warehouse world loaded successfully") + else: + print("Failed to load warehouse world") + return + + time.sleep(1.0) + + # Load a table prop + print("Spawning warehouse table...") + table_req = SpawnEntity.Request() + table_req.name = "/World/warehouse_table" + table_req.uri = os.path.join(DEMO_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(-1.0) + table_req.initial_pose.pose.position.y = float(-1.5) + table_req.initial_pose.pose.position.z = float(1.19) + quat = yaw_to_quaternion(1.5708) + table_req.initial_pose.pose.orientation.w = float(quat[0]) + table_req.initial_pose.pose.orientation.x = float(quat[1]) + table_req.initial_pose.pose.orientation.y = float(quat[2]) + table_req.initial_pose.pose.orientation.z = float(quat[3]) + 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") + + time.sleep(0.5) + + # Get table state to make all other positions relative to it + print("Getting table state for relative positioning...") + req = GetEntityState.Request() + req.entity = "/World/warehouse_table" + future = get_entity_state_client.call_async(req) + rclpy.spin_until_future_complete(node, future) + + if future.result() and future.result().result.result == Result.RESULT_OK: + table_state = future.result().state + table_x = table_state.pose.position.x + table_y = table_state.pose.position.y + table_z = table_state.pose.position.z + print(f"Table found at position: ({table_x:.2f}, {table_y:.2f}, {table_z:.2f})") + else: + print("Failed to get table state - cannot proceed with relative positioning") + return + + # Load some cubes around the table top (relative to table position) - mix of red and blue + print("Spawning cubes around table...") + cube_configs = [ + # (position, color) + ((table_x + 0.2, table_y + 0.2, table_z + 0.1), "blue"), # Top right - blue + ((table_x - 0.2, table_y + 0.2, table_z + 0.1), "red"), # Top left - red + ((table_x + 0.2, table_y - 0.2, table_z + 0.1), "red"), # Bottom right - red + ((table_x - 0.2, table_y - 0.2, table_z + 0.1), "blue"), # Bottom left - blue + ((table_x, table_y, table_z + 0.1), "blue"), # Center - blue + ((table_x + 0.1, table_y, table_z + 0.1), "red"), # Center right - red + ((table_x - 0.1, table_y, table_z + 0.1), "red"), # Center left - red + ] + + for i, (pos, color) in enumerate(cube_configs): + cube_req = SpawnEntity.Request() + cube_req.name = f"/World/{color}_cube" + + # Choose asset based on color + if color == "blue": + cube_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_blue_block/blue_block.usd") + else: # red + cube_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_red_block/red_block.usd") + + cube_req.allow_renaming = True + cube_req.initial_pose = PoseStamped() + cube_req.initial_pose.header.frame_id = "world" + cube_req.initial_pose.pose.position.x = float(pos[0]) + cube_req.initial_pose.pose.position.y = float(pos[1]) + cube_req.initial_pose.pose.position.z = float(pos[2]) + cube_req.initial_pose.pose.orientation.w = float(1.0) + cube_req.initial_pose.pose.orientation.x = float(0.0) + cube_req.initial_pose.pose.orientation.y = float(0.0) + cube_req.initial_pose.pose.orientation.z = float(0.0) + future = spawn_entity_client.call_async(cube_req) + rclpy.spin_until_future_complete(node, future) + + if future.result() and future.result().result.result == Result.RESULT_OK: + print(f"{color.capitalize()} cube spawned successfully at {pos}") + else: + print(f"Failed to spawn {color} cube") + + time.sleep(0.5) + + # Load Dingo robot + print("Spawning Dingo robot...") + dingo_req = SpawnEntity.Request() + dingo_req.name = "/World/dingo_robot" + dingo_req.uri = os.path.join(DEMO_ASSET_PATH, "dingo/dingo_ROS.usd") # Assuming dingo asset exists + dingo_req.entity_namespace = "dingo" + dingo_req.allow_renaming = False + dingo_req.initial_pose = PoseStamped() + dingo_req.initial_pose.header.frame_id = "world" + dingo_req.initial_pose.pose.position.x = float(-4.0) # Fixed position + dingo_req.initial_pose.pose.position.y = float(-3.0) # Fixed position + dingo_req.initial_pose.pose.position.z = float(0.0) + quat = yaw_to_quaternion(0.0) # Facing forward + dingo_req.initial_pose.pose.orientation.w = float(quat[0]) + dingo_req.initial_pose.pose.orientation.x = float(quat[1]) + dingo_req.initial_pose.pose.orientation.y = float(quat[2]) + dingo_req.initial_pose.pose.orientation.z = float(quat[3]) + future = spawn_entity_client.call_async(dingo_req) + rclpy.spin_until_future_complete(node, future) + + if future.result() and future.result().result.result == Result.RESULT_OK: + print("Dingo robot spawned successfully") + # Create cmd_vel publisher for dingo + dingo_cmd_vel_pub = node.create_publisher(Twist, "/dingo/cmd_vel", 10) + else: + print("Failed to spawn Dingo robot") + + time.sleep(0.5) + + # Load UR10 robot + print("Spawning UR10 robot...") + ur10_req = SpawnEntity.Request() + ur10_req.name = "/World/ur10_robot" + ur10_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_ur10e_robotiq2f-140_ROS/ur10e_robotiq2f-140_ROS.usd") # Assuming ur10e asset exists + ur10_req.entity_namespace = "ur10" + ur10_req.allow_renaming = False + ur10_req.initial_pose = PoseStamped() + ur10_req.initial_pose.header.frame_id = "world" + ur10_req.initial_pose.pose.position.x = float(table_x) # Match table x position + ur10_req.initial_pose.pose.position.y = float(table_y - 0.64) # Match table y position + ur10_req.initial_pose.pose.position.z = float(table_z) # On top of the table + quat = yaw_to_quaternion(-1.5708) # 180 rotation + ur10_req.initial_pose.pose.orientation.w = float(quat[0]) + ur10_req.initial_pose.pose.orientation.x = float(quat[1]) + ur10_req.initial_pose.pose.orientation.y = float(quat[2]) + ur10_req.initial_pose.pose.orientation.z = float(quat[3]) + future = spawn_entity_client.call_async(ur10_req) + rclpy.spin_until_future_complete(node, future) + + if future.result() and future.result().result.result == Result.RESULT_OK: + print("UR10 robot spawned successfully") + # Create joint state publisher for ur10 + ur10_joint_pub = node.create_publisher(JointState, "/ur10/joint_command", 10) + else: + print("Failed to spawn UR10 robot") + + time.sleep(1.0) + + # Start simulation + print("Starting simulation...") + 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 started successfully") + else: + print("Failed to start simulation") + + time.sleep(2.0) + + # Main simulation loop + for loop_iteration in range(3): + print(f"\n=== Loop iteration {loop_iteration + 1} ===") + + # Get updated table state (in case it moved) + print("Getting table state...") + req = GetEntityState.Request() + req.entity = "/World/warehouse_table" + future = get_entity_state_client.call_async(req) + + rclpy.spin_until_future_complete(node, future) + + if future.result() and future.result().result.result == Result.RESULT_OK: + table_state = future.result().state + table_x = table_state.pose.position.x + table_y = table_state.pose.position.y + table_z = table_state.pose.position.z + print(f"Table position: ({table_x:.2f}, {table_y:.2f}, {table_z:.2f})") + else: + print("Failed to get table state - ending simulation loop") + break + + # Spawn cardboard boxes at different positions based on loop iteration + print(f"Spawning obstacle boxes near table (iteration {loop_iteration + 1})...") + + # Different box positions for each iteration + box_positions_by_iteration = [ + # Iteration 1: Boxes around the south side + [ + (table_x - 1.5, table_y - 1.0, 0.0, 0.0), # Southwest + (table_x + 0.5, table_y - 2.0, 0.0, 1.5708), # South, rotated 90° + (table_x - 0.8, table_y - 1.8, 0.0, 0.7854), # Southeast, rotated 45° + ], + # Iteration 2: Boxes around the east and west sides + [ + (table_x - 2.0, table_y + 0.5, 0.0, 1.5708), # West, rotated 90° + (table_x + 1.5, table_y - 0.5, 0.0, 0.0), # East + (table_x - 1.0, table_y + 1.2, 0.0, -0.7854), # Northwest, rotated -45° + ], + # Iteration 3: Boxes around the north side + [ + (table_x - 0.8, table_y + 1.8, 0.0, 0.7854), # North, rotated 45° + (table_x + 0.3, table_y + 1.5, 0.0, 1.5708), # Northeast, rotated 90° + (table_x - 1.2, table_y + 0.8, 0.0, 0.0), # Northwest + ] + ] + + box_positions = box_positions_by_iteration[loop_iteration] + + for i, (box_x, box_y, box_z, box_yaw) in enumerate(box_positions): + box_req = SpawnEntity.Request() + box_req.name = f"/World/obstacle_box" + box_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_warehouse_with_forklifts/Props/SM_CardBoxA_02.usd") + box_req.allow_renaming = True + box_req.initial_pose = PoseStamped() + box_req.initial_pose.header.frame_id = "world" + box_req.initial_pose.pose.position.x = float(box_x) + box_req.initial_pose.pose.position.y = float(box_y) + box_req.initial_pose.pose.position.z = float(box_z) + # Specific rotation for each box + quat = yaw_to_quaternion(box_yaw) + box_req.initial_pose.pose.orientation.w = float(quat[0]) + box_req.initial_pose.pose.orientation.x = float(quat[1]) + box_req.initial_pose.pose.orientation.y = float(quat[2]) + box_req.initial_pose.pose.orientation.z = float(quat[3]) + future = spawn_entity_client.call_async(box_req) + rclpy.spin_until_future_complete(node, future) + + if future.result() and future.result().result.result == Result.RESULT_OK: + print(f"Obstacle box {i+1} spawned at ({box_x:.2f}, {box_y:.2f})") + else: + print(f"Failed to spawn obstacle box {i+1}") + + time.sleep(0.5) + + # Move the Dingo robot towards the table + if dingo_cmd_vel_pub: + print("Moving Dingo robot towards table...") + + # Calculate direction to table + req = GetEntityState.Request() + req.entity = "/World/dingo_robot" + future = get_entity_state_client.call_async(req) + + while not future.done(): + rclpy.spin_once(node, timeout_sec=0.1) + time.sleep(0.01) + + if future.result() and future.result().result.result == Result.RESULT_OK: + dingo_state = future.result().state + dingo_x = dingo_state.pose.position.x + dingo_y = dingo_state.pose.position.y + + # Calculate movement towards table + dx = table_x - dingo_x + dy = table_y - dingo_y + distance = np.sqrt(dx*dx + dy*dy) + + if distance > 1.5: # Move closer if too far + # Send velocity commands + cmd_vel = Twist() + cmd_vel.linear.x = 0.3 # Forward speed + cmd_vel.angular.z = np.arctan2(dy, dx) * 0.5 # Turn towards table + + # Send commands for 2 seconds + for _ in range(20): + dingo_cmd_vel_pub.publish(cmd_vel) + time.sleep(0.1) + + # Stop the robot + cmd_vel.linear.x = 0.0 + cmd_vel.angular.z = 0.0 + for _ in range(5): + dingo_cmd_vel_pub.publish(cmd_vel) + time.sleep(0.1) + + print(f"Dingo robot moved towards table (distance: {distance:.2f}m)") + + # Move the UR10 by sending manual joint commands + if ur10_joint_pub: + print("Moving UR10 robot joints...") + + joint_cmd = JointState() + joint_cmd.header = Header() + joint_cmd.header.stamp = node.get_clock().now().to_msg() + joint_cmd.name = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", + "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] + + # Predefined joint positions for each iteration + joint_positions_by_iteration = [ + [0.0, -1.5708, 1.5708, -1.5708, -1.5708, 0.0], # Iteration 1: Home position + [0.7854, -1.0472, 0.7854, -0.7854, -1.5708, 0.5236], # Iteration 2: Reach pose + [-0.5236, -2.0944, 2.0944, -2.0944, -1.5708, -0.7854], # Iteration 3: Pickup pose + ] + + # Use specific joint positions for this iteration + joint_cmd.position = joint_positions_by_iteration[loop_iteration] + + # Send joint commands + for _ in range(10): + ur10_joint_pub.publish(joint_cmd) + time.sleep(0.2) + + print("UR10 joint positions updated") + + time.sleep(0.1) + + # Move robot to a specific location relative to table + print("Moving Dingo to a specific location...") + new_x = table_x - 3.0 # 3 meters away from table + new_y = table_y - 2.5 # 2.5 meters in front of table + new_yaw = 1.5708 # Face towards table (90 degrees) + + # Use set_entity_state to teleport the robot to new position + from simulation_interfaces.msg import EntityState + from geometry_msgs.msg import Vector3 + + req = SetEntityState.Request() + req.entity = "/World/dingo_robot" + + state = EntityState() + state.pose = Pose() + state.pose.position = Point(x=float(new_x), y=float(new_y), z=float(0.0)) + quat = yaw_to_quaternion(new_yaw) + state.pose.orientation = Quaternion(w=float(quat[0]), x=float(quat[1]), + y=float(quat[2]), z=float(quat[3])) + state.twist = Twist() + state.twist.linear = Vector3(x=0.0, y=0.0, z=0.0) + state.twist.angular = Vector3(x=0.0, y=0.0, z=0.0) + + req.state = state + + future = set_entity_state_client.call_async(req) + + rclpy.spin_until_future_complete(node, future) + + if future.result() and future.result().result.result == Result.RESULT_OK: + print(f"Dingo moved to new position: ({new_x:.2f}, {new_y:.2f}, yaw: {new_yaw:.2f})") + else: + print("Failed to move Dingo to new position") + + time.sleep(0.1) # Wait between iterations + + print("\nSimulation loop completed!") + + # Stop simulation + print("Stopping simulation...") + 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 stopped successfully") + else: + print("Failed to stop simulation") + + time.sleep(0.5) + + # Unload world + print("Unloading 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") + + print("Warehouse simulation completed!") + + except KeyboardInterrupt: + print("\nInterrupted! Stopping simulation...") + + # Stop simulation + req = SetSimulationState.Request() + req.state.state = SimulationState.STATE_STOPPED + future = set_state_client.call_async(req) + rclpy.spin_until_future_complete(node, future) + + print("Simulation stopped due to interruption!") + + finally: + rclpy.shutdown() + +if __name__ == "__main__": + main() From 1bdc770585993f67243c1ed7d1f56e71db39b137 Mon Sep 17 00:00:00 2001 From: Ayush Ghosh Date: Wed, 15 Oct 2025 22:33:51 -0700 Subject: [PATCH 2/7] Added joint control --- warehouse_simulation_script.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/warehouse_simulation_script.py b/warehouse_simulation_script.py index c8354e6..0bebdd3 100644 --- a/warehouse_simulation_script.py +++ b/warehouse_simulation_script.py @@ -209,7 +209,7 @@ def main(): if future.result() and future.result().result.result == Result.RESULT_OK: print("UR10 robot spawned successfully") # Create joint state publisher for ur10 - ur10_joint_pub = node.create_publisher(JointState, "/ur10/joint_command", 10) + ur10_joint_pub = node.create_publisher(JointState, "/ur10/joint_commands", 10) else: print("Failed to spawn UR10 robot") @@ -347,7 +347,7 @@ def main(): print(f"Dingo robot moved towards table (distance: {distance:.2f}m)") - # Move the UR10 by sending manual joint commands + # Move the UR10 by sending manual joint commands (arm only) if ur10_joint_pub: print("Moving UR10 robot joints...") From cfe725951161cd6352816a8f7189a93c2a6d7ebf Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Thu, 16 Oct 2025 15:39:28 +0200 Subject: [PATCH 3/7] Add error logs; fix spawning position Signed-off-by: Jan Hanca --- warehouse_simulation_script.py | 42 ++++++++++++++++++---------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/warehouse_simulation_script.py b/warehouse_simulation_script.py index 0bebdd3..3e78388 100644 --- a/warehouse_simulation_script.py +++ b/warehouse_simulation_script.py @@ -66,7 +66,7 @@ def main(): if future.result() and future.result().result.result == Result.RESULT_OK: print("Warehouse world loaded successfully") else: - print("Failed to load warehouse world") + print("Failed to load warehouse world: " + future.result().result.error_message) return time.sleep(1.0) @@ -74,7 +74,7 @@ def main(): # Load a table prop print("Spawning warehouse table...") table_req = SpawnEntity.Request() - table_req.name = "/World/warehouse_table" + table_req.name = "warehouse_table" table_req.uri = os.path.join(DEMO_ASSET_PATH, "thor_table/thor_table.usd") table_req.allow_renaming = False table_req.initial_pose = PoseStamped() @@ -93,14 +93,14 @@ def main(): if future.result() and future.result().result.result == Result.RESULT_OK: print("Table spawned successfully") else: - print("Failed to spawn table") + print("Failed to spawn table: " + future.result().result.error_message) time.sleep(0.5) # Get table state to make all other positions relative to it print("Getting table state for relative positioning...") req = GetEntityState.Request() - req.entity = "/World/warehouse_table" + req.entity = "warehouse_table" future = get_entity_state_client.call_async(req) rclpy.spin_until_future_complete(node, future) @@ -112,6 +112,7 @@ def main(): print(f"Table found at position: ({table_x:.2f}, {table_y:.2f}, {table_z:.2f})") else: print("Failed to get table state - cannot proceed with relative positioning") + print("Error: " + future.result().result.error_message) return # Load some cubes around the table top (relative to table position) - mix of red and blue @@ -129,14 +130,14 @@ def main(): for i, (pos, color) in enumerate(cube_configs): cube_req = SpawnEntity.Request() - cube_req.name = f"/World/{color}_cube" + cube_req.name = f"{color}_cube" # Choose asset based on color if color == "blue": cube_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_blue_block/blue_block.usd") else: # red cube_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_red_block/red_block.usd") - + cube_req.allow_renaming = True cube_req.initial_pose = PoseStamped() cube_req.initial_pose.header.frame_id = "world" @@ -153,14 +154,14 @@ def main(): if future.result() and future.result().result.result == Result.RESULT_OK: print(f"{color.capitalize()} cube spawned successfully at {pos}") else: - print(f"Failed to spawn {color} cube") + print(f"Failed to spawn {color} cube: " + future.result().result.error_message) time.sleep(0.5) # Load Dingo robot print("Spawning Dingo robot...") dingo_req = SpawnEntity.Request() - dingo_req.name = "/World/dingo_robot" + dingo_req.name = "dingo_robot" dingo_req.uri = os.path.join(DEMO_ASSET_PATH, "dingo/dingo_ROS.usd") # Assuming dingo asset exists dingo_req.entity_namespace = "dingo" dingo_req.allow_renaming = False @@ -189,7 +190,7 @@ def main(): # Load UR10 robot print("Spawning UR10 robot...") ur10_req = SpawnEntity.Request() - ur10_req.name = "/World/ur10_robot" + ur10_req.name = "ur10_robot" ur10_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_ur10e_robotiq2f-140_ROS/ur10e_robotiq2f-140_ROS.usd") # Assuming ur10e asset exists ur10_req.entity_namespace = "ur10" ur10_req.allow_renaming = False @@ -199,7 +200,7 @@ def main(): ur10_req.initial_pose.pose.position.y = float(table_y - 0.64) # Match table y position ur10_req.initial_pose.pose.position.z = float(table_z) # On top of the table quat = yaw_to_quaternion(-1.5708) # 180 rotation - ur10_req.initial_pose.pose.orientation.w = float(quat[0]) + ur10_req.initial_pose.pose.orientation.w = float(quat[0]) ur10_req.initial_pose.pose.orientation.x = float(quat[1]) ur10_req.initial_pose.pose.orientation.y = float(quat[2]) ur10_req.initial_pose.pose.orientation.z = float(quat[3]) @@ -225,7 +226,7 @@ def main(): if future.result() and future.result().result.result == Result.RESULT_OK: print("Simulation started successfully") else: - print("Failed to start simulation") + print("Failed to start simulation: " + future.result().result.error_message) time.sleep(2.0) @@ -236,7 +237,7 @@ def main(): # Get updated table state (in case it moved) print("Getting table state...") req = GetEntityState.Request() - req.entity = "/World/warehouse_table" + req.entity = "warehouse_table" future = get_entity_state_client.call_async(req) rclpy.spin_until_future_complete(node, future) @@ -249,6 +250,7 @@ def main(): print(f"Table position: ({table_x:.2f}, {table_y:.2f}, {table_z:.2f})") else: print("Failed to get table state - ending simulation loop") + print("Error: " + future.result().result.error_message) break # Spawn cardboard boxes at different positions based on loop iteration @@ -266,7 +268,7 @@ def main(): [ (table_x - 2.0, table_y + 0.5, 0.0, 1.5708), # West, rotated 90° (table_x + 1.5, table_y - 0.5, 0.0, 0.0), # East - (table_x - 1.0, table_y + 1.2, 0.0, -0.7854), # Northwest, rotated -45° + (table_x + 1.8, table_y + 1.2, 0.0, 0.7854), # Northeast, rotated 45° ], # Iteration 3: Boxes around the north side [ @@ -280,7 +282,7 @@ def main(): for i, (box_x, box_y, box_z, box_yaw) in enumerate(box_positions): box_req = SpawnEntity.Request() - box_req.name = f"/World/obstacle_box" + box_req.name = "obstacle_box" box_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_warehouse_with_forklifts/Props/SM_CardBoxA_02.usd") box_req.allow_renaming = True box_req.initial_pose = PoseStamped() @@ -300,7 +302,7 @@ def main(): if future.result() and future.result().result.result == Result.RESULT_OK: print(f"Obstacle box {i+1} spawned at ({box_x:.2f}, {box_y:.2f})") else: - print(f"Failed to spawn obstacle box {i+1}") + print(f"Failed to spawn obstacle box {i+1}: " + future.result().result.error_message) time.sleep(0.5) @@ -310,7 +312,7 @@ def main(): # Calculate direction to table req = GetEntityState.Request() - req.entity = "/World/dingo_robot" + req.entity = "dingo_robot" future = get_entity_state_client.call_async(req) while not future.done(): @@ -387,7 +389,7 @@ def main(): from geometry_msgs.msg import Vector3 req = SetEntityState.Request() - req.entity = "/World/dingo_robot" + req.entity = "dingo_robot" state = EntityState() state.pose = Pose() @@ -408,7 +410,7 @@ def main(): if future.result() and future.result().result.result == Result.RESULT_OK: print(f"Dingo moved to new position: ({new_x:.2f}, {new_y:.2f}, yaw: {new_yaw:.2f})") else: - print("Failed to move Dingo to new position") + print("Failed to move Dingo to new position: " + future.result().result.error_message) time.sleep(0.1) # Wait between iterations @@ -424,7 +426,7 @@ def main(): if future.result() and future.result().result.result == Result.RESULT_OK: print("Simulation stopped successfully") else: - print("Failed to stop simulation") + print("Failed to stop simulation: " + future.result().result.error_message) time.sleep(0.5) @@ -437,7 +439,7 @@ def main(): if future.result() and future.result().result.result == Result.RESULT_OK: print("World unloaded successfully") else: - print("Failed to unload world") + print("Failed to unload world: " + future.result().result.error_message) print("Warehouse simulation completed!") From 9494ab0c8d7addc3202b836f59dadb482ac13d93 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Thu, 16 Oct 2025 19:30:58 +0200 Subject: [PATCH 4/7] Refactor script with GPT-5.0; add o3de uri paths Signed-off-by: Jan Hanca --- warehouse_simulation_script.py | 942 +++++++++++++++++++-------------- 1 file changed, 552 insertions(+), 390 deletions(-) diff --git a/warehouse_simulation_script.py b/warehouse_simulation_script.py index 3e78388..6e0218f 100644 --- a/warehouse_simulation_script.py +++ b/warehouse_simulation_script.py @@ -1,36 +1,40 @@ import rclpy from simulation_interfaces.srv import GetEntityState, SetEntityState, SpawnEntity, DeleteEntity, SetSimulationState, ResetSimulation, GetEntityInfo, LoadWorld, UnloadWorld from simulation_interfaces.msg import Result, SimulationState -from geometry_msgs.msg import Twist, PoseStamped, Pose, Point, Quaternion +from geometry_msgs.msg import Twist, PoseStamped, Pose, Point, Quaternion, Vector3 from sensor_msgs.msg import JointState from std_msgs.msg import Header +import argparse import os import numpy as np import time +import logging DEMO_ASSET_PATH = os.getenv('DEMO_ASSET_PATH') def yaw_to_quaternion(yaw): - """Convert a yaw angle (in radians) to a quaternion (w, x, y, z).""" + """Convert a yaw angle (in radians) to a geometry_msgs.msg.Quaternion. + + Returns a Quaternion message with normalized components (w, x, y, z). + """ yaw = np.arctan2(np.sin(yaw), np.cos(yaw)) half_yaw = yaw / 2.0 - q = np.array([ - np.cos(half_yaw), # w - 0.0, # x - 0.0, # y - np.sin(half_yaw) # z - ]) - q = q / np.linalg.norm(q) - return tuple(q) + w = float(np.cos(half_yaw)) + x = 0.0 + y = 0.0 + z = float(np.sin(half_yaw)) + # Normalize to be safe + norm = float(np.sqrt(w * w + x * x + y * y + z * z)) + if norm == 0.0: + norm = 1.0 + q = Quaternion(w=w / norm, x=x / norm, y=y / norm, z=z / norm) + return q -def main(): - rclpy.init() - - # Initialize main ROS node - node = rclpy.create_node("warehouse_simulation") - - # Create service clients + +def setup_service_clients(node): + """Create and wait for essential service clients, return them as a tuple.""" + logger = logging.getLogger(__name__) set_state_client = node.create_client(SetSimulationState, 'set_simulation_state') reset_client = node.create_client(ResetSimulation, 'reset_simulation') load_world_client = node.create_client(LoadWorld, 'load_world') @@ -39,420 +43,578 @@ def main(): get_entity_state_client = node.create_client(GetEntityState, 'get_entity_state') set_entity_state_client = node.create_client(SetEntityState, 'set_entity_state') get_entity_info_client = node.create_client(GetEntityInfo, 'get_entity_info') + logger.info("Waiting for simulation services...") + while not set_state_client.wait_for_service(timeout_sec=1.0): + logger.debug("set_simulation_state service not available, waiting...") + while not load_world_client.wait_for_service(timeout_sec=1.0): + logger.debug("load_world service not available, waiting...") + while not spawn_entity_client.wait_for_service(timeout_sec=1.0): + logger.debug("spawn_entity service not available, waiting...") + + logger.info("All services ready!") + return ( + set_state_client, + reset_client, + load_world_client, + unload_world_client, + spawn_entity_client, + get_entity_state_client, + set_entity_state_client, + get_entity_info_client, + ) + + +def load_warehouse_world(node, load_world_client, uri: str): + """Load a world by URI and return True on success. + + Args: + node: rclpy node (for spinning) + load_world_client: client for LoadWorld + uri: spawnable URI to load (required) + """ + logger = logging.getLogger(__name__) + logger.info("Loading world: %s", uri) + req = LoadWorld.Request() + req.uri = uri + 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: + logger.info("World loaded successfully: %s", uri) + return True + else: + logger.error("Failed to load world %s: %s", uri, future.result().result.error_message) + return False + + +def unload_world(node, unload_world_client): + """Call UnloadWorld service and return True on success.""" + logger = logging.getLogger(__name__) + logger.info("Unloading 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: + return True + else: + logger.error("Failed to unload world: %s", getattr(future.result().result, 'error_message', 'unknown')) + return False - # Create publishers for robot control - dingo_cmd_vel_pub = None - ur10_joint_pub = None +def spawn_entity(node, spawn_entity_client, name, uri, position, orientation, allow_renaming=True, entity_namespace=None): + """Spawn an entity with the given parameters. - try: - # Wait for services - print("Waiting for simulation services...") - while not set_state_client.wait_for_service(timeout_sec=1.0): - print("set_simulation_state service not available, waiting...") - while not load_world_client.wait_for_service(timeout_sec=1.0): - print("load_world service not available, waiting...") - while not spawn_entity_client.wait_for_service(timeout_sec=1.0): - print("spawn_entity service not available, waiting...") - - print("All services ready!") + Args: + node: rclpy node + spawn_entity_client: client for SpawnEntity + name: entity name (str) + uri: asset URI (str) + position: geometry_msgs.msg.Point + orientation: geometry_msgs.msg.Quaternion + allow_renaming: bool (default True) + entity_namespace: str or None - # Load warehouse world - print("Loading warehouse world...") - req = LoadWorld.Request() - req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_warehouse_with_forklifts/warehouse_with_forklifts.usd") - future = load_world_client.call_async(req) - rclpy.spin_until_future_complete(node, future) + Returns: + True if spawn succeeded, False otherwise. + """ + logger = logging.getLogger(__name__) + req = SpawnEntity.Request() + req.name = name + req.uri = uri + req.allow_renaming = allow_renaming + if entity_namespace: + req.entity_namespace = entity_namespace + req.initial_pose = PoseStamped() + req.initial_pose.header.frame_id = "world" + req.initial_pose.pose.position = position + req.initial_pose.pose.orientation = orientation + future = spawn_entity_client.call_async(req) + rclpy.spin_until_future_complete(node, future) + if future.result() and future.result().result.result == Result.RESULT_OK: + logger.info("Entity '%s' spawned successfully", name) + return True + else: + logger.error("Failed to spawn entity '%s': %s", name, future.result().result.error_message) + return False - if future.result() and future.result().result.result == Result.RESULT_OK: - print("Warehouse world loaded successfully") + +def spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client): + """Spawn the warehouse table and return (x,y,z) or (None,None,None) on failure.""" + logger = logging.getLogger(__name__) + logger.info("Spawning warehouse table...") + position = Point(x=-1.0, y=-1.5, z=1.19) + orientation = yaw_to_quaternion(1.5708) + success = spawn_entity( + node, + spawn_entity_client, + name="warehouse_table", + uri=ACTIVE_TABLE_URI, + position=position, + orientation=orientation, + allow_renaming=False, + entity_namespace=None, + ) + if not success: + logger.error("Failed to spawn table") + return (None, None, None) + else: + logger.info("Table spawned successfully") + + time.sleep(0.5) + + # Get table state for relative positioning + logger.info("Getting table state for relative positioning...") + req = GetEntityState.Request() + req.entity = "warehouse_table" + future = get_entity_state_client.call_async(req) + rclpy.spin_until_future_complete(node, future) + + if future.result() and future.result().result.result == Result.RESULT_OK: + table_state = future.result().state + table_x = table_state.pose.position.x + table_y = table_state.pose.position.y + table_z = table_state.pose.position.z + logger.info("Table found at position: (%.2f, %.2f, %.2f)", table_x, table_y, table_z) + return (table_x, table_y, table_z) + else: + logger.error("Failed to get table state - cannot proceed with relative positioning") + logger.error("Error: %s", future.result().result.error_message) + return (None, None, None) + + +def spawn_cubes_around_table(node, spawn_entity_client, table_x, table_y, table_z): + """Spawn cubes around the table using provided table position.""" + logger = logging.getLogger(__name__) + logger.info("Spawning cubes around table...") + cube_configs = [ + ((table_x + 0.2, table_y + 0.2, table_z + 0.1), "blue"), + ((table_x - 0.2, table_y + 0.2, table_z + 0.1), "red"), + ((table_x + 0.2, table_y - 0.2, table_z + 0.1), "red"), + ((table_x - 0.2, table_y - 0.2, table_z + 0.1), "blue"), + ((table_x, table_y, table_z + 0.1), "blue"), + ((table_x + 0.1, table_y, table_z + 0.1), "red"), + ((table_x - 0.1, table_y, table_z + 0.1), "red"), + ] + + for i, (pos, color) in enumerate(cube_configs): + position_vec = Point(x=float(pos[0]), y=float(pos[1]), z=float(pos[2])) + orientation_quat = Quaternion(w=1.0, x=0.0, y=0.0, z=0.0) + if color == "blue": + uri = ACTIVE_BLUE_CUBE_URI else: - print("Failed to load warehouse world: " + future.result().result.error_message) - return + uri = ACTIVE_RED_CUBE_URI + success = spawn_entity( + node, + spawn_entity_client, + name=f"{color}_cube", + uri=uri, + position=position_vec, + orientation=orientation_quat, + allow_renaming=True, + entity_namespace=None, + ) + if success: + logger.info("%s cube spawned successfully at %s", color.capitalize(), pos) + else: + logger.error("Failed to spawn %s cube at %s", color, pos) - time.sleep(1.0) - # Load a table prop - print("Spawning warehouse table...") - table_req = SpawnEntity.Request() - table_req.name = "warehouse_table" - table_req.uri = os.path.join(DEMO_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(-1.0) - table_req.initial_pose.pose.position.y = float(-1.5) - table_req.initial_pose.pose.position.z = float(1.19) - quat = yaw_to_quaternion(1.5708) - table_req.initial_pose.pose.orientation.w = float(quat[0]) - table_req.initial_pose.pose.orientation.x = float(quat[1]) - table_req.initial_pose.pose.orientation.y = float(quat[2]) - table_req.initial_pose.pose.orientation.z = float(quat[3]) - 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") +def spawn_dingo(node, spawn_entity_client): + """Spawn the Dingo robot and return a cmd_vel publisher or None.""" + logger = logging.getLogger(__name__) + logger.info("Spawning Dingo robot...") + position = Point(x=-4.0, y=-3.0, z=0.0) + orientation = yaw_to_quaternion(0.0) + success = spawn_entity( + node, + spawn_entity_client, + name="dingo_robot", + uri=ACTIVE_DINGO_URI, + position=position, + orientation=orientation, + allow_renaming=False, + entity_namespace="dingo", + ) + if success: + logger.info("Dingo robot spawned successfully") + # Create cmd_vel publisher for dingo + return node.create_publisher(Twist, "/dingo/cmd_vel", 10) + else: + logger.error("Failed to spawn Dingo robot") + return None + + +def spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z): + """Spawn the UR10 robot and return a joint command publisher or None.""" + logger = logging.getLogger(__name__) + logger.info("Spawning UR10 robot...") + position = Point(x=float(table_x), y=float(table_y - 0.64), z=float(table_z)) + orientation = yaw_to_quaternion(-1.5708) + success = spawn_entity( + node, + spawn_entity_client, + name="ur10_robot", + uri=ACTIVE_UR10_URI, + position=position, + orientation=orientation, + allow_renaming=False, + entity_namespace="ur10", + ) + if success: + logger.info("UR10 robot spawned successfully") + # Create joint state publisher for ur10 + return node.create_publisher(JointState, "/ur10/joint_commands", 10) + else: + logger.error("Failed to spawn UR10 robot") + return None + + +def spawn_obstacle_boxes(node, spawn_entity_client, box_positions): + """Spawn obstacle boxes for the provided box_positions list. + + box_positions should be an iterable of tuples: (x, y, z, yaw) + """ + logger = logging.getLogger(__name__) + for i, (box_x, box_y, box_z, box_yaw) in enumerate(box_positions): + position = Point(x=float(box_x), y=float(box_y), z=float(box_z)) + orientation = yaw_to_quaternion(box_yaw) + success = spawn_entity( + node, + spawn_entity_client, + name="obstacle_box", + uri=ACTIVE_CARDBOARD_URI, + position=position, + orientation=orientation, + allow_renaming=True, + entity_namespace=None, + ) + if success: + logger.info("Obstacle box %d spawned at (%.2f, %.2f)", i + 1, box_x, box_y) else: - print("Failed to spawn table: " + future.result().result.error_message) + logger.error("Failed to spawn obstacle box %d", i + 1) + - time.sleep(0.5) +def move_dingo_towards_table(node, get_entity_state_client, dingo_cmd_vel_pub, table_x, table_y): + """Query Dingo state and publish cmd_vel to move it towards the table if far away.""" + logger = logging.getLogger(__name__) + logger.info("Moving Dingo robot towards table...") + req = GetEntityState.Request() + req.entity = "dingo_robot" + future = get_entity_state_client.call_async(req) + + while not future.done(): + rclpy.spin_once(node, timeout_sec=0.1) + time.sleep(0.01) + + if future.result() and future.result().result.result == Result.RESULT_OK: + dingo_state = future.result().state + dingo_x = dingo_state.pose.position.x + dingo_y = dingo_state.pose.position.y + dx = table_x - dingo_x + dy = table_y - dingo_y + distance = np.sqrt(dx * dx + dy * dy) + + if distance > 1.5: + cmd_vel = Twist() + cmd_vel.linear.x = 0.3 + cmd_vel.angular.z = np.arctan2(dy, dx) * 0.5 + for _ in range(20): + dingo_cmd_vel_pub.publish(cmd_vel) + time.sleep(0.1) + cmd_vel.linear.x = 0.0 + cmd_vel.angular.z = 0.0 + for _ in range(5): + dingo_cmd_vel_pub.publish(cmd_vel) + time.sleep(0.1) + + logger.info("Dingo robot moved towards table (distance: %.2fm)", distance) + else: + logger.error("Failed to query Dingo state: %s", future.result().result.error_message) + + +def move_ur10_joints(node, ur10_joint_pub, loop_iteration): + """Publish joint positions for UR10 for the given iteration.""" + logger = logging.getLogger(__name__) + logger.info("Moving UR10 robot joints...") + joint_cmd = JointState() + joint_cmd.header = Header() + joint_cmd.header.stamp = node.get_clock().now().to_msg() + joint_cmd.name = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ] + joint_positions_by_iteration = [ + [0.0, -1.5708, 1.5708, -1.5708, -1.5708, 0.0], + [0.7854, -1.0472, 0.7854, -0.7854, -1.5708, 0.5236], + [-0.5236, -2.0944, 2.0944, -2.0944, -1.5708, -0.7854], + ] + joint_cmd.position = joint_positions_by_iteration[loop_iteration] + for _ in range(10): + ur10_joint_pub.publish(joint_cmd) + time.sleep(0.2) + logger.info("UR10 joint positions updated") - # Get table state to make all other positions relative to it - print("Getting table state for relative positioning...") + +def move_entity_to_location(node, set_entity_state_client, entity, target_x, target_y, target_yaw=1.5708): + """Set an entity's pose (position + orientation) via SetEntityState service. + + Args: + node: rclpy node + set_entity_state_client: client for SetEntityState + entity: str name of the entity to move + target_x, target_y: floats for desired position + target_yaw: yaw angle in radians (default 1.5708) + """ + logger = logging.getLogger(__name__) + logger.info("Moving '%s' to a specific location...", entity) + from simulation_interfaces.msg import EntityState + req = SetEntityState.Request() + req.entity = entity + state = EntityState() + state.pose = Pose() + state.pose.position = Point(x=float(target_x), y=float(target_y), z=float(0.0)) + quat = yaw_to_quaternion(target_yaw) + state.pose.orientation = quat + state.twist = Twist() + state.twist.linear = Vector3(x=0.0, y=0.0, z=0.0) + state.twist.angular = Vector3(x=0.0, y=0.0, z=0.0) + req.state = state + future = set_entity_state_client.call_async(req) + rclpy.spin_until_future_complete(node, future) + if future.result() and future.result().result.result == Result.RESULT_OK: + logger.info("%s moved to new position: (%.2f, %.2f, yaw: %.2f)", entity, target_x, target_y, target_yaw) + return True + else: + logger.error("Failed to move %s to new position: %s", entity, future.result().result.error_message) + return False + + +def set_simulation_state(node, set_state_client, state): + """Set the simulation state to the given state enum value. Returns True on success.""" + logger = logging.getLogger(__name__) + logger.info("Setting simulation state to %d...", state) + req = SetSimulationState.Request() + req.state.state = state + 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: + logger.info("Simulation state set successfully to %d", state) + return True + else: + logger.error("Failed to set simulation state: %s", future.result().result.error_message) + return False + + +def run_simulation_loop( + node, + set_state_client, + spawn_entity_client, + get_entity_state_client, + set_entity_state_client, + unload_world_client, + dingo_cmd_vel_pub, + ur10_joint_pub, + table_x, + table_y, + table_z, +): + """Run the main simulation loop which spawns obstacle boxes, moves robots and handles timing.""" + logger = logging.getLogger(__name__) + # Start simulation + if not set_simulation_state(node, set_state_client, SimulationState.STATE_PLAYING): + return False + time.sleep(2.0) + + # Main simulation loop + for loop_iteration in range(3): + logger.info("=== Loop iteration %d ===", loop_iteration + 1) + + # Get updated table state (in case it moved) + logger.info("Getting table state...") req = GetEntityState.Request() req.entity = "warehouse_table" future = get_entity_state_client.call_async(req) rclpy.spin_until_future_complete(node, future) - + if future.result() and future.result().result.result == Result.RESULT_OK: table_state = future.result().state table_x = table_state.pose.position.x table_y = table_state.pose.position.y table_z = table_state.pose.position.z - print(f"Table found at position: ({table_x:.2f}, {table_y:.2f}, {table_z:.2f})") + logger.info("Table position: (%.2f, %.2f, %.2f)", table_x, table_y, table_z) else: - print("Failed to get table state - cannot proceed with relative positioning") - print("Error: " + future.result().result.error_message) - return + logger.error("Failed to get table state - ending simulation loop") + logger.error("Error: %s", future.result().result.error_message) + break - # Load some cubes around the table top (relative to table position) - mix of red and blue - print("Spawning cubes around table...") - cube_configs = [ - # (position, color) - ((table_x + 0.2, table_y + 0.2, table_z + 0.1), "blue"), # Top right - blue - ((table_x - 0.2, table_y + 0.2, table_z + 0.1), "red"), # Top left - red - ((table_x + 0.2, table_y - 0.2, table_z + 0.1), "red"), # Bottom right - red - ((table_x - 0.2, table_y - 0.2, table_z + 0.1), "blue"), # Bottom left - blue - ((table_x, table_y, table_z + 0.1), "blue"), # Center - blue - ((table_x + 0.1, table_y, table_z + 0.1), "red"), # Center right - red - ((table_x - 0.1, table_y, table_z + 0.1), "red"), # Center left - red + # Spawn cardboard boxes at different positions based on loop iteration + logger.info("Spawning obstacle boxes near table (iteration %d)", loop_iteration + 1) + + box_positions_by_iteration = [ + [ + (table_x - 1.5, table_y - 1.0, 0.0, 0.0), + (table_x + 0.5, table_y - 2.0, 0.0, 1.5708), + (table_x - 0.8, table_y - 1.8, 0.0, 0.7854), + ], + [ + (table_x - 2.0, table_y + 0.5, 0.0, 1.5708), + (table_x + 1.5, table_y - 0.5, 0.0, 0.0), + (table_x + 1.8, table_y + 1.2, 0.0, 0.7854), + ], + [ + (table_x - 0.8, table_y + 1.8, 0.0, 0.7854), + (table_x + 0.3, table_y + 1.5, 0.0, 1.5708), + (table_x - 1.2, table_y + 0.8, 0.0, 0.0), + ], ] - - for i, (pos, color) in enumerate(cube_configs): - cube_req = SpawnEntity.Request() - cube_req.name = f"{color}_cube" - - # Choose asset based on color - if color == "blue": - cube_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_blue_block/blue_block.usd") - else: # red - cube_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_red_block/red_block.usd") - - cube_req.allow_renaming = True - cube_req.initial_pose = PoseStamped() - cube_req.initial_pose.header.frame_id = "world" - cube_req.initial_pose.pose.position.x = float(pos[0]) - cube_req.initial_pose.pose.position.y = float(pos[1]) - cube_req.initial_pose.pose.position.z = float(pos[2]) - cube_req.initial_pose.pose.orientation.w = float(1.0) - cube_req.initial_pose.pose.orientation.x = float(0.0) - cube_req.initial_pose.pose.orientation.y = float(0.0) - cube_req.initial_pose.pose.orientation.z = float(0.0) - future = spawn_entity_client.call_async(cube_req) - rclpy.spin_until_future_complete(node, future) - - if future.result() and future.result().result.result == Result.RESULT_OK: - print(f"{color.capitalize()} cube spawned successfully at {pos}") - else: - print(f"Failed to spawn {color} cube: " + future.result().result.error_message) - time.sleep(0.5) + box_positions = box_positions_by_iteration[loop_iteration] + spawn_obstacle_boxes(node, spawn_entity_client, box_positions) + time.sleep(0.5) - # Load Dingo robot - print("Spawning Dingo robot...") - dingo_req = SpawnEntity.Request() - dingo_req.name = "dingo_robot" - dingo_req.uri = os.path.join(DEMO_ASSET_PATH, "dingo/dingo_ROS.usd") # Assuming dingo asset exists - dingo_req.entity_namespace = "dingo" - dingo_req.allow_renaming = False - dingo_req.initial_pose = PoseStamped() - dingo_req.initial_pose.header.frame_id = "world" - dingo_req.initial_pose.pose.position.x = float(-4.0) # Fixed position - dingo_req.initial_pose.pose.position.y = float(-3.0) # Fixed position - dingo_req.initial_pose.pose.position.z = float(0.0) - quat = yaw_to_quaternion(0.0) # Facing forward - dingo_req.initial_pose.pose.orientation.w = float(quat[0]) - dingo_req.initial_pose.pose.orientation.x = float(quat[1]) - dingo_req.initial_pose.pose.orientation.y = float(quat[2]) - dingo_req.initial_pose.pose.orientation.z = float(quat[3]) - future = spawn_entity_client.call_async(dingo_req) - rclpy.spin_until_future_complete(node, future) - - if future.result() and future.result().result.result == Result.RESULT_OK: - print("Dingo robot spawned successfully") - # Create cmd_vel publisher for dingo - dingo_cmd_vel_pub = node.create_publisher(Twist, "/dingo/cmd_vel", 10) - else: - print("Failed to spawn Dingo robot") + # Move the Dingo robot towards the table + if dingo_cmd_vel_pub: + move_dingo_towards_table( + node, get_entity_state_client, dingo_cmd_vel_pub, table_x, table_y + ) - time.sleep(0.5) + if ur10_joint_pub: + move_ur10_joints(node, ur10_joint_pub, loop_iteration) + time.sleep(0.1) - # Load UR10 robot - print("Spawning UR10 robot...") - ur10_req = SpawnEntity.Request() - ur10_req.name = "ur10_robot" - ur10_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_ur10e_robotiq2f-140_ROS/ur10e_robotiq2f-140_ROS.usd") # Assuming ur10e asset exists - ur10_req.entity_namespace = "ur10" - ur10_req.allow_renaming = False - ur10_req.initial_pose = PoseStamped() - ur10_req.initial_pose.header.frame_id = "world" - ur10_req.initial_pose.pose.position.x = float(table_x) # Match table x position - ur10_req.initial_pose.pose.position.y = float(table_y - 0.64) # Match table y position - ur10_req.initial_pose.pose.position.z = float(table_z) # On top of the table - quat = yaw_to_quaternion(-1.5708) # 180 rotation - ur10_req.initial_pose.pose.orientation.w = float(quat[0]) - ur10_req.initial_pose.pose.orientation.x = float(quat[1]) - ur10_req.initial_pose.pose.orientation.y = float(quat[2]) - ur10_req.initial_pose.pose.orientation.z = float(quat[3]) - future = spawn_entity_client.call_async(ur10_req) - rclpy.spin_until_future_complete(node, future) - - if future.result() and future.result().result.result == Result.RESULT_OK: - print("UR10 robot spawned successfully") - # Create joint state publisher for ur10 - ur10_joint_pub = node.create_publisher(JointState, "/ur10/joint_commands", 10) - else: - print("Failed to spawn UR10 robot") + # Move (set) the Dingo to a specific location relative to the table + new_x = table_x - 3.0 + new_y = table_y - 2.5 + new_yaw = 1.5708 + move_entity_to_location(node, set_entity_state_client, 'dingo_robot', new_x, new_y, new_yaw) + time.sleep(0.1) - time.sleep(1.0) + logger.info("Simulation loop completed!") - # Start simulation - print("Starting simulation...") - 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 started successfully") - else: - print("Failed to start simulation: " + future.result().result.error_message) + # Stop simulation + stopped = set_simulation_state(node, set_state_client, SimulationState.STATE_STOPPED) + if stopped: + logger.info("Simulation stopped successfully") + else: + logger.error("Failed to stop simulation") - time.sleep(2.0) + time.sleep(0.5) - # Main simulation loop - for loop_iteration in range(3): - print(f"\n=== Loop iteration {loop_iteration + 1} ===") - - # Get updated table state (in case it moved) - print("Getting table state...") - req = GetEntityState.Request() - req.entity = "warehouse_table" - future = get_entity_state_client.call_async(req) - - rclpy.spin_until_future_complete(node, future) - - if future.result() and future.result().result.result == Result.RESULT_OK: - table_state = future.result().state - table_x = table_state.pose.position.x - table_y = table_state.pose.position.y - table_z = table_state.pose.position.z - print(f"Table position: ({table_x:.2f}, {table_y:.2f}, {table_z:.2f})") - else: - print("Failed to get table state - ending simulation loop") - print("Error: " + future.result().result.error_message) - break - - # Spawn cardboard boxes at different positions based on loop iteration - print(f"Spawning obstacle boxes near table (iteration {loop_iteration + 1})...") - - # Different box positions for each iteration - box_positions_by_iteration = [ - # Iteration 1: Boxes around the south side - [ - (table_x - 1.5, table_y - 1.0, 0.0, 0.0), # Southwest - (table_x + 0.5, table_y - 2.0, 0.0, 1.5708), # South, rotated 90° - (table_x - 0.8, table_y - 1.8, 0.0, 0.7854), # Southeast, rotated 45° - ], - # Iteration 2: Boxes around the east and west sides - [ - (table_x - 2.0, table_y + 0.5, 0.0, 1.5708), # West, rotated 90° - (table_x + 1.5, table_y - 0.5, 0.0, 0.0), # East - (table_x + 1.8, table_y + 1.2, 0.0, 0.7854), # Northeast, rotated 45° - ], - # Iteration 3: Boxes around the north side - [ - (table_x - 0.8, table_y + 1.8, 0.0, 0.7854), # North, rotated 45° - (table_x + 0.3, table_y + 1.5, 0.0, 1.5708), # Northeast, rotated 90° - (table_x - 1.2, table_y + 0.8, 0.0, 0.0), # Northwest - ] - ] - - box_positions = box_positions_by_iteration[loop_iteration] - - for i, (box_x, box_y, box_z, box_yaw) in enumerate(box_positions): - box_req = SpawnEntity.Request() - box_req.name = "obstacle_box" - box_req.uri = os.path.join(DEMO_ASSET_PATH, "Collected_warehouse_with_forklifts/Props/SM_CardBoxA_02.usd") - box_req.allow_renaming = True - box_req.initial_pose = PoseStamped() - box_req.initial_pose.header.frame_id = "world" - box_req.initial_pose.pose.position.x = float(box_x) - box_req.initial_pose.pose.position.y = float(box_y) - box_req.initial_pose.pose.position.z = float(box_z) - # Specific rotation for each box - quat = yaw_to_quaternion(box_yaw) - box_req.initial_pose.pose.orientation.w = float(quat[0]) - box_req.initial_pose.pose.orientation.x = float(quat[1]) - box_req.initial_pose.pose.orientation.y = float(quat[2]) - box_req.initial_pose.pose.orientation.z = float(quat[3]) - future = spawn_entity_client.call_async(box_req) - rclpy.spin_until_future_complete(node, future) - - if future.result() and future.result().result.result == Result.RESULT_OK: - print(f"Obstacle box {i+1} spawned at ({box_x:.2f}, {box_y:.2f})") - else: - print(f"Failed to spawn obstacle box {i+1}: " + future.result().result.error_message) - - time.sleep(0.5) - - # Move the Dingo robot towards the table - if dingo_cmd_vel_pub: - print("Moving Dingo robot towards table...") - - # Calculate direction to table - req = GetEntityState.Request() - req.entity = "dingo_robot" - future = get_entity_state_client.call_async(req) - - while not future.done(): - rclpy.spin_once(node, timeout_sec=0.1) - time.sleep(0.01) - - if future.result() and future.result().result.result == Result.RESULT_OK: - dingo_state = future.result().state - dingo_x = dingo_state.pose.position.x - dingo_y = dingo_state.pose.position.y - - # Calculate movement towards table - dx = table_x - dingo_x - dy = table_y - dingo_y - distance = np.sqrt(dx*dx + dy*dy) - - if distance > 1.5: # Move closer if too far - # Send velocity commands - cmd_vel = Twist() - cmd_vel.linear.x = 0.3 # Forward speed - cmd_vel.angular.z = np.arctan2(dy, dx) * 0.5 # Turn towards table - - # Send commands for 2 seconds - for _ in range(20): - dingo_cmd_vel_pub.publish(cmd_vel) - time.sleep(0.1) - - # Stop the robot - cmd_vel.linear.x = 0.0 - cmd_vel.angular.z = 0.0 - for _ in range(5): - dingo_cmd_vel_pub.publish(cmd_vel) - time.sleep(0.1) - - print(f"Dingo robot moved towards table (distance: {distance:.2f}m)") - - # Move the UR10 by sending manual joint commands (arm only) - if ur10_joint_pub: - print("Moving UR10 robot joints...") - - joint_cmd = JointState() - joint_cmd.header = Header() - joint_cmd.header.stamp = node.get_clock().now().to_msg() - joint_cmd.name = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", - "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] - - # Predefined joint positions for each iteration - joint_positions_by_iteration = [ - [0.0, -1.5708, 1.5708, -1.5708, -1.5708, 0.0], # Iteration 1: Home position - [0.7854, -1.0472, 0.7854, -0.7854, -1.5708, 0.5236], # Iteration 2: Reach pose - [-0.5236, -2.0944, 2.0944, -2.0944, -1.5708, -0.7854], # Iteration 3: Pickup pose - ] - - # Use specific joint positions for this iteration - joint_cmd.position = joint_positions_by_iteration[loop_iteration] - - # Send joint commands - for _ in range(10): - ur10_joint_pub.publish(joint_cmd) - time.sleep(0.2) - - print("UR10 joint positions updated") - - time.sleep(0.1) - - # Move robot to a specific location relative to table - print("Moving Dingo to a specific location...") - new_x = table_x - 3.0 # 3 meters away from table - new_y = table_y - 2.5 # 2.5 meters in front of table - new_yaw = 1.5708 # Face towards table (90 degrees) - - # Use set_entity_state to teleport the robot to new position - from simulation_interfaces.msg import EntityState - from geometry_msgs.msg import Vector3 - - req = SetEntityState.Request() - req.entity = "dingo_robot" - - state = EntityState() - state.pose = Pose() - state.pose.position = Point(x=float(new_x), y=float(new_y), z=float(0.0)) - quat = yaw_to_quaternion(new_yaw) - state.pose.orientation = Quaternion(w=float(quat[0]), x=float(quat[1]), - y=float(quat[2]), z=float(quat[3])) - state.twist = Twist() - state.twist.linear = Vector3(x=0.0, y=0.0, z=0.0) - state.twist.angular = Vector3(x=0.0, y=0.0, z=0.0) - - req.state = state - - future = set_entity_state_client.call_async(req) - - rclpy.spin_until_future_complete(node, future) - - if future.result() and future.result().result.result == Result.RESULT_OK: - print(f"Dingo moved to new position: ({new_x:.2f}, {new_y:.2f}, yaw: {new_yaw:.2f})") - else: - print("Failed to move Dingo to new position: " + future.result().result.error_message) + # Unload world + if unload_world(node, unload_world_client): + logger.info("World unloaded successfully") + else: + logger.error("Failed to unload world") - time.sleep(0.1) # Wait between iterations + logger.info("Warehouse simulation completed!") + return True - print("\nSimulation loop completed!") - # Stop simulation - print("Stopping simulation...") - req = SetSimulationState.Request() - req.state.state = SimulationState.STATE_STOPPED - future = set_state_client.call_async(req) - rclpy.spin_until_future_complete(node, future) +def main(): + # Configure logging early with a compact, readable format (omit module name) + logging.basicConfig(level=logging.INFO, format='%(levelname)s: %(message)s') + # Parse command-line args to allow runtime selection of asset backend + parser = argparse.ArgumentParser() + parser.add_argument("--asset-backend", choices=["isaac", "o3de"], default="isaac", + help="Choose which asset backend to use (isaac or o3de).") + args, unknown = parser.parse_known_args() + + # Initialize ROS client library + rclpy.init() + + # If Isaac backend requested, map ACTIVE URIs to USD files under DEMO_ASSET_PATH + if args.asset_backend == "isaac": + if not DEMO_ASSET_PATH: + raise RuntimeError("DEMO_ASSET_PATH must be set to use IsaacSim asset backend") + # Override ACTIVE_* URIs to point to USD files + global ACTIVE_TABLE_URI, ACTIVE_BLUE_CUBE_URI, ACTIVE_RED_CUBE_URI, ACTIVE_DINGO_URI, ACTIVE_UR10_URI, ACTIVE_CARDBOARD_URI, ACTIVE_WORLD_URI + ACTIVE_WORLD_URI = os.path.join(DEMO_ASSET_PATH, "Collected_warehouse_with_forklifts/warehouse_with_forklifts.usd") + ACTIVE_TABLE_URI = os.path.join(DEMO_ASSET_PATH, "thor_table/thor_table.usd") + ACTIVE_BLUE_CUBE_URI = os.path.join(DEMO_ASSET_PATH, "Collected_blue_block/blue_block.usd") + ACTIVE_RED_CUBE_URI = os.path.join(DEMO_ASSET_PATH, "Collected_red_block/red_block.usd") + ACTIVE_DINGO_URI = os.path.join(DEMO_ASSET_PATH, "dingo/dingo_ROS.usd") + ACTIVE_UR10_URI = os.path.join(DEMO_ASSET_PATH, "Collected_ur10e_robotiq2f-140_ROS/ur10e_robotiq2f-140_ROS.usd") + ACTIVE_CARDBOARD_URI = os.path.join(DEMO_ASSET_PATH, "Collected_warehouse_with_forklifts/Props/SM_CardBoxA_02.usd") + + if args.asset_backend == "o3de": + ACTIVE_WORLD_URI = "levels/warehouse/warehouse.spawnable" + ACTIVE_TABLE_URI = "product_asset:///assets/props/thortable/thortable.spawnable" + ACTIVE_BLUE_CUBE_URI = "product_asset:///assets/props/collectedblocks/basicblock_blue.spawnable" + ACTIVE_RED_CUBE_URI = "product_asset:///assets/props/collectedblocks/basicblock_red.spawnable" + ACTIVE_DINGO_URI = "product_asset:///assets/dingo/dingo-d.spawnable" + ACTIVE_UR10_URI = "product_asset:///prefabs/ur10-with-fingers.spawnable" + ACTIVE_CARDBOARD_URI = "product_asset:///assets/props/sm_cardboxa_02.spawnable" - if future.result() and future.result().result.result == Result.RESULT_OK: - print("Simulation stopped successfully") - else: - print("Failed to stop simulation: " + future.result().result.error_message) + # Initialize main ROS node + node = rclpy.create_node("warehouse_simulation") - time.sleep(0.5) + # Create publishers for robot control + dingo_cmd_vel_pub = None + ur10_joint_pub = None - # Unload world - print("Unloading world...") - req = UnloadWorld.Request() - future = unload_world_client.call_async(req) - rclpy.spin_until_future_complete(node, future) + try: + # Setup service clients and wait for critical services + ( + set_state_client, + reset_client, + load_world_client, + unload_world_client, + spawn_entity_client, + get_entity_state_client, + set_entity_state_client, + get_entity_info_client, + ) = setup_service_clients(node) + + # Load the warehouse world (explicit URI) + if not load_warehouse_world(node, load_world_client, ACTIVE_WORLD_URI): + return + time.sleep(1.0) + + # Spawn table and get its pose + table_x, table_y, table_z = spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client) + if table_x is None: + return + + # Spawn cubes around the table + spawn_cubes_around_table(node, spawn_entity_client, table_x, table_y, table_z) + time.sleep(0.5) - if future.result() and future.result().result.result == Result.RESULT_OK: - print("World unloaded successfully") - else: - print("Failed to unload world: " + future.result().result.error_message) + # Spawn Dingo robot (returns cmd_vel publisher or None) + dingo_cmd_vel_pub = spawn_dingo(node, spawn_entity_client) + time.sleep(0.5) + + # Spawn UR10 robot (returns joint command publisher or None) + ur10_joint_pub = spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z) + time.sleep(1.0) - print("Warehouse simulation completed!") + # Run the main simulation loop (spawns obstacles, moves robots, stops and unloads) + success = run_simulation_loop( + node, + set_state_client, + spawn_entity_client, + get_entity_state_client, + set_entity_state_client, + unload_world_client, + dingo_cmd_vel_pub, + ur10_joint_pub, + table_x, + table_y, + table_z, + ) + if not success: + logging.getLogger(__name__).error("Simulation loop failed or stopped early") except KeyboardInterrupt: - print("\nInterrupted! Stopping simulation...") - + logger = logging.getLogger(__name__) + logger.info("Interrupted! Stopping simulation...") + # Stop simulation - req = SetSimulationState.Request() - req.state.state = SimulationState.STATE_STOPPED - future = set_state_client.call_async(req) - rclpy.spin_until_future_complete(node, future) - - print("Simulation stopped due to interruption!") + set_simulation_state(node, set_state_client, SimulationState.STATE_STOPPED) + logger.info("Simulation stopped due to interruption!") finally: rclpy.shutdown() From bbee2560c374aae9f6d48d125a861d43365939c7 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Fri, 17 Oct 2025 15:00:45 +0200 Subject: [PATCH 5/7] Rotate robotic arm; add action client Signed-off-by: Jan Hanca --- warehouse_simulation_script.py | 144 ++++++++++++++++++++++----------- 1 file changed, 98 insertions(+), 46 deletions(-) diff --git a/warehouse_simulation_script.py b/warehouse_simulation_script.py index 6e0218f..550fac5 100644 --- a/warehouse_simulation_script.py +++ b/warehouse_simulation_script.py @@ -3,6 +3,9 @@ from simulation_interfaces.msg import Result, SimulationState from geometry_msgs.msg import Twist, PoseStamped, Pose, Point, Quaternion, Vector3 from sensor_msgs.msg import JointState +from rclpy.action import ActionClient +from control_msgs.msg import JointTolerance +from builtin_interfaces.msg import Duration from std_msgs.msg import Header import argparse import os @@ -182,7 +185,7 @@ def spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client) def spawn_cubes_around_table(node, spawn_entity_client, table_x, table_y, table_z): - """Spawn cubes around the table using provided table position.""" + """Spawn cubes around the table using the provided table position.""" logger = logging.getLogger(__name__) logger.info("Spawning cubes around table...") cube_configs = [ @@ -244,11 +247,11 @@ def spawn_dingo(node, spawn_entity_client): def spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z): - """Spawn the UR10 robot and return a joint command publisher or None.""" + """Spawn the UR10 robot on the table using the provided table position.""" logger = logging.getLogger(__name__) logger.info("Spawning UR10 robot...") position = Point(x=float(table_x), y=float(table_y - 0.64), z=float(table_z)) - orientation = yaw_to_quaternion(-1.5708) + orientation = yaw_to_quaternion(1.5708) success = spawn_entity( node, spawn_entity_client, @@ -261,11 +264,8 @@ def spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z): ) if success: logger.info("UR10 robot spawned successfully") - # Create joint state publisher for ur10 - return node.create_publisher(JointState, "/ur10/joint_commands", 10) else: logger.error("Failed to spawn UR10 robot") - return None def spawn_obstacle_boxes(node, spawn_entity_client, box_positions): @@ -331,14 +331,16 @@ def move_dingo_towards_table(node, get_entity_state_client, dingo_cmd_vel_pub, t logger.error("Failed to query Dingo state: %s", future.result().result.error_message) -def move_ur10_joints(node, ur10_joint_pub, loop_iteration): +def move_ur10_joints(node, loop_iteration, sim_backend): """Publish joint positions for UR10 for the given iteration.""" logger = logging.getLogger(__name__) logger.info("Moving UR10 robot joints...") - joint_cmd = JointState() - joint_cmd.header = Header() - joint_cmd.header.stamp = node.get_clock().now().to_msg() - joint_cmd.name = [ + joint_positions_by_iteration = [ + [0.0, -1.5708, 1.5708, -1.5708, -1.5708, 0.0], + [0.7854, -1.0472, 0.7854, -0.7854, -1.5708, 0.5236], + [-0.5236, -2.0944, 2.0944, -2.0944, -1.5708, -0.7854], + ] + joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", @@ -346,18 +348,68 @@ def move_ur10_joints(node, ur10_joint_pub, loop_iteration): "wrist_2_joint", "wrist_3_joint", ] - joint_positions_by_iteration = [ - [0.0, -1.5708, 1.5708, -1.5708, -1.5708, 0.0], - [0.7854, -1.0472, 0.7854, -0.7854, -1.5708, 0.5236], - [-0.5236, -2.0944, 2.0944, -2.0944, -1.5708, -0.7854], - ] - joint_cmd.position = joint_positions_by_iteration[loop_iteration] + positions = joint_positions_by_iteration[loop_iteration] + if sim_backend == "isaac": + move_ur10_joints_topic(node, joint_names, positions) + elif sim_backend == "o3de": + # o3de adds namespace to joint names + joint_names = [f"ur10/{name}" for name in joint_names] + move_ur10_joints_action(node, joint_names, positions) + else: + logger.error("Unknown simulation backend: %s", sim_backend) + + +def move_ur10_joints_topic(node, joint_names, positions): + """Move UR10 joints using JointState publisher.""" + logger = logging.getLogger(__name__) + ur10_joint_pub = node.create_publisher(JointState, "/ur10/joint_commands", 10) + joint_cmd = JointState() + joint_cmd.header = Header() + joint_cmd.header.stamp = node.get_clock().now().to_msg() + joint_cmd.name = joint_names + joint_cmd.position = positions for _ in range(10): ur10_joint_pub.publish(joint_cmd) time.sleep(0.2) - logger.info("UR10 joint positions updated") + logger.info("UR10 joint positions updated (topic)") +def move_ur10_joints_action(node, joint_names, positions): + """Move UR10 joints using FollowJointTrajectory action.""" + logger = logging.getLogger(__name__) + from control_msgs.action import FollowJointTrajectory + from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + action_client = ActionClient(node, FollowJointTrajectory, "/ur10/joint_trajectory_controller/follow_joint_trajectory") + while not action_client.wait_for_server(timeout_sec=1.0): + logging.info("Waiting for action server...") + + goal_msg = FollowJointTrajectory.Goal() + trajectory = JointTrajectory() + trajectory.joint_names = joint_names + point = JointTrajectoryPoint() + point.positions = positions + point.velocities = [0.0] * len(joint_names) + point.effort = [0.0] * len(joint_names) + point.time_from_start = Duration(sec=1, nanosec=0) + trajectory.points.append(point) + goal_tolerance = [JointTolerance(position=0.01) for _ in range(2)] + goal_msg = FollowJointTrajectory.Goal() + goal_msg.trajectory = trajectory + goal_msg.goal_tolerance = goal_tolerance + + future = action_client.send_goal_async(goal_msg) + rclpy.spin_until_future_complete(node, future) + goal_handle = future.result() + if not goal_handle.accepted: + logging.error("Goal rejected") + return + + result_future = goal_handle.get_result_async() + rclpy.spin_until_future_complete(node, result_future) + result = result_future.result().result + logger.info("UR10 trajectory executed (action): %s", result) + + def move_entity_to_location(node, set_entity_state_client, entity, target_x, target_y, target_yaw=1.5708): """Set an entity's pose (position + orientation) via SetEntityState service. @@ -415,12 +467,11 @@ def run_simulation_loop( spawn_entity_client, get_entity_state_client, set_entity_state_client, - unload_world_client, dingo_cmd_vel_pub, - ur10_joint_pub, table_x, table_y, table_z, + sim_backend, ): """Run the main simulation loop which spawns obstacle boxes, moves robots and handles timing.""" logger = logging.getLogger(__name__) @@ -482,8 +533,7 @@ def run_simulation_loop( node, get_entity_state_client, dingo_cmd_vel_pub, table_x, table_y ) - if ur10_joint_pub: - move_ur10_joints(node, ur10_joint_pub, loop_iteration) + move_ur10_joints(node, loop_iteration, sim_backend) time.sleep(0.1) # Move (set) the Dingo to a specific location relative to the table @@ -495,22 +545,6 @@ def run_simulation_loop( logger.info("Simulation loop completed!") - # Stop simulation - stopped = set_simulation_state(node, set_state_client, SimulationState.STATE_STOPPED) - if stopped: - logger.info("Simulation stopped successfully") - else: - logger.error("Failed to stop simulation") - - time.sleep(0.5) - - # Unload world - if unload_world(node, unload_world_client): - logger.info("World unloaded successfully") - else: - logger.error("Failed to unload world") - - logger.info("Warehouse simulation completed!") return True @@ -519,7 +553,7 @@ def main(): logging.basicConfig(level=logging.INFO, format='%(levelname)s: %(message)s') # Parse command-line args to allow runtime selection of asset backend parser = argparse.ArgumentParser() - parser.add_argument("--asset-backend", choices=["isaac", "o3de"], default="isaac", + parser.add_argument("--sim-backend", choices=["isaac", "o3de"], default="isaac", help="Choose which asset backend to use (isaac or o3de).") args, unknown = parser.parse_known_args() @@ -527,7 +561,7 @@ def main(): rclpy.init() # If Isaac backend requested, map ACTIVE URIs to USD files under DEMO_ASSET_PATH - if args.asset_backend == "isaac": + if args.sim_backend == "isaac": if not DEMO_ASSET_PATH: raise RuntimeError("DEMO_ASSET_PATH must be set to use IsaacSim asset backend") # Override ACTIVE_* URIs to point to USD files @@ -539,8 +573,7 @@ def main(): ACTIVE_DINGO_URI = os.path.join(DEMO_ASSET_PATH, "dingo/dingo_ROS.usd") ACTIVE_UR10_URI = os.path.join(DEMO_ASSET_PATH, "Collected_ur10e_robotiq2f-140_ROS/ur10e_robotiq2f-140_ROS.usd") ACTIVE_CARDBOARD_URI = os.path.join(DEMO_ASSET_PATH, "Collected_warehouse_with_forklifts/Props/SM_CardBoxA_02.usd") - - if args.asset_backend == "o3de": + elif args.sim_backend == "o3de": ACTIVE_WORLD_URI = "levels/warehouse/warehouse.spawnable" ACTIVE_TABLE_URI = "product_asset:///assets/props/thortable/thortable.spawnable" ACTIVE_BLUE_CUBE_URI = "product_asset:///assets/props/collectedblocks/basicblock_blue.spawnable" @@ -548,6 +581,8 @@ def main(): ACTIVE_DINGO_URI = "product_asset:///assets/dingo/dingo-d.spawnable" ACTIVE_UR10_URI = "product_asset:///prefabs/ur10-with-fingers.spawnable" ACTIVE_CARDBOARD_URI = "product_asset:///assets/props/sm_cardboxa_02.spawnable" + else: + raise RuntimeError(f"Unknown simulation backend: {args.sim_backend}") # Initialize main ROS node node = rclpy.create_node("warehouse_simulation") @@ -587,8 +622,8 @@ def main(): dingo_cmd_vel_pub = spawn_dingo(node, spawn_entity_client) time.sleep(0.5) - # Spawn UR10 robot (returns joint command publisher or None) - ur10_joint_pub = spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z) + # Spawn UR10 robot + spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z) time.sleep(1.0) # Run the main simulation loop (spawns obstacles, moves robots, stops and unloads) @@ -598,13 +633,30 @@ def main(): spawn_entity_client, get_entity_state_client, set_entity_state_client, - unload_world_client, dingo_cmd_vel_pub, - ur10_joint_pub, table_x, table_y, table_z, + sim_backend=args.sim_backend, ) + + # Stop simulation + logger = logging.getLogger(__name__) + stopped = set_simulation_state(node, set_state_client, SimulationState.STATE_STOPPED) + if stopped: + logger.info("Simulation stopped successfully") + else: + logger.error("Failed to stop simulation") + + time.sleep(0.5) + + # Unload world + if unload_world(node, unload_world_client): + logger.info("World unloaded successfully") + else: + logger.error("Failed to unload world") + + logger.info("Warehouse simulation completed!") if not success: logging.getLogger(__name__).error("Simulation loop failed or stopped early") From 765ef6ebab18b5e4e31d44bee13c57c330fac668 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 20 Oct 2025 17:36:21 -0700 Subject: [PATCH 6/7] Add gazebo specific sim backend checks, and a new function for setting states and stepping sim Signed-off-by: Ian Chen --- warehouse_simulation_script.py | 139 ++++++++++++++++++++++++++------- 1 file changed, 111 insertions(+), 28 deletions(-) diff --git a/warehouse_simulation_script.py b/warehouse_simulation_script.py index 550fac5..b7b598c 100644 --- a/warehouse_simulation_script.py +++ b/warehouse_simulation_script.py @@ -1,12 +1,12 @@ import rclpy -from simulation_interfaces.srv import GetEntityState, SetEntityState, SpawnEntity, DeleteEntity, SetSimulationState, ResetSimulation, GetEntityInfo, LoadWorld, UnloadWorld +from simulation_interfaces.srv import GetEntityState, SetEntityState, SpawnEntity, DeleteEntity, SetSimulationState, ResetSimulation, GetEntityInfo, LoadWorld, UnloadWorld, StepSimulation from simulation_interfaces.msg import Result, SimulationState from geometry_msgs.msg import Twist, PoseStamped, Pose, Point, Quaternion, Vector3 from sensor_msgs.msg import JointState from rclpy.action import ActionClient from control_msgs.msg import JointTolerance from builtin_interfaces.msg import Duration -from std_msgs.msg import Header +from std_msgs.msg import Float64MultiArray, Header import argparse import os import numpy as np @@ -35,22 +35,34 @@ def yaw_to_quaternion(yaw): return q -def setup_service_clients(node): +def setup_service_clients(node, sim_backend): """Create and wait for essential service clients, return them as a tuple.""" + load_world_client = None + unload_world_client = None + if sim_backend != "gazebo": + load_world_client = node.create_client(LoadWorld, 'load_world') + unload_world_client = node.create_client(UnloadWorld, 'unload_world') + logger = logging.getLogger(__name__) - set_state_client = node.create_client(SetSimulationState, 'set_simulation_state') - reset_client = node.create_client(ResetSimulation, 'reset_simulation') - load_world_client = node.create_client(LoadWorld, 'load_world') - unload_world_client = node.create_client(UnloadWorld, 'unload_world') - spawn_entity_client = node.create_client(SpawnEntity, 'spawn_entity') - get_entity_state_client = node.create_client(GetEntityState, 'get_entity_state') - set_entity_state_client = node.create_client(SetEntityState, 'set_entity_state') - get_entity_info_client = node.create_client(GetEntityInfo, 'get_entity_info') + service_prefix_str ='' + if sim_backend == "gazebo": + service_prefix_str = '/gz_server/' + set_state_client = node.create_client(SetSimulationState, service_prefix_str + 'set_simulation_state') + reset_client = node.create_client(ResetSimulation, service_prefix_str + 'reset_simulation') + spawn_entity_client = node.create_client(SpawnEntity, service_prefix_str + 'spawn_entity') + get_entity_state_client = node.create_client(GetEntityState, service_prefix_str + 'get_entity_state') + set_entity_state_client = node.create_client(SetEntityState, service_prefix_str + 'set_entity_state') + get_entity_info_client = node.create_client(GetEntityInfo, service_prefix_str + 'get_entity_info') + step_sim_client = node.create_client(StepSimulation, service_prefix_str + 'step_simulation') + logger.info("Waiting for simulation services...") while not set_state_client.wait_for_service(timeout_sec=1.0): logger.debug("set_simulation_state service not available, waiting...") - while not load_world_client.wait_for_service(timeout_sec=1.0): - logger.debug("load_world service not available, waiting...") + logger.info("Waiting for simulation services2...") + if sim_backend != "gazebo": + while not load_world_client.wait_for_service(timeout_sec=1.0): + logger.debug("load_world service not available, waiting...") + logger.info("Waiting for simulation services3...") while not spawn_entity_client.wait_for_service(timeout_sec=1.0): logger.debug("spawn_entity service not available, waiting...") @@ -64,6 +76,7 @@ def setup_service_clients(node): get_entity_state_client, set_entity_state_client, get_entity_info_client, + step_sim_client, ) @@ -208,7 +221,7 @@ def spawn_cubes_around_table(node, spawn_entity_client, table_x, table_y, table_ success = spawn_entity( node, spawn_entity_client, - name=f"{color}_cube", + name=f"{color}_cube_{i}", uri=uri, position=position_vec, orientation=orientation_quat, @@ -220,6 +233,48 @@ def spawn_cubes_around_table(node, spawn_entity_client, table_x, table_y, table_ else: logger.error("Failed to spawn %s cube at %s", color, pos) +def move_cubes_and_step_sim(node, set_state_client, set_entity_state_client, step_sim_client, table_x, table_y, table_z): + """Moving cubes above the table and stepping simulation for 1 second.""" + logger = logging.getLogger(__name__) + logger.info("Pausing, moving cubes above the table, and stepping simulation...") + + paused = set_simulation_state(node, set_state_client, SimulationState.STATE_PAUSED) + if paused: + logger.info("Simulation paused successfully") + else: + logger.error("Failed to paused simulation") + + time.sleep(0.5) + + cube_configs = [ + ((table_x + 0.1, table_y + 0.1, table_z + 0.1), "blue"), + ((table_x - 0.1, table_y + 0.1, table_z + 0.1), "red"), + ((table_x + 0.1, table_y - 0.1, table_z + 0.1), "red"), + ((table_x - 0.1, table_y - 0.1, table_z + 0.1), "blue"), + ((table_x, table_y, table_z + 0.1), "blue"), + ((table_x + 0.2, table_y, table_z + 0.1), "red"), + ((table_x - 0.2, table_y, table_z + 0.1), "red"), + ] + + for i, (pos, color) in enumerate(cube_configs): + if color == "blue": + uri = ACTIVE_BLUE_CUBE_URI + else: + uri = ACTIVE_RED_CUBE_URI + + z_pos = pos[2] + i * 0.05 + move_entity_to_location(node, set_entity_state_client, f'{color}_cube_{i}', pos[0], pos[1], z_pos, 0.0) + + time.sleep(0.5) + + req = StepSimulation.Request() + req.steps = 1000 + future = step_sim_client.call_async(req) + rclpy.spin_until_future_complete(node, future) + if future.result() and future.result().result.result == Result.RESULT_OK: + logger.info("Successfully stepped simulation") + else: + logger.info("Failed to step simulation") def spawn_dingo(node, spawn_entity_client): """Spawn the Dingo robot and return a cmd_vel publisher or None.""" @@ -355,6 +410,8 @@ def move_ur10_joints(node, loop_iteration, sim_backend): # o3de adds namespace to joint names joint_names = [f"ur10/{name}" for name in joint_names] move_ur10_joints_action(node, joint_names, positions) + if sim_backend == "gazebo": + move_ur10_joint_array_topic(node, joint_names, positions) else: logger.error("Unknown simulation backend: %s", sim_backend) @@ -373,6 +430,16 @@ def move_ur10_joints_topic(node, joint_names, positions): time.sleep(0.2) logger.info("UR10 joint positions updated (topic)") +def move_ur10_joint_array_topic(node, joint_names, positions): + """Move UR10 joints using Float64MultiArray publisher.""" + logger = logging.getLogger(__name__) + ur10_joint_pub = node.create_publisher(Float64MultiArray, "/ur10/joint_commands", 10) + joint_cmd = Float64MultiArray() + joint_cmd.data = positions + for _ in range(10): + ur10_joint_pub.publish(joint_cmd) + time.sleep(0.2) + logger.info("UR10 joint positions updated (topic)") def move_ur10_joints_action(node, joint_names, positions): """Move UR10 joints using FollowJointTrajectory action.""" @@ -410,7 +477,7 @@ def move_ur10_joints_action(node, joint_names, positions): logger.info("UR10 trajectory executed (action): %s", result) -def move_entity_to_location(node, set_entity_state_client, entity, target_x, target_y, target_yaw=1.5708): +def move_entity_to_location(node, set_entity_state_client, entity, target_x, target_y, target_z, target_yaw=1.5708): """Set an entity's pose (position + orientation) via SetEntityState service. Args: @@ -427,7 +494,7 @@ def move_entity_to_location(node, set_entity_state_client, entity, target_x, tar req.entity = entity state = EntityState() state.pose = Pose() - state.pose.position = Point(x=float(target_x), y=float(target_y), z=float(0.0)) + state.pose.position = Point(x=float(target_x), y=float(target_y), z=float(target_z)) quat = yaw_to_quaternion(target_yaw) state.pose.orientation = quat state.twist = Twist() @@ -437,7 +504,7 @@ def move_entity_to_location(node, set_entity_state_client, entity, target_x, tar future = set_entity_state_client.call_async(req) rclpy.spin_until_future_complete(node, future) if future.result() and future.result().result.result == Result.RESULT_OK: - logger.info("%s moved to new position: (%.2f, %.2f, yaw: %.2f)", entity, target_x, target_y, target_yaw) + logger.info("%s moved to new position: (%.2f, %.2f, %.2f, yaw: %.2f)", entity, target_x, target_y, target_z, target_yaw) return True else: logger.error("Failed to move %s to new position: %s", entity, future.result().result.error_message) @@ -539,8 +606,9 @@ def run_simulation_loop( # Move (set) the Dingo to a specific location relative to the table new_x = table_x - 3.0 new_y = table_y - 2.5 + new_z = 0.0 new_yaw = 1.5708 - move_entity_to_location(node, set_entity_state_client, 'dingo_robot', new_x, new_y, new_yaw) + move_entity_to_location(node, set_entity_state_client, 'dingo_robot', new_x, new_y, new_z, new_yaw) time.sleep(0.1) logger.info("Simulation loop completed!") @@ -553,8 +621,8 @@ def main(): logging.basicConfig(level=logging.INFO, format='%(levelname)s: %(message)s') # Parse command-line args to allow runtime selection of asset backend parser = argparse.ArgumentParser() - parser.add_argument("--sim-backend", choices=["isaac", "o3de"], default="isaac", - help="Choose which asset backend to use (isaac or o3de).") + parser.add_argument("--sim-backend", choices=["isaac", "o3de", "gazebo"], default="isaac", + help="Choose which asset backend to use (isaac, o3de or gazebo).") args, unknown = parser.parse_known_args() # Initialize ROS client library @@ -581,6 +649,13 @@ def main(): ACTIVE_DINGO_URI = "product_asset:///assets/dingo/dingo-d.spawnable" ACTIVE_UR10_URI = "product_asset:///prefabs/ur10-with-fingers.spawnable" ACTIVE_CARDBOARD_URI = "product_asset:///assets/props/sm_cardboxa_02.spawnable" + elif args.sim_backend == "gazebo": + ACTIVE_TABLE_URI = os.path.join(DEMO_ASSET_PATH, "thor_table") + ACTIVE_BLUE_CUBE_URI = os.path.join(DEMO_ASSET_PATH, "blue_block") + ACTIVE_RED_CUBE_URI = os.path.join(DEMO_ASSET_PATH, "red_block") + ACTIVE_DINGO_URI = os.path.join(DEMO_ASSET_PATH, "dingo_d") + ACTIVE_UR10_URI = os.path.join(DEMO_ASSET_PATH, "ur10") + ACTIVE_CARDBOARD_URI = os.path.join(DEMO_ASSET_PATH, "cardboard_box") else: raise RuntimeError(f"Unknown simulation backend: {args.sim_backend}") @@ -602,12 +677,14 @@ def main(): get_entity_state_client, set_entity_state_client, get_entity_info_client, - ) = setup_service_clients(node) + step_sim_client, + ) = setup_service_clients(node, sim_backend=args.sim_backend) # Load the warehouse world (explicit URI) - if not load_warehouse_world(node, load_world_client, ACTIVE_WORLD_URI): - return - time.sleep(1.0) + if args.sim_backend != "gazebo": + if not load_warehouse_world(node, load_world_client, ACTIVE_WORLD_URI): + return + time.sleep(1.0) # Spawn table and get its pose table_x, table_y, table_z = spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client) @@ -624,6 +701,11 @@ def main(): # Spawn UR10 robot spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z) + time.sleep(2.0) + + # Move box and set entity state + move_cubes_and_step_sim(node, set_state_client, set_entity_state_client, step_sim_client, table_x, table_y, table_z) + time.sleep(1.0) # Run the main simulation loop (spawns obstacles, moves robots, stops and unloads) @@ -651,10 +733,11 @@ def main(): time.sleep(0.5) # Unload world - if unload_world(node, unload_world_client): - logger.info("World unloaded successfully") - else: - logger.error("Failed to unload world") + if args.sim_backend != "gazebo": + if unload_world(node, unload_world_client): + logger.info("World unloaded successfully") + else: + logger.error("Failed to unload world") logger.info("Warehouse simulation completed!") if not success: From 6c114788f2b7378078b58f124cf2eb15247f4783 Mon Sep 17 00:00:00 2001 From: Ayush Ghosh Date: Tue, 21 Oct 2025 02:42:03 -0700 Subject: [PATCH 7/7] Added box pushing routine and refactored script for isaacsim --- warehouse_simulation_script.py | 353 +++++++++++++++++++++++++++++---- 1 file changed, 313 insertions(+), 40 deletions(-) diff --git a/warehouse_simulation_script.py b/warehouse_simulation_script.py index b7b598c..d1990ba 100644 --- a/warehouse_simulation_script.py +++ b/warehouse_simulation_script.py @@ -35,6 +35,24 @@ def yaw_to_quaternion(yaw): return q +def format_entity_name(entity_name, sim_backend): + """Format entity name according to simulation backend requirements. + + For Isaacsim Sim backend, prefixes entity names with forward slash. + For other backends, returns the name unchanged. + + Args: + entity_name: The entity name to format + sim_backend: The simulation backend being used + + Returns: + Formatted entity name + """ + if sim_backend == "isaacsim": + return f"/{entity_name}" if not entity_name.startswith("/") else entity_name + return entity_name + + def setup_service_clients(node, sim_backend): """Create and wait for essential service clients, return them as a tuple.""" load_world_client = None @@ -116,7 +134,7 @@ def unload_world(node, unload_world_client): logger.error("Failed to unload world: %s", getattr(future.result().result, 'error_message', 'unknown')) return False -def spawn_entity(node, spawn_entity_client, name, uri, position, orientation, allow_renaming=True, entity_namespace=None): +def spawn_entity(node, spawn_entity_client, name, uri, position, orientation, allow_renaming=True, entity_namespace=None, sim_backend="isaacsim"): """Spawn an entity with the given parameters. Args: @@ -128,13 +146,14 @@ def spawn_entity(node, spawn_entity_client, name, uri, position, orientation, al orientation: geometry_msgs.msg.Quaternion allow_renaming: bool (default True) entity_namespace: str or None + sim_backend: simulation backend being used (str) Returns: True if spawn succeeded, False otherwise. """ logger = logging.getLogger(__name__) req = SpawnEntity.Request() - req.name = name + req.name = format_entity_name(name, sim_backend) req.uri = uri req.allow_renaming = allow_renaming if entity_namespace: @@ -153,7 +172,7 @@ def spawn_entity(node, spawn_entity_client, name, uri, position, orientation, al return False -def spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client): +def spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client, sim_backend): """Spawn the warehouse table and return (x,y,z) or (None,None,None) on failure.""" logger = logging.getLogger(__name__) logger.info("Spawning warehouse table...") @@ -168,6 +187,7 @@ def spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client) orientation=orientation, allow_renaming=False, entity_namespace=None, + sim_backend=sim_backend, ) if not success: logger.error("Failed to spawn table") @@ -180,7 +200,7 @@ def spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client) # Get table state for relative positioning logger.info("Getting table state for relative positioning...") req = GetEntityState.Request() - req.entity = "warehouse_table" + req.entity = format_entity_name("warehouse_table", sim_backend) future = get_entity_state_client.call_async(req) rclpy.spin_until_future_complete(node, future) @@ -197,7 +217,7 @@ def spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client) return (None, None, None) -def spawn_cubes_around_table(node, spawn_entity_client, table_x, table_y, table_z): +def spawn_cubes_around_table(node, spawn_entity_client, table_x, table_y, table_z, sim_backend): """Spawn cubes around the table using the provided table position.""" logger = logging.getLogger(__name__) logger.info("Spawning cubes around table...") @@ -227,13 +247,14 @@ def spawn_cubes_around_table(node, spawn_entity_client, table_x, table_y, table_ orientation=orientation_quat, allow_renaming=True, entity_namespace=None, + sim_backend=sim_backend, ) if success: logger.info("%s cube spawned successfully at %s", color.capitalize(), pos) else: logger.error("Failed to spawn %s cube at %s", color, pos) -def move_cubes_and_step_sim(node, set_state_client, set_entity_state_client, step_sim_client, table_x, table_y, table_z): +def move_cubes_and_step_sim(node, set_state_client, set_entity_state_client, step_sim_client, table_x, table_y, table_z, sim_backend): """Moving cubes above the table and stepping simulation for 1 second.""" logger = logging.getLogger(__name__) logger.info("Pausing, moving cubes above the table, and stepping simulation...") @@ -263,12 +284,12 @@ def move_cubes_and_step_sim(node, set_state_client, set_entity_state_client, ste uri = ACTIVE_RED_CUBE_URI z_pos = pos[2] + i * 0.05 - move_entity_to_location(node, set_entity_state_client, f'{color}_cube_{i}', pos[0], pos[1], z_pos, 0.0) + move_entity_to_location(node, set_entity_state_client, f'{color}_cube_{i}', pos[0], pos[1], z_pos, 0.0, sim_backend) time.sleep(0.5) req = StepSimulation.Request() - req.steps = 1000 + req.steps = 100 future = step_sim_client.call_async(req) rclpy.spin_until_future_complete(node, future) if future.result() and future.result().result.result == Result.RESULT_OK: @@ -276,7 +297,7 @@ def move_cubes_and_step_sim(node, set_state_client, set_entity_state_client, ste else: logger.info("Failed to step simulation") -def spawn_dingo(node, spawn_entity_client): +def spawn_dingo(node, spawn_entity_client, sim_backend): """Spawn the Dingo robot and return a cmd_vel publisher or None.""" logger = logging.getLogger(__name__) logger.info("Spawning Dingo robot...") @@ -291,6 +312,7 @@ def spawn_dingo(node, spawn_entity_client): orientation=orientation, allow_renaming=False, entity_namespace="dingo", + sim_backend=sim_backend, ) if success: logger.info("Dingo robot spawned successfully") @@ -301,7 +323,7 @@ def spawn_dingo(node, spawn_entity_client): return None -def spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z): +def spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z, sim_backend): """Spawn the UR10 robot on the table using the provided table position.""" logger = logging.getLogger(__name__) logger.info("Spawning UR10 robot...") @@ -316,6 +338,7 @@ def spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z): orientation=orientation, allow_renaming=False, entity_namespace="ur10", + sim_backend=sim_backend, ) if success: logger.info("UR10 robot spawned successfully") @@ -323,37 +346,47 @@ def spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z): logger.error("Failed to spawn UR10 robot") -def spawn_obstacle_boxes(node, spawn_entity_client, box_positions): +def spawn_obstacle_boxes(node, spawn_entity_client, box_positions, sim_backend, loop_iteration): """Spawn obstacle boxes for the provided box_positions list. box_positions should be an iterable of tuples: (x, y, z, yaw) + loop_iteration: current loop iteration number to include in box names + + Returns: + List of successfully spawned box names """ logger = logging.getLogger(__name__) + spawned_boxes = [] for i, (box_x, box_y, box_z, box_yaw) in enumerate(box_positions): position = Point(x=float(box_x), y=float(box_y), z=float(box_z)) orientation = yaw_to_quaternion(box_yaw) + box_name = f"obstacle_box_{loop_iteration}_{i}" success = spawn_entity( node, spawn_entity_client, - name="obstacle_box", + name=box_name, uri=ACTIVE_CARDBOARD_URI, position=position, orientation=orientation, allow_renaming=True, entity_namespace=None, + sim_backend=sim_backend, ) if success: - logger.info("Obstacle box %d spawned at (%.2f, %.2f)", i + 1, box_x, box_y) + spawned_boxes.append(box_name) + logger.info("Obstacle box %s spawned at (%.2f, %.2f)", box_name, box_x, box_y) else: logger.error("Failed to spawn obstacle box %d", i + 1) + + return spawned_boxes -def move_dingo_towards_table(node, get_entity_state_client, dingo_cmd_vel_pub, table_x, table_y): +def move_dingo_towards_table(node, get_entity_state_client, dingo_cmd_vel_pub, table_x, table_y, sim_backend): """Query Dingo state and publish cmd_vel to move it towards the table if far away.""" logger = logging.getLogger(__name__) logger.info("Moving Dingo robot towards table...") req = GetEntityState.Request() - req.entity = "dingo_robot" + req.entity = format_entity_name("dingo_robot", sim_backend) future = get_entity_state_client.call_async(req) while not future.done(): @@ -386,6 +419,186 @@ def move_dingo_towards_table(node, get_entity_state_client, dingo_cmd_vel_pub, t logger.error("Failed to query Dingo state: %s", future.result().result.error_message) +def push_box(node, get_entity_state_client, set_entity_state_client, dingo_cmd_vel_pub, box_name, sim_backend, push_direction, box_initial_pos=None): + """Push boxes: position Dingo behind box based on direction, use cmd_vel to push box 0.5 meters forward in the specified direction, check progress continuously. + + Args: + node: rclpy node + get_entity_state_client: client for GetEntityState + set_entity_state_client: client for SetEntityState + dingo_cmd_vel_pub: publisher for Dingo cmd_vel + box_name: name of the box to push + sim_backend: simulation backend being used + push_direction: direction to push the box ("+X", "-X", "+Y", "-Y") + box_initial_pos: initial box position (x, y, z) tuple, if None will get current position + + Returns: + tuple: (box_moved_distance_in_direction, box_current_pos) or (None, None) if failed + """ + logger = logging.getLogger(__name__) + + # Get current box state + req = GetEntityState.Request() + # For Isaacsim Sim, use child prim path of cardboard box for state retrieval + if sim_backend == "isaacsim": + req.entity = format_entity_name(f"{box_name}/SM_CardBoxA_02", sim_backend) + else: + req.entity = format_entity_name(box_name, sim_backend) + future = get_entity_state_client.call_async(req) + rclpy.spin_until_future_complete(node, future) + + if not (future.result() and future.result().result.result == Result.RESULT_OK): + logger.error("Failed to get box state for %s: %s", box_name, future.result().result.error_message) + return None, None + + box_state = future.result().state + current_box_x = box_state.pose.position.x + current_box_y = box_state.pose.position.y + current_box_z = box_state.pose.position.z + current_box_pos = (current_box_x, current_box_y, current_box_z) + + # If this is the first time, store initial position and setup + if box_initial_pos is None: + box_initial_pos = current_box_pos + logger.info("Initial box position for %s: (%.2f, %.2f, %.2f)", box_name, current_box_x, current_box_y, current_box_z) + + # Position Dingo behind the box based on push direction (1.5 meters away) + if push_direction == "+X": + dingo_x = current_box_x - 0.75 # Behind the box to push towards +X + dingo_y = current_box_y + dingo_yaw = 0.0 # Facing +X direction (east) + elif push_direction == "-X": + dingo_x = current_box_x + 0.75 # Behind the box to push towards -X + dingo_y = current_box_y + dingo_yaw = 3.14159 # Facing -X direction (west) + elif push_direction == "+Y": + dingo_x = current_box_x + dingo_y = current_box_y - 0.75 # Behind the box to push towards +Y + dingo_yaw = 1.5708 # Facing +Y direction (north) + elif push_direction == "-Y": + dingo_x = current_box_x + dingo_y = current_box_y + 0.75 # Behind the box to push towards -Y + dingo_yaw = -1.5708 # Facing -Y direction (south) + else: + logger.error("Invalid push direction: %s. Must be +X, -X, +Y, or -Y", push_direction) + return None, None + + dingo_z = 0.05 + + if sim_backend == "isaacsim": + dingo_entity_name = "dingo_robot/base_link" + else: + dingo_entity_name = "dingo_robot" + success = move_entity_to_location(node, set_entity_state_client, dingo_entity_name, dingo_x, dingo_y, dingo_z, dingo_yaw, sim_backend) + if success: + logger.info("Positioned Dingo behind box %s at (%.2f, %.2f) to push in %s direction", box_name, dingo_x, dingo_y, push_direction) + else: + logger.error("Failed to position Dingo behind box %s", box_name) + return None, None + + time.sleep(1.0) # Wait a moment for positioning to settle + + # Start pushing and monitor continuously until target is reached + logger.info("Starting continuous box pushing with cmd_vel in %s direction...", push_direction) + cmd_vel = Twist() + cmd_vel.linear.x = 0.2 # Move forward (Dingo orientation determines the actual push direction) + cmd_vel.angular.z = 0.0 + + # Continuous pushing loop - monitor box position until 1.0m moved + max_iterations = 500 # Safety limit (50 seconds at 0.1s intervals) + iteration = 0 + + while iteration < max_iterations: + # Publish cmd_vel to keep pushing + if dingo_cmd_vel_pub: + dingo_cmd_vel_pub.publish(cmd_vel) + + # Check box position every few iterations to avoid overwhelming the system + if iteration % 5 == 0: # Check every 0.5 seconds + # Get updated box position + req = GetEntityState.Request() + # For Isaacsim Sim, use child prim path of cardboard box for state retrieval + if sim_backend == "isaacsim": + req.entity = format_entity_name(f"{box_name}/SM_CardBoxA_02", sim_backend) + else: + req.entity = format_entity_name(box_name, sim_backend) + future = get_entity_state_client.call_async(req) + rclpy.spin_until_future_complete(node, future) + + if future.result() and future.result().result.result == Result.RESULT_OK: + box_state = future.result().state + current_box_x = box_state.pose.position.x + current_box_y = box_state.pose.position.y + current_box_z = box_state.pose.position.z + current_box_pos = (current_box_x, current_box_y, current_box_z) + + # Calculate distance moved in the specified direction + if push_direction == "+X": + displacement = current_box_x - box_initial_pos[0] # Positive when moved in +X direction + elif push_direction == "-X": + displacement = box_initial_pos[0] - current_box_x # Positive when moved in -X direction + elif push_direction == "+Y": + displacement = current_box_y - box_initial_pos[1] # Positive when moved in +Y direction + elif push_direction == "-Y": + displacement = box_initial_pos[1] - current_box_y # Positive when moved in -Y direction + else: + displacement = 0 + + # Calculate the distance the box has moved in the intended direction so we know when to stop pushing. + distance_moved = max(0, displacement) + logger.info( + "Box %s has moved %.2f meters out of 1.0 meter goal in the %s direction", + box_name, distance_moved, push_direction + ) + + # Check if target has moved 0.5 meters + if distance_moved >= 0.5: + logger.info("Target reached! Box %s moved %.2f meters. Stopping Dingo.", box_name, distance_moved) + + # Stop Dingo cmd_vel + if dingo_cmd_vel_pub: + stop_cmd = Twist() + stop_cmd.linear.x = 0.0 + stop_cmd.angular.z = 0.0 + for _ in range(10): + dingo_cmd_vel_pub.publish(stop_cmd) + time.sleep(0.1) + + return distance_moved, current_box_pos + else: + logger.error("Failed to get updated box state during pushing") + + time.sleep(0.1) # 10Hz update rate + iteration += 1 + + # Safety timeout reached + logger.warning("Box pushing timeout reached after %.1f seconds", max_iterations * 0.1) + if dingo_cmd_vel_pub: + stop_cmd = Twist() + stop_cmd.linear.x = 0.0 + stop_cmd.angular.z = 0.0 + for _ in range(10): + dingo_cmd_vel_pub.publish(stop_cmd) + time.sleep(0.1) + + return distance_moved if 'distance_moved' in locals() else 0.0, current_box_pos + + # If box_initial_pos is provided, this means pushing is already complete + if push_direction == "+X": + displacement = current_box_x - box_initial_pos[0] # Positive when moved in +X direction + elif push_direction == "-X": + displacement = box_initial_pos[0] - current_box_x # Positive when moved in -X direction + elif push_direction == "+Y": + displacement = current_box_y - box_initial_pos[1] # Positive when moved in +Y direction + elif push_direction == "-Y": + displacement = box_initial_pos[1] - current_box_y # Positive when moved in -Y direction + else: + displacement = 0 + + distance_moved = max(0, displacement) # Only count movement in the specified direction + return distance_moved, current_box_pos + + def move_ur10_joints(node, loop_iteration, sim_backend): """Publish joint positions for UR10 for the given iteration.""" logger = logging.getLogger(__name__) @@ -404,13 +617,13 @@ def move_ur10_joints(node, loop_iteration, sim_backend): "wrist_3_joint", ] positions = joint_positions_by_iteration[loop_iteration] - if sim_backend == "isaac": + if sim_backend == "isaacsim": move_ur10_joints_topic(node, joint_names, positions) elif sim_backend == "o3de": # o3de adds namespace to joint names joint_names = [f"ur10/{name}" for name in joint_names] move_ur10_joints_action(node, joint_names, positions) - if sim_backend == "gazebo": + elif sim_backend == "gazebo": move_ur10_joint_array_topic(node, joint_names, positions) else: logger.error("Unknown simulation backend: %s", sim_backend) @@ -477,7 +690,7 @@ def move_ur10_joints_action(node, joint_names, positions): logger.info("UR10 trajectory executed (action): %s", result) -def move_entity_to_location(node, set_entity_state_client, entity, target_x, target_y, target_z, target_yaw=1.5708): +def move_entity_to_location(node, set_entity_state_client, entity, target_x, target_y, target_z, target_yaw=1.5708, sim_backend="isaacsim"): """Set an entity's pose (position + orientation) via SetEntityState service. Args: @@ -486,12 +699,13 @@ def move_entity_to_location(node, set_entity_state_client, entity, target_x, tar entity: str name of the entity to move target_x, target_y: floats for desired position target_yaw: yaw angle in radians (default 1.5708) + sim_backend: simulation backend being used (str) """ logger = logging.getLogger(__name__) logger.info("Moving '%s' to a specific location...", entity) from simulation_interfaces.msg import EntityState req = SetEntityState.Request() - req.entity = entity + req.entity = format_entity_name(entity, sim_backend) state = EntityState() state.pose = Pose() state.pose.position = Point(x=float(target_x), y=float(target_y), z=float(target_z)) @@ -547,6 +761,19 @@ def run_simulation_loop( return False time.sleep(2.0) + # Hardcoded box selection and push directions for each iteration + box_push_configs = [ + ("obstacle_box_0_1", "-X"), # Iteration 0: push obstacle_box_0_1 in -X direction + ("obstacle_box_1_1", "+Y"), # Iteration 1: push obstacle_box_1_1 in +Y direction + ("obstacle_box_2_0", "+X"), # Iteration 2: push obstacle_box_2_1 in -Y direction + ] + + # Track box pushing across iterations + selected_box_name = None + selected_direction = None + box_initial_position = None + box_movement_complete = False + # Main simulation loop for loop_iteration in range(3): logger.info("=== Loop iteration %d ===", loop_iteration + 1) @@ -554,7 +781,7 @@ def run_simulation_loop( # Get updated table state (in case it moved) logger.info("Getting table state...") req = GetEntityState.Request() - req.entity = "warehouse_table" + req.entity = format_entity_name("warehouse_table", sim_backend) future = get_entity_state_client.call_async(req) rclpy.spin_until_future_complete(node, future) @@ -591,24 +818,70 @@ def run_simulation_loop( ] box_positions = box_positions_by_iteration[loop_iteration] - spawn_obstacle_boxes(node, spawn_entity_client, box_positions) + spawned_boxes = spawn_obstacle_boxes(node, spawn_entity_client, box_positions, sim_backend, loop_iteration) time.sleep(0.5) - # Move the Dingo robot towards the table - if dingo_cmd_vel_pub: - move_dingo_towards_table( - node, get_entity_state_client, dingo_cmd_vel_pub, table_x, table_y + # Select box and direction based on current iteration + if spawned_boxes and loop_iteration < len(box_push_configs): + target_box, target_direction = box_push_configs[loop_iteration] + + if target_box in spawned_boxes: + current_iteration_box = target_box + current_iteration_direction = target_direction + logger.info("Selected box %s for pushing in %s direction (iteration %d)", current_iteration_box, current_iteration_direction, loop_iteration + 1) + else: + logger.warning("%s not found, falling back to first available box", target_box) + current_iteration_box = spawned_boxes[0] + current_iteration_direction = target_direction # Keep the planned direction + logger.info("Selected fallback box %s for pushing in %s direction", current_iteration_box, current_iteration_direction) + + # If this is a new box or first time, set it as selected and reset pushing state + if current_iteration_box != selected_box_name: + selected_box_name = current_iteration_box + selected_direction = current_iteration_direction + box_initial_position = None + box_movement_complete = False + + # Handle box pushing instead of moving towards table (only when new box selected) + if selected_box_name and not box_movement_complete and box_initial_position is None: + logger.info("Starting box pushing for %s in %s direction", selected_box_name, selected_direction) + distance_moved, current_pos = push_box( + node, get_entity_state_client, set_entity_state_client, dingo_cmd_vel_pub, + selected_box_name, sim_backend, selected_direction, box_initial_position ) + + if distance_moved is not None: + box_initial_position = current_pos + + # Mark as complete if box has been pushed 0.5 meters + if distance_moved >= 0.5: + box_movement_complete = True + logger.info("Box pushing completed! Box moved %.2f meters.", distance_moved) + else: + # This shouldn't happen with the new continuous monitoring, but just in case + logger.warning("Box pushing ended prematurely with only %.2f meters moved", distance_moved) + box_movement_complete = True + else: + logger.error("Failed to push boxes") + box_movement_complete = True # Mark as complete to avoid retrying move_ur10_joints(node, loop_iteration, sim_backend) time.sleep(0.1) - # Move (set) the Dingo to a specific location relative to the table - new_x = table_x - 3.0 - new_y = table_y - 2.5 - new_z = 0.0 - new_yaw = 1.5708 - move_entity_to_location(node, set_entity_state_client, 'dingo_robot', new_x, new_y, new_z, new_yaw) + # Only move Dingo to specific location if box movement is complete + if box_movement_complete: + logger.info("Box movement complete, positioning Dingo at final location") + new_x = table_x - 3.0 + new_y = table_y - 2.5 + new_z = 0.0 + new_yaw = 1.5708 + # Use "dingo_robot/base_link" for Isaacsim, otherwise "dingo_robot" + if sim_backend == "isaacsim": + dingo_entity_name = "dingo_robot/base_link" + else: + dingo_entity_name = "dingo_robot" + move_entity_to_location(node, set_entity_state_client, dingo_entity_name, new_x, new_y, new_z, new_yaw, sim_backend) + time.sleep(0.1) logger.info("Simulation loop completed!") @@ -621,15 +894,15 @@ def main(): logging.basicConfig(level=logging.INFO, format='%(levelname)s: %(message)s') # Parse command-line args to allow runtime selection of asset backend parser = argparse.ArgumentParser() - parser.add_argument("--sim-backend", choices=["isaac", "o3de", "gazebo"], default="isaac", - help="Choose which asset backend to use (isaac, o3de or gazebo).") + parser.add_argument("--sim-backend", choices=["isaacsim", "o3de", "gazebo"], default="isaacsim", + help="Choose which asset backend to use (isaacsim, o3de or gazebo).") args, unknown = parser.parse_known_args() # Initialize ROS client library rclpy.init() - # If Isaac backend requested, map ACTIVE URIs to USD files under DEMO_ASSET_PATH - if args.sim_backend == "isaac": + # If Isaacsim backend requested, map ACTIVE URIs to USD files under DEMO_ASSET_PATH + if args.sim_backend == "isaacsim": if not DEMO_ASSET_PATH: raise RuntimeError("DEMO_ASSET_PATH must be set to use IsaacSim asset backend") # Override ACTIVE_* URIs to point to USD files @@ -687,24 +960,24 @@ def main(): time.sleep(1.0) # Spawn table and get its pose - table_x, table_y, table_z = spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client) + table_x, table_y, table_z = spawn_table_and_get_pose(node, spawn_entity_client, get_entity_state_client, args.sim_backend) if table_x is None: return # Spawn cubes around the table - spawn_cubes_around_table(node, spawn_entity_client, table_x, table_y, table_z) + spawn_cubes_around_table(node, spawn_entity_client, table_x, table_y, table_z, args.sim_backend) time.sleep(0.5) # Spawn Dingo robot (returns cmd_vel publisher or None) - dingo_cmd_vel_pub = spawn_dingo(node, spawn_entity_client) + dingo_cmd_vel_pub = spawn_dingo(node, spawn_entity_client, args.sim_backend) time.sleep(0.5) # Spawn UR10 robot - spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z) + spawn_ur10(node, spawn_entity_client, table_x, table_y, table_z, args.sim_backend) time.sleep(2.0) # Move box and set entity state - move_cubes_and_step_sim(node, set_state_client, set_entity_state_client, step_sim_client, table_x, table_y, table_z) + move_cubes_and_step_sim(node, set_state_client, set_entity_state_client, step_sim_client, table_x, table_y, table_z, args.sim_backend) time.sleep(1.0)