#!/usr/bin/env python3
"""
Advanced Features Example - Phase 4 Complete
Demonstrates:
1. Mesh loading (custom 3D models)
2. Grid-based object placement
3. Batch spawning for performance
This example shows how to use the advanced world generation features
for creating complex environments efficiently.
Requirements:
- ROS2 Humble
- Gazebo (for spawning examples)
- Custom mesh files (examples use placeholder paths)
"""
import sys
from pathlib import Path
# Add the src directory to Python path
src_path = Path(__file__).parent.parent / "src"
sys.path.insert(0, str(src_path))
from gazebo_mcp.tools import world_generation
from gazebo_mcp.common.result import OperationResult
def print_result(result: OperationResult, operation: str):
"""Helper to print operation results."""
if result.success:
print(f"✓ {operation} succeeded")
if "total_objects" in result.data:
print(f" Total objects: {result.data['total_objects']}")
if "spawned" in result.data and isinstance(result.data["spawned"], int):
print(f" Spawned: {result.data['spawned']}/{result.data.get('total', 0)}")
if "failed" in result.data and isinstance(result.data["failed"], int):
print(f" Failed: {result.data['failed']}")
else:
print(f"✗ {operation} failed: {result.error}")
if result.suggestions:
print(f" Suggestions: {', '.join(result.suggestions)}")
def main():
print("=" * 70)
print("Advanced Features Example - Phase 4 Complete")
print("=" * 70)
print()
# ===========================================================================
# Part 1: Mesh Loading (Offline - No Gazebo Required)
# ===========================================================================
print("Part 1: Mesh Loading - Generate SDF for Custom Models")
print("-" * 70)
print()
print("1.1. Basic mesh placement (robot model)")
print(" Generates SDF for a custom .dae robot mesh")
print()
result = world_generation.place_mesh(
name="custom_robot",
mesh_file="models/turtlebot3_burger/meshes/burger_base.dae",
x=0.0,
y=0.0,
z=0.1,
scale=1.0,
static=False,
mass=1.5
)
print_result(result, "Generate robot mesh SDF")
if result.success:
print(f" Generated SDF length: {len(result.data['sdf_content'])} chars")
print(f" Mesh file: {result.data['mesh_file']}")
print(f" Scale: {result.data['scale']}")
print()
print("1.2. Mesh with separate collision geometry")
print(" Uses simplified collision mesh for better performance")
print()
result = world_generation.place_mesh(
name="complex_building",
mesh_file="models/building/building_visual.dae",
collision_mesh_file="models/building/building_collision.stl",
x=10.0,
y=10.0,
z=0.0,
roll=0.0,
pitch=0.0,
yaw=1.57, # 90 degrees rotation
scale=2.0,
static=True
)
print_result(result, "Generate building mesh SDF (separate collision)")
if result.success:
print(f" Visual mesh: {result.data['mesh_file']}")
print(f" Collision mesh: {result.data['collision_mesh_file']}")
print(f" Orientation: roll={result.data['orientation']['roll']:.2f}, "
f"pitch={result.data['orientation']['pitch']:.2f}, "
f"yaw={result.data['orientation']['yaw']:.2f}")
print()
print("1.3. Invalid mesh format handling")
print(" Demonstrates error handling for unsupported formats")
print()
result = world_generation.place_mesh(
name="invalid_model",
mesh_file="models/model.txt", # Invalid format
x=0, y=0, z=0
)
print_result(result, "Generate mesh with invalid format")
print()
# ===========================================================================
# Part 2: Grid-Based Placement (Offline)
# ===========================================================================
print("\nPart 2: Grid-Based Object Placement")
print("-" * 70)
print()
print("2.1. Create a 3x3 grid of boxes (obstacle course)")
print(" Automatically arranges objects in a grid pattern")
print()
result = world_generation.place_grid(
object_type="box",
rows=3,
cols=3,
spacing=2.0, # 2 meters between centers
offset_x=0.0,
offset_y=0.0,
offset_z=0.5, # Half meter above ground
object_params={
"width": 1.0,
"height": 1.0,
"depth": 1.0,
"static": True,
"color": {"r": 0.8, "g": 0.2, "b": 0.2, "a": 1.0} # Red boxes
}
)
print_result(result, "Generate 3x3 box grid")
if result.success:
print(f" Grid size: {result.data['rows']}x{result.data['cols']}")
print(f" Spacing: {result.data['spacing']} meters")
print(f" Objects generated: {result.data['total_objects']}")
# Show first object as example
if result.data['objects']:
first_obj = result.data['objects'][0]
print(f" First object: {first_obj['name']} at ({first_obj['position']['x']}, "
f"{first_obj['position']['y']}, {first_obj['position']['z']})")
print()
print("2.2. Create a 2x4 grid of spheres (slalom course)")
print(" Offset from origin with custom color")
print()
result = world_generation.place_grid(
object_type="sphere",
rows=2,
cols=4,
spacing=1.5,
offset_x=10.0,
offset_y=5.0,
offset_z=0.3,
object_params={
"radius": 0.3,
"static": True,
"color": {"r": 0.0, "g": 0.8, "b": 1.0, "a": 1.0} # Cyan spheres
}
)
print_result(result, "Generate 2x4 sphere grid")
if result.success:
print(f" Grid offset: ({result.data['offset']['x']}, "
f"{result.data['offset']['y']}, {result.data['offset']['z']})")
print(f" Total spheres: {result.data['total_objects']}")
print()
print("2.3. Grid with invalid object type")
print(" Demonstrates validation of object types")
print()
result = world_generation.place_grid(
object_type="pyramid", # Invalid type
rows=2,
cols=2,
spacing=2.0
)
print_result(result, "Generate grid with invalid type")
print()
# ===========================================================================
# Part 3: Batch Spawning (Online - Requires Gazebo)
# ===========================================================================
print("\nPart 3: Batch Spawning - Efficient Multiple Object Spawning")
print("-" * 70)
print()
# Check if user wants to run online examples
try:
user_input = input("Do you have Gazebo running? (yes/no): ").strip().lower()
has_gazebo = user_input in ["yes", "y"]
except EOFError:
print("Running in non-interactive mode. Skipping online examples.")
has_gazebo = False
if has_gazebo:
print("\n3.1. Spawn multiple objects in batch")
print(" More efficient than spawning one by one")
print()
# Define a list of objects to spawn
objects_to_spawn = [
{
"type": "box",
"name": "obstacle_1",
"position": {"x": 1, "y": 0, "z": 0.5},
"params": {
"width": 1.0,
"height": 1.0,
"depth": 1.0,
"static": True,
"color": {"r": 1.0, "g": 0.0, "b": 0.0, "a": 1.0}
}
},
{
"type": "sphere",
"name": "ball_1",
"position": {"x": 3, "y": 0, "z": 0.5},
"params": {
"radius": 0.5,
"static": False, # Physics-enabled ball
"color": {"r": 0.0, "g": 1.0, "b": 0.0, "a": 1.0}
}
},
{
"type": "cylinder",
"name": "pole_1",
"position": {"x": 5, "y": 0, "z": 1.0},
"params": {
"radius": 0.2,
"length": 2.0,
"static": True,
"color": {"r": 0.0, "g": 0.0, "b": 1.0, "a": 1.0}
}
},
{
"type": "box",
"name": "obstacle_2",
"position": {"x": 7, "y": 0, "z": 0.5},
"params": {
"width": 1.0,
"height": 1.0,
"depth": 1.0,
"static": True
}
}
]
result = world_generation.spawn_multiple(
objects=objects_to_spawn,
continue_on_error=True, # Keep going even if some fail
timeout=10.0
)
print_result(result, "Batch spawn 4 objects")
if result.success:
print(f" Success rate: {result.data['spawned']}/{result.data['total']} "
f"({100*result.data['spawned']/result.data['total']:.0f}%)")
# Show individual results
print("\n Individual results:")
for obj_result in result.data['results']:
status = "✓" if obj_result['success'] else "✗"
name = obj_result['name']
obj_type = obj_result.get('type', 'unknown')
print(f" {status} {name} ({obj_type})")
if not obj_result['success']:
print(f" Error: {obj_result.get('error', 'Unknown error')}")
print()
print("\n3.2. Spawn mesh objects in batch")
print(" Demonstrates batch spawning with custom meshes")
print()
mesh_objects = [
{
"type": "mesh",
"name": "robot_1",
"position": {"x": -2, "y": 0, "z": 0.1},
"params": {
"mesh_file": "models/turtlebot3_burger/meshes/burger_base.dae",
"scale": 1.0,
"static": False,
"mass": 1.5
}
},
{
"type": "mesh",
"name": "robot_2",
"position": {"x": -2, "y": 2, "z": 0.1},
"params": {
"mesh_file": "models/turtlebot3_burger/meshes/burger_base.dae",
"scale": 1.0,
"static": False,
"mass": 1.5
}
}
]
result = world_generation.spawn_multiple(
objects=mesh_objects,
continue_on_error=True,
timeout=10.0
)
print_result(result, "Batch spawn 2 robot meshes")
print()
print("\n3.3. Spawn individual mesh (alternative to batch)")
print(" Direct spawning of a single mesh object")
print()
result = world_generation.spawn_mesh(
name="single_robot",
mesh_file="models/turtlebot3_burger/meshes/burger_base.dae",
x=-2.0,
y=-2.0,
z=0.1,
scale=1.0,
static=False,
mass=1.5,
timeout=10.0
)
print_result(result, "Spawn single robot mesh")
if result.success:
print(f" Spawned: {result.data['name']}")
print(f" Position: ({result.data['position']['x']}, "
f"{result.data['position']['y']}, {result.data['position']['z']})")
print()
else:
print("\nSkipping Part 3 (Gazebo not running)")
print("To run this part:")
print(" 1. Terminal 1: ros2 launch gazebo_ros gazebo.launch.py")
print(" 2. Terminal 2: python examples/06_advanced_features.py")
print()
# ===========================================================================
# Part 4: Combined Example - Complete Test Environment
# ===========================================================================
print("\nPart 4: Combined Example - Complete Test Environment")
print("-" * 70)
print()
print("4.1. Generate a complete test environment with all features")
print(" Combines grids, meshes, and individual objects")
print()
# Step 1: Create grid of obstacles
print(" Step 1: Creating obstacle grid...")
grid_result = world_generation.place_grid(
object_type="box",
rows=5,
cols=5,
spacing=3.0,
offset_x=-6.0,
offset_y=-6.0,
offset_z=0.5,
object_params={
"width": 0.5,
"height": 0.5,
"depth": 0.5,
"static": True,
"color": {"r": 0.5, "g": 0.5, "b": 0.5, "a": 1.0}
}
)
if grid_result.success:
print(f" ✓ Created {grid_result.data['total_objects']} obstacles in grid")
# Save to file
save_result = world_generation.save_world(
world_name="test_environment",
sdf_content=grid_result.data['objects'][0]['sdf_content'],
file_path="/tmp/test_environment_grid.sdf"
)
if save_result.success:
print(f" ✓ Saved grid to {save_result.data['file_path']}")
else:
print(f" ✗ Failed to create grid: {grid_result.error}")
# Step 2: Generate target zones (spheres)
print("\n Step 2: Creating target zones...")
targets_result = world_generation.place_grid(
object_type="sphere",
rows=1,
cols=4,
spacing=4.0,
offset_x=-6.0,
offset_y=10.0,
offset_z=0.3,
object_params={
"radius": 0.5,
"static": True,
"color": {"r": 0.0, "g": 1.0, "b": 0.0, "a": 0.6} # Semi-transparent green
}
)
if targets_result.success:
print(f" ✓ Created {targets_result.data['total_objects']} target zones")
else:
print(f" ✗ Failed to create targets: {targets_result.error}")
print("\n Summary:")
print(f" - Total objects generated: {(grid_result.data.get('total_objects', 0) +
targets_result.data.get('total_objects', 0))}")
print(f" - Obstacle grid: 5x5 boxes")
print(f" - Target zones: 4 spheres")
print(f" - All objects ready for spawning or saving")
print()
# ===========================================================================
# Summary
# ===========================================================================
print("\n" + "=" * 70)
print("Summary - Advanced Features Demonstrated")
print("=" * 70)
print()
print("1. Mesh Loading:")
print(" ✓ Load custom 3D models (.dae, .stl, .obj)")
print(" ✓ Separate collision meshes for performance")
print(" ✓ Custom scaling and orientation")
print(" ✓ Support for both static and dynamic objects")
print()
print("2. Grid-Based Placement:")
print(" ✓ Automatic object arrangement in grids")
print(" ✓ Configurable rows, columns, and spacing")
print(" ✓ Custom origin offset")
print(" ✓ Works with boxes, spheres, and cylinders")
print()
print("3. Batch Spawning:")
print(" ✓ Efficient spawning of multiple objects")
print(" ✓ Continue-on-error option for robustness")
print(" ✓ Detailed success/failure tracking")
print(" ✓ Support for all object types (including meshes)")
print()
print("4. Integration:")
print(" ✓ Seamless offline (generation) and online (spawning) modes")
print(" ✓ Complete error handling with helpful suggestions")
print(" ✓ Production-ready quality and performance")
print()
print("Phase 4 Complete: 100% Functionality Achieved! 🎉")
print("=" * 70)
if __name__ == "__main__":
try:
main()
except KeyboardInterrupt:
print("\n\nInterrupted by user")
except Exception as e:
print(f"\nError: {e}")
import traceback
traceback.print_exc()
