diff --git a/.dockerignore b/.dockerignore
deleted file mode 100644
index 44f7239..0000000
--- a/.dockerignore
+++ /dev/null
@@ -1,62 +0,0 @@
-# Python
-__pycache__/
-*.py[cod]
-*$py.class
-*.so
-.Python
-*.egg-info/
-dist/
-build/
-*.egg
-
-# Virtual environments
-venv/
-env/
-ENV/
-.venv/
-.conda/
-
-# IDE
-.vscode/
-.idea/
-*.swp
-*.swo
-*~
-
-# Testing
-.pytest_cache/
-.coverage
-htmlcov/
-.tox/
-
-# Outputs
-outputs/
-logs/
-*.log
-
-# Git
-.git/
-.gitignore
-
-# Documentation
-docs/_build/
-
-# OS
-.DS_Store
-Thumbs.db
-
-# Docker
-Dockerfile
-.dockerignore
-docker-compose.yml
-
-# Temporary files
-*.tmp
-*.bak
-*.orig
-
-# Large data files (uncomment if you have large datasets)
-# data/
-# *.csv
-# *.h5
-# *.hdf5
\ No newline at end of file
diff --git a/.gitignore b/.gitignore
index acd1484..a18733b 100644
--- a/.gitignore
+++ b/.gitignore
@@ -178,10 +178,12 @@ examples/maps/ann_arbor
/phase1_results
phase1_results.zip
/sumo
-sim_configs
-vis_configs
-uv.lock
texas_example
README_QUICKSTART.md
-run_experiments_debug.py
-test_docker.sh
+run_experiments_debug.pyeb.geohjson
+jupiter/eb/eb.geojson
+jupiter/wb/wb.geojson
+jupiter
+*.geojson
+output.geojson
+11_03_demo
diff --git a/DOCKER_SETUP_GUIDE.md b/DOCKER_SETUP_GUIDE.md
deleted file mode 100644
index f744d20..0000000
--- a/DOCKER_SETUP_GUIDE.md
+++ /dev/null
@@ -1,312 +0,0 @@
-# TeraSim Docker Setup Guide
-
-## Overview
-
-TeraSim now provides two Docker deployment options:
-
-1. **Base Image**: Lightweight deployment for core TeraSim functionality (simulation, planning, control testing)
-2. **GPU Image**: Full-featured deployment for TeraSim-Cosmos with NVIDIA GPU support (world model training, video generation)
-
----
-
-## Quick Decision Guide
-
-### Use **Base Docker Image** if you need:
-- ✅ Traffic simulation with SUMO
-- ✅ Naturalistic and adversarial environment generation (NDE-NADE)
-- ✅ Planning and control testing
-- ✅ FastAPI service for integration
-- ✅ Basic visualization tools
-- ✅ Smaller image size (~2-3 GB)
-- ✅ No GPU required
-
-### Use **GPU Docker Image** if you need:
-- ✅ TeraSim-Cosmos functionality
-- ✅ NVIDIA Cosmos-Drive integration
-- ✅ HD map video generation
-- ✅ LiDAR rendering
-- ✅ ML model training/inference
-- ✅ Street view analysis
-- ⚠️ Requires NVIDIA GPU
-- ⚠️ Larger image size (~8-10 GB)
-
----
-
-## Option 1: Base Docker Image
-
-### Quick Start
-
-```bash
-# From TeraSim root directory
-docker-compose up -d --build
-docker-compose exec terasim bash
-
-# Inside container - run simulation
-python run_experiments.py
-```
-
-### What's Included
-- Core TeraSim packages:
- - `terasim` - Core simulation engine
- - `terasim-nde-nade` - Naturalistic & adversarial environments
- - `terasim-service` - FastAPI service
- - `terasim-envgen` - Environment generation
- - `terasim-datazoo` - Data processing
- - `terasim-vis` - Visualization tools
-- SUMO 1.23.1 traffic simulator
-- Redis for service components
-- Development tools (pytest, black, ruff, mypy)
-
-### What's NOT Included
-- ❌ `terasim-cosmos` (requires GPU)
-- ❌ PyTorch/TensorFlow with GPU support
-- ❌ Waymo Open Dataset tools
-- ❌ GPU-accelerated rendering
-
-### Documentation
-See [README_DOCKER.md](README_DOCKER.md)
-
----
-
-## Option 2: GPU Docker Image (TeraSim-Cosmos)
-
-### Prerequisites
-
-#### Hardware
-- NVIDIA GPU with CUDA 11.8+ support
-- 16GB+ RAM (32GB recommended)
-- 50GB+ free disk space
-
-#### Software
-Install NVIDIA Container Toolkit:
-
-```bash
-# Ubuntu/Debian
-distribution=$(. /etc/os-release;echo $ID$VERSION_ID)
-curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg
-curl -s -L https://nvidia.github.io/libnvidia-container/$distribution/libnvidia-container.list | \
- sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \
- sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list
-
-sudo apt-get update
-sudo apt-get install -y nvidia-container-toolkit
-sudo nvidia-ctk runtime configure --runtime=docker
-sudo systemctl restart docker
-
-# Test installation
-docker run --rm --gpus all nvidia/cuda:11.8.0-base-ubuntu22.04 nvidia-smi
-```
-
-### Quick Start
-
-```bash
-# Navigate to terasim-cosmos directory
-cd packages/terasim-cosmos
-
-# Build and start GPU container
-docker-compose -f docker-compose.gpu.yml up -d --build
-
-# Enter container
-docker-compose -f docker-compose.gpu.yml exec terasim-cosmos bash
-
-# Inside container - verify GPU
-python -c "import torch; print(f'CUDA: {torch.cuda.is_available()}')"
-
-# Run conversion
-python packages/terasim-cosmos/terasim_cosmos/converter.py \
- --path_to_config configs/converter/scenario.yaml \
- --streetview_retrieval True
-```
-
-### What's Included
-- **Everything from Base Image**, PLUS:
-- `terasim-cosmos` package
-- PyTorch with CUDA 11.8
-- TensorFlow with GPU support
-- Waymo Open Dataset
-- GPU-accelerated rendering libraries
-- Additional ML tools (transformers, ray)
-- Jupyter Notebook
-
-### Documentation
-See [packages/terasim-cosmos/README_DOCKER_GPU.md](packages/terasim-cosmos/README_DOCKER_GPU.md)
-
----
-
-## Comparison Table
-
-| Feature | Base Image | GPU Image |
-|---------|------------|-----------|
-| **Base OS** | Ubuntu 22.04 | Ubuntu 22.04 + CUDA 11.8 |
-| **Image Size** | ~2-3 GB | ~8-10 GB |
-| **Build Time** | 10-15 min | 30-60 min |
-| **GPU Required** | No | Yes (NVIDIA) |
-| **RAM Required** | 4GB+ | 16GB+ |
-| **Disk Space** | 10GB+ | 50GB+ |
-| | | |
-| **Core Packages** | | |
-| terasim | ✅ | ✅ |
-| terasim-nde-nade | ✅ | ✅ |
-| terasim-service | ✅ | ✅ |
-| terasim-envgen | ✅ | ✅ |
-| terasim-datazoo | ✅ | ✅ |
-| terasim-vis | ✅ | ✅ |
-| terasim-cosmos | ❌ | ✅ |
-| | | |
-| **ML Libraries** | | |
-| PyTorch | ❌ | ✅ (CUDA 11.8) |
-| TensorFlow | ❌ | ✅ (GPU) |
-| Transformers | ❌ | ✅ |
-| Ray | ❌ | ✅ |
-| | | |
-| **Capabilities** | | |
-| Traffic simulation | ✅ | ✅ |
-| NDE-NADE testing | ✅ | ✅ |
-| FastAPI service | ✅ | ✅ |
-| Cosmos-Drive integration | ❌ | ✅ |
-| HD map video gen | ❌ | ✅ |
-| LiDAR rendering | ❌ | ✅ |
-| Street view analysis | ❌ | ✅ |
-| GPU acceleration | ❌ | ✅ |
-
----
-
-## Common Workflows
-
-### Workflow 1: Basic Simulation Testing (Use Base Image)
-
-```bash
-# Start base container
-docker-compose up -d
-docker-compose exec terasim bash
-
-# Run simulation
-python run_experiments.py
-
-# Run tests
-pytest
-
-# View outputs
-ls outputs/
-```
-
-### Workflow 2: Cosmos-Drive Video Generation (Use GPU Image)
-
-```bash
-# Start GPU container
-cd packages/terasim-cosmos
-docker-compose -f docker-compose.gpu.yml up -d
-docker-compose -f docker-compose.gpu.yml exec terasim-cosmos bash
-
-# Step 1: Run TeraSim simulation (or use existing outputs)
-cd /app/TeraSim
-python run_experiments.py
-
-# Step 2: Convert to Cosmos format
-python packages/terasim-cosmos/terasim_cosmos/converter.py \
- --path_to_config configs/converter/my_scenario.yaml
-
-# Step 3: View outputs
-ls outputs/my_scenario/render/
-```
-
-### Workflow 3: Development with Live Code Changes
-
-```bash
-# For base image - mount source code
-docker run -it --rm \
- -v $(pwd)/packages:/app/TeraSim/packages \
- -v $(pwd)/outputs:/app/TeraSim/outputs \
- terasim:latest bash
-
-# For GPU image - edit docker-compose.gpu.yml to add volume:
-# volumes:
-# - ../../packages/terasim-cosmos:/app/TeraSim/packages/terasim-cosmos
-```
-
----
-
-## Troubleshooting
-
-### Issue: "libproj25 not found" during build
-**Solution**: This has been fixed. Update to latest Dockerfile (uses libproj22).
-
-### Issue: "terasim_cosmos import error" in base image
-**Solution**: This is expected. Use GPU image for terasim-cosmos functionality.
-
-### Issue: GPU not detected in GPU container
-**Solution**:
-```bash
-# Verify NVIDIA Container Toolkit
-docker run --rm --gpus all nvidia/cuda:11.8.0-base-ubuntu22.04 nvidia-smi
-
-# Check GPU visibility in container
-docker-compose -f docker-compose.gpu.yml exec terasim-cosmos nvidia-smi
-```
-
-### Issue: CUDA out of memory
-**Solution**:
-```bash
-# Set memory growth for TensorFlow
-export TF_FORCE_GPU_ALLOW_GROWTH=true
-
-# Reduce batch sizes in your code
-```
-
----
-
-## File Structure
-
-```
-TeraSim/
-├── Dockerfile # Base image (no GPU)
-├── docker-compose.yml # Base image compose
-├── README_DOCKER.md # Base image docs
-├── DOCKER_SETUP_GUIDE.md # This file
-│
-├── packages/terasim-cosmos/
-│ ├── Dockerfile.gpu # GPU-enabled image
-│ ├── docker-compose.gpu.yml # GPU image compose
-│ └── README_DOCKER_GPU.md # GPU image docs
-│
-├── outputs/ # Simulation outputs (mounted)
-├── logs/ # Log files (mounted)
-└── configs/ # Configuration files
- └── converter/ # Cosmos converter configs
-```
-
----
-
-## Next Steps
-
-### For Base Image Users
-1. Read [README_DOCKER.md](README_DOCKER.md)
-2. Run example simulations
-3. Explore NDE-NADE scenarios
-4. Test FastAPI service integration
-
-### For GPU Image Users
-1. Install NVIDIA Container Toolkit
-2. Read [packages/terasim-cosmos/README_DOCKER_GPU.md](packages/terasim-cosmos/README_DOCKER_GPU.md)
-3. Run TeraSim simulation
-4. Convert outputs to Cosmos-Drive format
-5. Generate HD map videos
-6. Integrate with NVIDIA Cosmos platform
-
----
-
-## Support
-
-- **Main Documentation**: [README.md](README.md)
-- **Development Guide**: [CLAUDE.md](CLAUDE.md)
-- **GitHub Issues**: https://github.com/mcity/TeraSim/issues
-- **Discussions**: https://github.com/mcity/TeraSim/discussions
-
----
-
-## License
-
-- **TeraSim Core**: Apache 2.0 License
-- **TeraSim Visualization**: MIT License
-
-See [LICENSE](LICENSE) for details.
\ No newline at end of file
diff --git a/Dockerfile b/Dockerfile
deleted file mode 100644
index 25e25db..0000000
--- a/Dockerfile
+++ /dev/null
@@ -1,146 +0,0 @@
-# TeraSim Docker Image
-# Base: Ubuntu 22.04 with Python 3.10
-# Includes: gcc, g++, SUMO, Redis, and all TeraSim dependencies
-
-FROM ubuntu:22.04 AS builder
-
-# Prevent interactive prompts during package installation
-ENV DEBIAN_FRONTEND=noninteractive
-
-# Install system dependencies
-RUN apt-get update && apt-get install -y \
- # Build tools
- gcc \
- g++ \
- make \
- cmake \
- build-essential \
- # Python 3.10
- python3.10 \
- python3.10-dev \
- python3-pip \
- python3.10-venv \
- # Version control
- git \
- # Redis
- redis-server \
- # Spatial libraries (for rtree, shapely, pyproj)
- libspatialindex-dev \
- libgeos-dev \
- libproj-dev \
- # XML processing (for lxml)
- libxml2-dev \
- libxslt1-dev \
- # Other utilities
- wget \
- curl \
- && rm -rf /var/lib/apt/lists/*
-
-# Set Python 3.10 as default python3
-RUN update-alternatives --install /usr/bin/python3 python3 /usr/bin/python3.10 1 \
- && update-alternatives --install /usr/bin/python python /usr/bin/python3.10 1
-
-# Upgrade pip
-RUN python3 -m pip install --upgrade pip setuptools wheel
-
-# Set working directory
-WORKDIR /app/TeraSim
-
-# Copy project files
-COPY . .
-
-# Clone SUMO tools repository for SUMO_HOME
-RUN mkdir -p /root/.terasim/deps && \
- git clone --depth 1 https://github.com/eclipse/sumo.git /root/.terasim/deps/sumo && \
- echo "SUMO_HOME=/root/.terasim/deps/sumo" > /root/.terasim/deps/.sumo_home
-
-# Set SUMO_HOME environment variable
-ENV SUMO_HOME=/root/.terasim/deps/sumo
-ENV PATH="${SUMO_HOME}/bin:${PATH}"
-
-# Install Python packages in editable mode
-# Note: terasim-cosmos is excluded from base image (requires GPU support)
-# For terasim-cosmos, use packages/terasim-cosmos/Dockerfile.gpu instead
-RUN pip install -e packages/terasim && \
- pip install -e packages/terasim-nde-nade && \
- pip install -e packages/terasim-service && \
- pip install -e packages/terasim-envgen && \
- pip install -e packages/terasim-datazoo && \
- pip install -e packages/terasim-vis
-
-# Install development dependencies
-RUN pip install \
- pytest>=7.4.0 \
- pytest-cov>=4.1.0 \
- black>=23.7.0 \
- ruff>=0.1.0 \
- mypy>=1.5.1 \
- isort>=5.12.0
-
-# Build Cython extensions for NDE-NADE
-RUN cd packages/terasim-nde-nade && \
- python setup.py build_ext --inplace && \
- cd ../..
-
-# Create output directories
-RUN mkdir -p outputs logs
-
-# Verify installation
-RUN python3 -c "import terasim; print('✅ TeraSim core imported successfully')" && \
- python3 -c "import terasim_nde_nade; print('✅ TeraSim NDE-NADE imported successfully')" && \
- python3 -c "import terasim_service; print('✅ TeraSim Service imported successfully')" && \
- python3 -c "import terasim_vis; print('✅ TeraSim Visualization imported successfully')"
-
-# === Runtime Stage ===
-FROM ubuntu:22.04
-
-ENV DEBIAN_FRONTEND=noninteractive
-
-# Install runtime dependencies only
-RUN apt-get update && apt-get install -y \
- python3.10 \
- python3-pip \
- git \
- redis-server \
- libspatialindex-c6 \
- libgeos-c1v5 \
- libproj22 \
- libxml2 \
- libxslt1.1 \
- && rm -rf /var/lib/apt/lists/*
-
-# Set Python 3.10 as default
-RUN update-alternatives --install /usr/bin/python3 python3 /usr/bin/python3.10 1 \
- && update-alternatives --install /usr/bin/python python /usr/bin/python3.10 1
-
-# Copy installed packages and SUMO from builder
-COPY --from=builder /usr/local/lib/python3.10 /usr/local/lib/python3.10
-COPY --from=builder /usr/local/bin /usr/local/bin
-COPY --from=builder /root/.terasim /root/.terasim
-COPY --from=builder /app/TeraSim /app/TeraSim
-
-# Set working directory
-WORKDIR /app/TeraSim
-
-# Set environment variables
-ENV SUMO_HOME=/root/.terasim/deps/sumo
-ENV PATH="${SUMO_HOME}/bin:${PATH}"
-ENV PYTHONUNBUFFERED=1
-
-# Expose ports
-# 8000: TeraSim FastAPI service
-# 6379: Redis
-EXPOSE 8000 6379
-
-# Default command: start bash shell
-CMD ["/bin/bash"]
-
-# Alternative entry points (uncomment as needed):
-# Run simulation with GUI disabled
-# CMD ["python3", "run_experiments.py"]
-
-# Start TeraSim service
-# CMD ["python3", "run_service.py"]
-
-# Start Redis and then run simulation
-# CMD ["sh", "-c", "redis-server --daemonize yes && python3 run_experiments.py"]
\ No newline at end of file
diff --git a/README.md b/README.md
index b17a93b..2e44d07 100644
--- a/README.md
+++ b/README.md
@@ -3,187 +3,214 @@
-
-
-
-
----
-
-# TeraSim
-
-**Generative AI–Driven Autonomous Vehicle Simulation for Unknown Unsafe Events Discovery**
+**Generative Autonomous Vehicle Testing Environment for Unknown Unsafe Events Discovery**
---
-## Overview
+Website • Paper • Examples • Community
-TeraSim is an open-source platform for automated autonomous-vehicle (AV) simulation using generative AI.
-Its primary objective is to **efficiently uncover real-world unknown unsafe events** by automatically creating diverse and statistically realistic traffic environments.
+[](https://pypi.org/project/terasim)
+[](https://pypi.org/project/terasim)
+[](LICENSE)
+[](https://github.com/mcity/TeraSim/issues)
-The framework has evolved from its initial focus on planning-and-control testing to a **complete simulation workflow**, which now includes:
-
-1. **High-fidelity HD map generation** for large-scale, accurate simulation environments
-2. **Generative traffic environment creation** for naturalistic and adversarial scenario testing
-3. **Generative sensor simulation** for camera and LiDAR perception validation
-
-This expanded scope enables a unified pipeline from map generation to perception and planning validation.
-
-## 🚀 **Updates**
-
-- **[09/29/2025]**: TeraSim-World source codes are available. See [TeraSim_World.md](docs/TeraSim_World.md) to get started.
+
+
+## About
+TeraSim is an **open-source traffic simulation platform** designed for **naturalistic and adversarial testing** of autonomous vehicles (AVs). It enables **high-speed, AI-driven testing environment generation** to expose AVs to both routine and **rare, high-risk driving conditions**.
-## **🌎 New Feature: TeraSim-World**
+Developed with **researchers, AV developers, and regulators** in mind, TeraSim is designed to better support **ISO 21448 (SOTIF) and ISO 34502 compliance**, providing a **scalable, automated, and unbiased AV evaluation framework**.
+Built upon the open-source traffic simulation software [SUMO (Simulation of Urban MObility)](https://www.eclipse.org/sumo/), TeraSim extends its capabilities to provide specialized features for autonomous vehicle testing.
-
+## **🎥 Demo Video**
-**TeraSim-World** automatically synthesizes geographically grounded, safety-critical data for End-to-End autonomous driving **anywhere in the world**. See 📄 [paper](https://arxiv.org/abs/2503.03629) and 🌐 [videos](https://wjiawei.com/terasim-world-web/) for details.
+[](https://www.youtube.com/watch?v=9wc5QTAETTk)
-✨ **Key Capabilities:**
-- 🗺️ **Global Coverage**: Generate realistic driving scenarios for any location worldwide
-- 🎯 **Safety-Critical Data**: Automatically create safety-critical events for E2E AV safety testing
-- 🔄 **NVIDIA Cosmos-Drive Compatible**: Direct integration with video generation model training platforms
+TeraSim is built upon a series of foundational academic works in autonomous vehicle testing:
-🚀 **Source code is now available!** See [TeraSim_World.md](docs/TeraSim_World.md) for getting started guide.
+- **NDE** ([Paper](https://doi.org/10.1038/s41467-023-37677-5) | [Code](https://github.com/michigan-traffic-lab/Learning-Naturalistic-Driving-Environment)): Learning naturalistic driving environment with statistical realism. *Yan, X., Zou, Z., Feng, S., et al. Nature Communications 14, 2037 (2023).*
+- **NADE** ([Paper](https://doi.org/10.1038/s41467-021-21007-8) | [Code](https://github.com/michigan-traffic-lab/Naturalistic-and-Adversarial-Driving-Environment)): Intelligent driving intelligence test for autonomous vehicles with naturalistic and adversarial environment. *Feng, S., Yan, X., Sun, H. et al. Nature Communications 12, 748 (2021).*
-## **🎥 Demo Video**
+- **D2RL** ([Paper](https://doi.org/10.1038/s41586-023-05732-2) | [Code](https://github.com/michigan-traffic-lab/Dense-Deep-Reinforcement-Learning)): Dense reinforcement learning for safety validation of autonomous vehicles. *Feng, S., Sun, H., Yan, X., et al. Nature 615, 620–627 (2023).*
-## Background and Foundations
+---
-TeraSim builds on prior research in naturalistic and adversarial driving environments:
+## **🌟 Key Features**
+✅ **Generative Driving Environment Testing**
+→ **Adaptive and interactive** environments replace static, manually designed scenarios.
+→ **Automatically uncovers unknown unsafe events**, enhancing AV safety validation.
+→ **Scalable and efficient**, reducing manual effort while expanding test coverage.
-* **NDE** – Learning naturalistic driving environment with statistical realism
- [Paper](https://doi.org/10.1038/s41467-023-37677-5) | [Code](https://github.com/michigan-traffic-lab/Learning-Naturalistic-Driving-Environment)
+✅ **Naturalistic & Adversarial Driving Environments (NADE)**
+→ Real-world traffic behavior modeling based on **large-scale naturalistic driving data**.
+→ Injects **corner cases** (e.g., jaywalking pedestrians, sudden lane changes) to rigorously test AV safety.
-* **NADE** – Intelligent driving intelligence test with naturalistic and adversarial environment
- [Paper](https://doi.org/10.1038/s41467-021-21007-8) | [Code](https://github.com/michigan-traffic-lab/Naturalistic-and-Adversarial-Driving-Environment)
+✅ **Scalable & Automated AV Testing**
+→ AI-driven **naturalistic and adversarial driving environment** accelerates AV validation **by 1,000x - 100,000x** compared to real-world testing.
+→ Dynamically adapts test cases to **urban, highway, and mixed-traffic conditions**.
-* **D2RL** – Dense deep reinforcement learning for AV safety validation
- [Paper](https://doi.org/10.1038/s41586-023-05732-2) | [Code](https://github.com/michigan-traffic-lab/Dense-Deep-Reinforcement-Learning)
+✅ **Seamless Integration with Third-Party Simulators**
+→ Works with **CARLA, Autoware**, and more.
+→ API-driven design enables **plug-and-play simulation** for integration with third-party simulators.
-These works provide the statistical and algorithmic basis for TeraSim’s scenario generation and risk discovery.
+✅ **City-Scale AV Testing with TeraSim-Macro**
+→ Extends simulations from **single intersections to entire cities**, supporting **policy-level AV impact analysis**.
---
-## Key Capabilities
+## **🛠️ System Architecture**
-### 1. High-Fidelity HD Map Generation
+TeraSim is modular, allowing users to **customize and extend** simulations easily.
-* Tools for building **city-scale, high-resolution digital twins** suitable for AV testing.
-* Automated conversion of real-world survey data into simulation-ready HD maps.
-* Provides accurate lane geometry and traffic-control metadata for downstream simulations.
+
-### 2. Generative Traffic Environment Creation
-* Automated scenario generation based on **large-scale naturalistic driving data**.
-* **Adversarial scenario synthesis** to reveal rare or high-risk interactions (e.g., aggressive cut-ins, unexpected pedestrian crossings).
-* Integration with [SUMO](https://www.eclipse.org/sumo/) and third-party simulators such as [CARLA](https://carla.org/) and Autoware.
+📌 **Packages:**
+- **`terasim`:** Core simulation engine for generating AV test environments.
+- **`terasim-nde-nade`:** Realistic & adversarial driving environments for safety evaluation.
+ - **Vehicle Adversities** (e.g., aggressive cut-ins, emergency braking).
+ - **VRU Adversities** (e.g., jaywalking pedestrians, erratic cyclists).
+- **`terasim-service`:** RESTful API service built with FastAPI for seamless integration with **popular simulators like CARLA** and **AV planning and control system**.
+- **`terasim-envgen`:** Automatic environment generation (map and traffic) tools for creating test scenarios.
+- **`terasim-datazoo`:** Data processing utilities for **real-world driving datasets (Waymo, NuScenes, NuPlan)**.
+- **`terasim-vis`:** Advanced visualization tools for trajectory and network analysis.
-### 3. Generative Sensor Simulation
-
-* **`terasim-cosmos`** integrates TeraSim-World with **generative AI–based camera and LiDAR simulation**.
-* Enables perception validation and sensor pipeline testing under diverse conditions.
-* **Ongoing work:** support for fully **custom sensor models and configurable realism levels** is under active development.
+📌 **Plug-and-Play Compatibility:**
+✅ SUMO-based microsimulation
+✅ CARLA & Autoware integration
+✅ Real-world dataset support
---
-## System Architecture
-
-TeraSim uses a modular monorepo design. Each package can be used independently or combined into a complete simulation pipeline.
-
-```
-TeraSim/
-├── packages/
-│ ├── terasim/ # Core simulation engine
-│ ├── terasim-envgen/ # HD map and environment generation
-│ ├── terasim-nde-nade/ # Naturalistic & adversarial environment algorithms
-│ ├── terasim-cosmos/ # TeraSim-World integration & generative AI sensor simulation
-│ ├── terasim-sensor/ # Baseline sensor utilities
-│ ├── terasim-datazoo/ # Data processing utilities for real driving datasets
-│ ├── terasim-service/ # RESTful API for external simulators
-│ └── terasim-vis/ # Visualization and analysis tools
-├── examples/ # Example configurations and scenarios
-├── docs/ # Documentation and figures
-└── tests/ # Test suites
-```
-
----
+## **🔧 Installation**
-## Installation
+### Quick Installation (Recommended)
-### Quick Setup
+TeraSim is now available as a unified monorepo with multiple packages. Use our automated setup script for the easiest installation:
```bash
+# Clone the monorepo
git clone https://github.com/mcity/TeraSim.git
cd TeraSim
+
+# Run automated setup (installs all components)
./setup_environment.sh
```
-This script installs all required Python packages and dependencies, including [SUMO](https://www.eclipse.org/sumo/).
-
-### Docker Installation (Recommended for Production)
-
-For a containerized environment with all dependencies pre-installed:
-
-```bash
-git clone https://github.com/mcity/TeraSim.git
-cd TeraSim
-docker-compose up -d --build
-docker-compose exec terasim bash
-```
-See [README_DOCKER.md](README_DOCKER.md) for detailed Docker deployment instructions.
+### Development Installation
-### Development Environment
+For development or if you want the latest features:
```bash
+# Create environment (recommended)
conda create -n terasim python=3.10
conda activate terasim
+
+# Clone and install in development mode
git clone https://github.com/mcity/TeraSim.git
cd TeraSim
+
+# Install gcc and g++
+conda install -c conda-forge gcc
+conda install -c conda-forge gxx
./setup_environment.sh
```
-**Requirements**
-
-* Python 3.10–3.12
-* SUMO 1.23.1 (installed by the setup script)
-* Redis for service components
-* gcc/g++ compilers (for Cython extensions)
+**System Requirements:**
+- Python 3.10-3.12
+- SUMO 1.23.1 (automatically installed)
+- Redis (for service components)
---
-## Quick Start Example
+## **🚀 Quick Start**
+
+After installation, try a basic simulation:
```python
+import terasim
+
+# Create and run a simple simulation
from terasim import Simulator
from terasim.envs import EnvTemplate
+# Initialize simulator with example map
sim = Simulator("examples/maps/Mcity/sim.sumocfg")
+
+# Set up environment
env = EnvTemplate()
sim.bind_env(env)
+# Run simulation
sim.start()
sim.run(steps=1000)
sim.close()
```
-Additional examples are available in the [`examples/`](examples/) directory.
+For more examples, see the [`examples/`](examples/) directory.
+
+**Development Commands:**
+```bash
+# Run tests
+uv run pytest
+
+# Format code
+uv run black .
+
+# Start Python shell with TeraSim
+uv run python
+```
+
+---
+
+## **🚀 Why TeraSim?**
+
+🔍 **Uncover Hidden AV Risks**
+→ Dynamically generates realistic and adversarial traffic environments, identifying **corner cases**.
+
+⚡ **Automated & Scalable**
+→ Uses AI to generate simulations across cities, with **1000x faster testing efficiency** than real-world methods.
+
+🔗 **Seamless Integration**
+→ Plugin-based design works with **existing AV stacks & third-party simulators**.
+
+📢 **Open-Source & Extensible**
+→ Encourages industry collaboration for **safer, more reliable AV deployment**.
---
-## Contributing
+## **📦 Monorepo Structure**
-Contributions are welcome. Please read the [CONTRIBUTING.md](CONTRIBUTING.md) guidelines and join the [GitHub discussions](https://github.com/mcity/TeraSim/discussions) for feedback or proposals.
+```
+TeraSim/
+├── packages/ # Python packages
+│ ├── terasim/ # Core simulation engine
+│ ├── terasim-nde-nade/ # NDE-NADE algorithms
+│ ├── terasim-service/ # API service
+│ ├── terasim-envgen/ # Environment generation
+│ ├── terasim-datazoo/ # Data processing
+│ └── terasim-vis/ # Visualization tools
+├── apps/ # Applications & deployment
+├── examples/ # Example simulations
+├── docs/ # Documentation
+└── tests/ # Test suites
+```
---
+## **📌 Contributing**
+
+We welcome contributions! Please see our [contribution guidelines](CONTRIBUTING.md) and join our [community discussions](https://github.com/mcity/TeraSim/discussions).
+
+---
-## **📄 License**
+## **📄 License & Attribution**
- **TeraSim Core and other packages**: Apache 2.0 License
- **Visualization Tools**: MIT License
diff --git a/README_DOCKER.md b/README_DOCKER.md
deleted file mode 100644
index ec63c0b..0000000
--- a/README_DOCKER.md
+++ /dev/null
@@ -1,402 +0,0 @@
-# TeraSim Docker Deployment Guide
-
-## Overview
-
-This Dockerfile provides a complete, production-ready environment for TeraSim based on **Ubuntu 22.04** and **Python 3.10**. It includes all necessary dependencies and tools for running autonomous vehicle simulations.
-
-### What's Included
-
-- ✅ **Compilers**: gcc, g++, build-essential
-- ✅ **SUMO 1.23.1**: Traffic simulation engine (installed via pip)
-- ✅ **SUMO Tools**: Full toolchain cloned to `~/.terasim/deps/sumo`
-- ✅ **Redis**: Required for service components
-- ✅ **Python Dependencies**: All packages including Cython extensions
-- ✅ **Optimized Build**: Multi-stage Docker build (~2-3 GB final image)
-
----
-
-## Quick Start
-
-### Using Docker Compose (Recommended)
-
-```bash
-# Build and start the container
-docker-compose up -d --build
-
-# Enter the container
-docker-compose exec terasim bash
-
-# Run simulation inside container
-python run_experiments.py
-
-# Stop the container
-docker-compose down
-```
-
-### Using Docker CLI
-
-```bash
-# Build the image
-docker build -t terasim:latest .
-
-# Run container in interactive mode
-docker run -it --name terasim \
- -p 8000:8000 \
- -p 6379:6379 \
- -v $(pwd)/outputs:/app/TeraSim/outputs \
- -v $(pwd)/logs:/app/TeraSim/logs \
- terasim:latest
-
-# Inside the container, start Redis (if needed)
-redis-server --daemonize yes
-
-# Run your simulation
-python run_experiments.py
-```
-
----
-
-## Common Operations
-
-### Running Different Simulations
-
-```bash
-# Standard simulation (police pullover scenario)
-docker-compose exec terasim python run_experiments.py
-
-# Debug mode (cutin scenario, GUI required)
-docker-compose exec terasim python run_experiments_debug.py
-
-# Start FastAPI service
-docker-compose exec terasim python run_service.py
-
-# Run example scripts
-docker-compose exec terasim python examples/scripts/example.py
-```
-
-### Running Tests
-
-```bash
-# Run all tests
-docker-compose exec terasim pytest
-
-# Run specific test suite
-docker-compose exec terasim pytest tests/test_core/
-
-# Generate HTML coverage report
-docker-compose exec terasim pytest --cov=terasim --cov-report=html
-
-# Run only fast tests (skip slow integration tests)
-docker-compose exec terasim pytest -m "not slow"
-```
-
-### Code Quality Tools
-
-```bash
-# Format code with Black
-docker-compose exec terasim black packages/
-
-# Lint with Ruff
-docker-compose exec terasim ruff check packages/
-
-# Type checking with mypy
-docker-compose exec terasim mypy packages/terasim/
-
-# Sort imports with isort
-docker-compose exec terasim isort packages/
-```
-
----
-
-## Configuration
-
-### Data Persistence
-
-The Docker Compose configuration automatically mounts volumes for data persistence:
-
-- `./outputs` → Simulation output files
-- `./logs` → Log files
-
-Your data persists on the host machine even after container removal.
-
-### Port Mappings
-
-- **8000**: TeraSim FastAPI service endpoint
-- **6379**: Redis database
-
-### Environment Variables
-
-The following environment variables are pre-configured in the container:
-
-```bash
-SUMO_HOME=/root/.terasim/deps/sumo
-PATH=$SUMO_HOME/bin:$PATH
-PYTHONUNBUFFERED=1
-```
-
-### Resource Limits
-
-Default resource limits in `docker-compose.yml`:
-
-```yaml
-deploy:
- resources:
- limits:
- cpus: '4'
- memory: 8G
- reservations:
- cpus: '2'
- memory: 4G
-```
-
-Adjust these values based on your simulation requirements.
-
----
-
-## Customization
-
-### Changing Startup Command
-
-Edit the `command` field in `docker-compose.yml`:
-
-```yaml
-# Auto-start simulation on container launch
-command: sh -c "redis-server --daemonize yes && python run_experiments.py"
-
-# Auto-start FastAPI service
-command: sh -c "redis-server --daemonize yes && python run_service.py"
-
-# Keep container running with bash (default)
-command: sh -c "redis-server --daemonize yes && /bin/bash"
-```
-
-### Using Separate Redis Container
-
-Uncomment the Redis service in `docker-compose.yml`:
-
-```yaml
-services:
- redis:
- image: redis:7-alpine
- container_name: terasim-redis
- ports:
- - "6379:6379"
- volumes:
- - redis-data:/data
- command: redis-server --appendonly yes
-
-volumes:
- redis-data:
-```
-
-Then update the TeraSim service to link to Redis:
-
-```yaml
-services:
- terasim:
- # ... other config ...
- depends_on:
- - redis
- environment:
- - REDIS_HOST=redis
-```
-
----
-
-## Troubleshooting
-
-### Build Failures
-
-```bash
-# Clean Docker cache and rebuild
-docker-compose build --no-cache
-
-# Check Docker disk space
-docker system df
-
-# Prune unused resources
-docker system prune -a
-```
-
-### Redis Connection Issues
-
-```bash
-# Check Redis status inside container
-docker-compose exec terasim redis-cli ping
-
-# Manually start Redis
-docker-compose exec terasim redis-server --daemonize yes
-
-# View Redis logs
-docker-compose exec terasim redis-cli INFO
-```
-
-### SUMO_HOME Not Set
-
-```bash
-# Verify environment variable
-docker-compose exec terasim echo $SUMO_HOME
-
-# Manually set if needed
-docker-compose exec terasim bash -c "export SUMO_HOME=/root/.terasim/deps/sumo"
-
-# Check SUMO installation
-docker-compose exec terasim sumo --version
-```
-
-### Permission Issues with Mounted Volumes
-
-```bash
-# On host machine, adjust permissions
-chmod -R 777 outputs logs
-
-# Or change ownership to your user
-sudo chown -R $USER:$USER outputs logs
-```
-
-### Container Won't Start
-
-```bash
-# View container logs
-docker-compose logs terasim
-
-# Check specific error messages
-docker-compose logs --tail=50 terasim
-
-# Restart container
-docker-compose restart terasim
-```
-
-### Python Import Errors
-
-```bash
-# Verify package installation inside container
-docker-compose exec terasim python -c "import terasim; print(terasim.__version__)"
-
-# Reinstall packages if needed
-docker-compose exec terasim pip install -e packages/terasim
-```
-
----
-
-## Development Workflow
-
-### Making Code Changes
-
-1. Edit code on your host machine
-2. Rebuild the container to apply changes:
-
-```bash
-docker-compose up -d --build
-```
-
-Alternatively, mount your source code as a volume for live editing:
-
-```yaml
-services:
- terasim:
- volumes:
- - ./packages:/app/TeraSim/packages
-```
-
-### Debugging Inside Container
-
-```bash
-# Enter container with bash
-docker-compose exec terasim bash
-
-# Run Python interactively
-docker-compose exec terasim python
-
-# Use IPython for better debugging
-docker-compose exec terasim pip install ipython
-docker-compose exec terasim ipython
-```
-
-### Running GUI Applications
-
-For SUMO GUI (`sumo-gui`) to work, you need X11 forwarding:
-
-```yaml
-services:
- terasim:
- environment:
- - DISPLAY=$DISPLAY
- volumes:
- - /tmp/.X11-unix:/tmp/.X11-unix
-```
-
-On the host:
-
-```bash
-# Allow X11 connections
-xhost +local:docker
-
-# Run container
-docker-compose up -d
-```
-
----
-
-## Image Information
-
-- **Base Image**: `ubuntu:22.04`
-- **Python Version**: `3.10`
-- **SUMO Version**: `1.23.1`
-- **Build Type**: Multi-stage (optimized)
-- **Estimated Size**: 2-3 GB
-- **Note**: Base image excludes `terasim-cosmos` (requires GPU support)
-
-### TeraSim-Cosmos GPU Image
-
-For full TeraSim-Cosmos functionality with GPU support, use the dedicated GPU Dockerfile:
-
-```bash
-# Navigate to terasim-cosmos package
-cd packages/terasim-cosmos
-
-# Build and run GPU-enabled image
-docker-compose -f docker-compose.gpu.yml up -d --build
-
-# Enter container
-docker-compose -f docker-compose.gpu.yml exec terasim-cosmos bash
-```
-
-See [packages/terasim-cosmos/README_DOCKER_GPU.md](packages/terasim-cosmos/README_DOCKER_GPU.md) for detailed GPU setup instructions.
-
----
-
-## Performance Tips
-
-1. **Use BuildKit**: Enable faster builds with Docker BuildKit:
- ```bash
- DOCKER_BUILDKIT=1 docker-compose build
- ```
-
-2. **Layer Caching**: Order Dockerfile commands from least to most frequently changed
-
-3. **Parallel Builds**: Build with multiple CPU cores:
- ```bash
- docker-compose build --parallel
- ```
-
-4. **Resource Allocation**: Increase Docker Desktop resources in Settings → Resources
-
----
-
-## Support
-
-For issues, questions, or contributions:
-
-- **GitHub Issues**: [https://github.com/mcity/TeraSim/issues](https://github.com/mcity/TeraSim/issues)
-- **Discussions**: [https://github.com/mcity/TeraSim/discussions](https://github.com/mcity/TeraSim/discussions)
-- **Documentation**: See main [README.md](README.md) and [CLAUDE.md](CLAUDE.md)
-
----
-
-## License
-
-- **TeraSim Core**: Apache 2.0 License
-- **TeraSim Visualization Tools**: MIT License
-
-See [LICENSE](LICENSE) for details.
\ No newline at end of file
diff --git a/configs/adversities/static/stalled_object_latlon_example.yaml b/configs/adversities/static/stalled_object_latlon_example.yaml
new file mode 100644
index 0000000..43dd5f8
--- /dev/null
+++ b/configs/adversities/static/stalled_object_latlon_example.yaml
@@ -0,0 +1,40 @@
+# Example configuration for stalled object using latlon_degree placement mode
+adversity_cfg:
+ static:
+ stalled_object:
+ _target_: terasim_nde_nade.adversity.StalledObjectAdversity
+ _convert_: 'all'
+ placement_mode: "latlon_degree" # Use lat/lon/degree placement mode
+ lon: -83.7430 # Longitude (example: Ann Arbor, MI)
+ lat: 42.2808 # Latitude (example: Ann Arbor, MI)
+ degree: 90.0 # Heading angle in degrees (0=North, 90=East, 180=South, 270=West)
+ object_type: "TRUCK" # Type of stalled object
+ start_time: 0 # Start time in simulation seconds
+ end_time: -1 # End time (-1 for infinite duration)
+ other_settings:
+ duration: 1200
+
+# Alternative examples:
+#
+# Using xy_angle mode:
+# adversity_cfg:
+# static:
+# stalled_object:
+# _target_: terasim_nde_nade.adversity.StalledObjectAdversity
+# _convert_: 'all'
+# placement_mode: "xy_angle"
+# x: 100.0
+# y: 200.0
+# angle: 90.0
+# object_type: "TRUCK"
+#
+# Using lane_position mode (default):
+# adversity_cfg:
+# static:
+# stalled_object:
+# _target_: terasim_nde_nade.adversity.StalledObjectAdversity
+# _convert_: 'all'
+# placement_mode: "lane_position"
+# lane_id: "edge1_0"
+# lane_position: 100
+# object_type: "veh_passenger"
diff --git a/configs/config.yaml b/configs/config.yaml
new file mode 100644
index 0000000..c437153
--- /dev/null
+++ b/configs/config.yaml
@@ -0,0 +1,30 @@
+map_download:
+ distance: 150
+ network_type: drive
+map_search:
+ bbox_size: 150
+ default_city: San Francisco
+ max_results: 5
+output:
+ base_dir: output
+ scene_prefix: scene_
+sumo:
+ begin_time: 0
+ end_time: 3600
+traffic:
+ end_time: 3600
+ vehicle_period: 0.5
+visualization:
+ preview:
+ dpi: 300
+ enabled: true
+ figure_height: 8
+ figure_width: 16
+ interactive_map: true
+ randomize:
+ enabled: true
+ map_style: true
+ node_selection: true
+ seed: 0
+ visualization: true
+ satellite_view: true
diff --git a/configs/simulation/example.yaml b/configs/simulation/example.yaml
deleted file mode 100644
index 01725c1..0000000
--- a/configs/simulation/example.yaml
+++ /dev/null
@@ -1,190 +0,0 @@
-input:
- sumo_net_file: "path_to_net"
- sumo_config_file: "path_to_config"
-
-output:
- dir: "path_to_output" # Output directory (use "output" for non-AV simulations)
- name: "test" # Experiment name
- nth: "0_2" # Experiment number
- aggregated_dir: "aggregated" # Directory for aggregated logs
-
-simulation_module: "my_simulations"
-simulation_class: "CarSimulation"
-parameters:
- num_cars: 10
- simulation_time: 3600
-
-# Simulation type: Choose between "safetest_mcity_av" or "safetest_mcity"
-simulation_type: "safetest_mcity"
-
-
-environment:
- # Module containing the environment class
- module: "terasim_nde_nade.envs.safetest_nade" # Use "terasim_nde_nade.envs.safetest_nade" for non-AV simulations
- # Environment class name
- class: "SafeTestNADE" # Use "SafeTestNADE" for non-AV simulations
- parameters:
- # Vehicle factory class
- vehicle_factory: "terasim_nde_nade.vehicle.nde_vehicle_factory.NDEVehicleFactory"
- # Info extractor class
- info_extractor: "terasim.logger.infoextractor.InfoExtractor"
- log_flag: true
- warmup_time_lb: 100 # Lower bound for warmup time
- warmup_time_ub: 200 # Upper bound for warmup time
- run_time: 1200 # Simulation run time (use 30 for non-AV simulations)
-
- MOBIL_lc_flag: True
- stochastic_acc_flag: False
- drive_rule: "righthand"
-
- adversity_sampling_probability: 1
- adversity_cfg:
- vehicle:
- roundabout_cutin:
- _target_: terasim_nde_nade.adversity.vehicles.lanechange_adversity.LanechangeAdversity
- _convert_: 'all'
- location: 'roundabout'
- ego_type: 'vehicle'
- probability: 2.3123152310029683e-05
- predicted_collision_type: "roundabout_cutin"
-
- roundabout_rearend:
- _target_: terasim_nde_nade.adversity.vehicles.leader_adversity.LeaderAdversity
- _convert_: 'all'
- location: 'roundabout'
- ego_type: 'vehicle'
- probability: 0 # 1.8383121748678325e-10
- predicted_collision_type: "roundabout_rearend"
-
- roundabout_fail_to_yield:
- _target_: terasim_nde_nade.adversity.vehicles.trafficrule_adversity.TrafficRuleAdversity
- _convert_: 'all'
- location: 'roundabout'
- ego_type: 'vehicle'
- probability: 0.0005025411072000003
- predicted_collision_type: "roundabout_fail_to_yield"
-
- # roundabout_neglect_conflict_lead:
- # _target_: terasim_nde_nade.adversity.roundabout_cutin_adversity.RoundaboutCutinAdversity
- # _convert_: 'all'
- # location: 'roundabout'
- # ego_type: 'vehicle'
- # probability: 0.00027119028871789013
-
- highway_cutin:
- _target_: terasim_nde_nade.adversity.vehicles.lanechange_adversity.LanechangeAdversity
- _convert_: 'all'
- location: 'highway'
- ego_type: 'vehicle'
- probability: 3.1996225726187704e-05
- predicted_collision_type: "highway_cutin"
-
- highway_rearend:
- _target_: terasim_nde_nade.adversity.vehicles.leader_adversity.LeaderAdversity
- _convert_: 'all'
- location: 'highway'
- ego_type: 'vehicle'
- probability: 0 # 0.8783674048511999
- predicted_collision_type: "highway_rearend"
-
- intersection_cutin:
- _target_: terasim_nde_nade.adversity.vehicles.lanechange_adversity.LanechangeAdversity
- _convert_: 'all'
- location: 'intersection'
- ego_type: 'vehicle'
- probability: 6.694592721399203e-05
- predicted_collision_type: "intersection_cutin"
-
- intersection_rearend:
- _target_: terasim_nde_nade.adversity.vehicles.leader_adversity.LeaderAdversity
- _convert_: 'all'
- location: 'intersection'
- ego_type: 'vehicle'
- probability: 0 # 0.00017365940078885788
- predicted_collision_type: "intersection_rearend"
-
- intersection_headon:
- _target_: terasim_nde_nade.adversity.vehicles.headon_adversity.HeadonAdversity
- _convert_: 'all'
- location: 'intersection'
- ego_type: 'vehicle'
- probability: 1.0663168272664859e-08
- predicted_collision_type: "intersection_headon"
-
- intersection_tfl:
- _target_: terasim_nde_nade.adversity.vehicles.trafficrule_adversity.TrafficRuleAdversity
- _convert_: 'all'
- location: 'intersection'
- ego_type: 'vehicle'
- probability: 0.010141902545438387
- predicted_collision_type: "intersection_tfl"
-
- # intersection_neglect_conflict_lead:
- # _target_: terasim_nde_nade.adversity.roundabout_cutin_adversity.RoundaboutCutinAdversity
- # _convert_: 'all'
- # location: 'roundabout'
- # ego_type: 'vehicle'
- # probability: 0.015050715723004704
-
- vulnerable_road_user:
- jaywalking:
- _target_: terasim_nde_nade.adversity.vru.jaywalking_adversity.JaywalkingAdversity
- _convert_: 'all'
- location: 'crosswalk'
- ego_type: 'vulnerable_road_user'
- probability: 0.001
- predicted_collision_type: "intersection_jaywalking"
-
- runningredlight:
- _target_: terasim_nde_nade.adversity.vru.runningredlight_adversity.RunningRedLightAdversity
- _convert_: 'all'
- location: 'crosswalk'
- ego_type: 'vulnerable_road_user'
- probability: 0.001
- predicted_collision_type: "intersection_runningredlight"
-
- stopcrossing:
- _target_: terasim_nde_nade.adversity.vru.stopcrossing_adversity.StopCrossingAdversity
- _convert_: 'all'
- location: 'crosswalk'
- ego_type: 'vulnerable_road_user'
- probability: 0.001
- predicted_collision_type: "intersection_stopcrossing"
-
- AV_cfg:
- route: ["EG_35_1_14", "EG_1_3_1", "EG_1_3_1.61", "EG_1_3_1.136", "EG_34_1_24", "EG_34_1_3", "gneE0", "EG_4_1_1", "EG_10_1_1", "EG_15_1_17", "EG_15_1_1", "EG_16_45_1", "EG_16_23_1", "EG_17_1_1", "EG_14_2_1", "EG_9_1_1", "EG_21_1_1", "EG_21_1_5", "EG_20_1_11", "EG_29_1_1", "EG_35_1_14"] # list of SUMO edges (example for Mcity)
- type: "NDE_HIGHWAY"
- cache_radius: 100
- control_radius: 50
- warmup_control:
- enabled: true # Enable AV warmup (false keeps original behavior)
-
- # Trigger condition (choose one)
- trigger_type: "time" # Options: "position", "time", "zone", "edge"
-
- # Position trigger configuration
- time_trigger:
- duration: 20.0 # Warmup duration in seconds
-simulator:
- module: "terasim.simulator"
- class: "Simulator"
- parameters:
- num_tries: 10
- gui_flag: false
- realtime_flag: false # Only applicable for AV mode
- sumo_output_file_types:
- - "fcd_all"
- - "collision"
- - "tripinfo"
-
-# Path resolution mode:
-# - "config_relative": Relative paths are resolved from the config file's directory
-# - "cwd_relative": Relative paths are resolved from the current working directory
-path_resolution: "config_relative"
-
-
-
-logging:
- levels:
- - "TRACE" # Log level for the main log file
- - "INFO" # Log level for the aggregated log file
diff --git a/configs/visulation/example.yaml b/configs/visulation/example.yaml
deleted file mode 100644
index 9545595..0000000
--- a/configs/visulation/example.yaml
+++ /dev/null
@@ -1,51 +0,0 @@
-fcd: "path_to_fcd"
-net: "path_to_net"
-output_dir: "path_to_output"
-video_name: "bev_trajectory"
-
-
-start_time: 95 # Start time for visualization (default: 0)
-end_time: 99 # End time for visualization (null = use all data)
-ego_vehicle_id: "flow_0.6"
-adv_vehicle_id: "ped_flow_cross_1.15"
-
-animate: true # true for animation, false for static plot
-speed_colors: false # Color trajectories by speed (overrides other colors)
-lane_colors: false # Color trajectories by lane (overrides vehicle type colors)
-
-# Plot appearance
-figsize: "9,12" # Figure size in inches (width,height)
-dpi: 300 # Resolution for saved images/animations
-max_frames: 19 # Maximum number of frames to visualize
-
-# Map styling
-map_style:
- style: "EUR" # Map style: EUR, USA, etc.
- zoom_to_extents: false # Auto-zoom to fit all roads (set false for custom view)
- show_title: false # Show plot title
- show_labels: false # Show axis labels
- title: "Traffic Simulation" # Custom title (if show_title: true)
-
-
- # Custom view settings (only used when zoom_to_extents: false)
- view:
- width: 60 # View width in meters
- height: 80 # View height in meters
- center_on_ego: true # Center view on ego vehicle (if false, use fixed center)
- center_x: 0 # Fixed center X coordinate (only if center_on_ego: false)
- center_y: 0 # Fixed center Y coordinate (only if center_on_ego: false)
-
- # Colors for map elements
- colors:
- lane_color: '0.9'
- lane_marking_color: '0.2'
-
-# Vehicle type coloring (when speed_colors and lane_colors are false):
-# - Regular vehicles (BV_*, AV): red
-# - Bicycles: yellow
-# - Pedestrians (VRU_*): blue
-
-# Animation settings (only used when animate: true)
-# - Frame rate: 10 fps
-# - Marker sizes: vehicles=6, bicycles=4, pedestrians=2
-# - Uses ffmpeg writer for MP4 output
\ No newline at end of file
diff --git a/docker-compose.yml b/docker-compose.yml
deleted file mode 100644
index 9fede28..0000000
--- a/docker-compose.yml
+++ /dev/null
@@ -1,49 +0,0 @@
-version: '3.8'
-
-services:
- terasim:
- build:
- context: .
- dockerfile: Dockerfile
- image: terasim:latest
- container_name: terasim
- working_dir: /app/TeraSim
- ports:
- - "8000:8000" # TeraSim FastAPI service
- - "6379:6379" # Redis
- volumes:
- # Mount outputs and logs to host for persistence
- - ./outputs:/app/TeraSim/outputs
- - ./logs:/app/TeraSim/logs
- # Optional: mount examples for easy editing
- # - ./examples:/app/TeraSim/examples
- environment:
- - SUMO_HOME=/root/.terasim/deps/sumo
- - PYTHONUNBUFFERED=1
- # Start Redis and keep container running
- command: sh -c "redis-server --daemonize yes && /bin/bash"
- stdin_open: true
- tty: true
- # Resource limits (adjust as needed)
- deploy:
- resources:
- limits:
- cpus: '4'
- memory: 8G
- reservations:
- cpus: '2'
- memory: 4G
-
- # Optional: Separate Redis service
- # Uncomment if you prefer Redis as a separate container
- # redis:
- # image: redis:7-alpine
- # container_name: terasim-redis
- # ports:
- # - "6379:6379"
- # volumes:
- # - redis-data:/data
- # command: redis-server --appendonly yes
-
-# volumes:
-# redis-data:
\ No newline at end of file
diff --git a/docs/TeraSim_World.md b/docs/TeraSim_World.md
deleted file mode 100644
index 448e1df..0000000
--- a/docs/TeraSim_World.md
+++ /dev/null
@@ -1,101 +0,0 @@
-# TeraSim-World
-
-
-
-
+gcloud installation (need sudo) and downloading from terminal
+
+```bash
+sudo apt-get update
+sudo apt-get install apt-transport-https ca-certificates gnupg curl
+curl https://packages.cloud.google.com/apt/doc/apt-key.gpg | sudo gpg --dearmor -o /usr/share/keyrings/cloud.google.gpg
+echo "deb [signed-by=/usr/share/keyrings/cloud.google.gpg] https://packages.cloud.google.com/apt cloud-sdk main" | sudo tee -a /etc/apt/sources.list.d/google-cloud-sdk.list
+sudo apt-get update && sudo apt-get install google-cloud-cli
+```
+
+Then you can login your google account and download the above tfrecords via
+```bash
+# or use `gcloud init --no-launch-browser` if you are in a remote terminal session
+gcloud init
+bash download_waymo.sh config/waymo_all.json
+```
+
+
+After downloading tfrecord files, we expect a folder structure as follows:
+```bash
+
+|-- segment-10247954040621004675_2180_000_2200_000_with_camera_labels.tfrecord
+|-- segment-11379226583756500423_6230_810_6250_810_with_camera_labels.tfrecord
+|-- ...
+`-- segment-1172406780360799916_1660_000_1680_000_with_camera_labels.tfrecord
+```
+
+> [!NOTE]
+> If you download the tfrecord files from the console, you will have prefixes like `individual_files_training_` or `individual_files_validation_`. Make sure these prefixes are removed before further processing.
+
+
+#### Step 2: Convert Waymo Open Dataset to RDS-HQ format
+
+Next, convert the Waymo Open Dataset to RDS-HQ format. Suppose you have a folder with Waymo Open Dataset's tfrecords, you can convert it to RDS-HQ format by:
+```bash
+python convert_waymo_to_rds_hq.py -i -o /videos -n 16
+```
+Here `` can be set to any folder you want, and number of workers can be changed from `16`.
+
+#### Step 3: Create T5 Text Embeddings
+Lastly, we need to create T5 text embeddings.
+Make sure you have completed Cosmos-predict1 installation and use the cosmos-predict1 environment for this step:
+```bash
+conda activate cosmos-predict1
+```
+We offer two set of captions, a more complete set of single view captions in `assets/waymo_caption.csv`, and a set of 5k multiview captions in `assets/waymo_multiview_texts.json`.
+To use the 5k multiview captions in `assets/waymo_multiview_texts.json`:
+```bash
+python create_t5_embed.py --text_file ./assets/waymo_multiview_texts.json --data_root
+```
+Alternatively, to use the single view captions in `assets/waymo_caption.csv`:
+```bash
+python create_t5_embed.py --caption_file ./assets/waymo_caption.csv --data_root
+```
+
+The resulting folder structure should look like this:
+```
+/waymo/
+├── cache/
+│ ├── prefix_t5_embeddings_pinhole_front.pickle
+│ ├── prefix_t5_embeddings_pinhole_front_left.pickle
+│ ├── prefix_t5_embeddings_pinhole_front_right.pickle
+│ ├── prefix_t5_embeddings_pinhole_side_left.pickle
+│ └── prefix_t5_embeddings_pinhole_side_right.pickle
+├── videos/
+│ ├── pinhole_front
+│ ├── *.mp4
+│ ├── pinhole_front_left
+│ ├── pinhole_front_right
+│ ├── pinhole_side_left
+│ ├── pinhole_side_right
+│ ...
+└── t5_xxl/
+ ├── pinhole_front
+ └── *.pkl
+```
+You are now ready to train cosmos-predict1 models on Waymo!
diff --git a/packages/terasim-world/docs/processing_waymo_for_transfer1.md b/packages/terasim-world/docs/processing_waymo_for_transfer1.md
new file mode 100644
index 0000000..53c5eb4
--- /dev/null
+++ b/packages/terasim-world/docs/processing_waymo_for_transfer1.md
@@ -0,0 +1,142 @@
+# Processing Waymo dataset for Cosmos-Transfer1-SampleAV
+
+## Installation
+If you haven't set up the data processing environment yet, follow our installation instructions [here](https://github.com/nv-tlabs/cosmos-av-sample-toolkits?tab=readme-ov-file#installation) to setup the environment.
+
+## Inference
+
+We provide pre-rendered examples [here](https://huggingface.co/datasets/nvidia/Cosmos-Transfer1-7B-Sample-AV-Data-Example/tree/main/examples) (rendered HDMAP / rendered LiDAR / text prompts). You can download and use these examples to test [**Cosmos-Transfer1-7B-Sample-AV**](https://github.com/nvidia-cosmos/cosmos-transfer1)!
+
+## Installation
+```bash
+git clone https://github.com/nv-tlabs/cosmos-av-sample-toolkits.git
+cd cosmos-av-sample-toolkits
+conda env create -f environment.yaml
+conda activate cosmos-av-toolkits
+```
+
+## Download Examples
+We provide 10 examples of input prompts with HD map and LiDAR, to help test the model.
+1. Add your SSH public key to your [user settings](https://huggingface.co/settings/keys) on Hugging Face.
+2. Download the examples from [Hugging Face](https://huggingface.co/datasets/nvidia/Cosmos-Transfer1-7B-Sample-AV-Data-Example) (about 8GB):
+```bash
+git lfs install
+git clone git@hf.co:datasets/nvidia/Cosmos-Transfer1-7B-Sample-AV-Data-Example
+```
+
+## Visualize Dataset
+You can use `visualize_rds_hq.py` to visualize the RDS-HQ dataset.
+```bash
+python visualize_rds_hq.py -i -c
+```
+This python script will launch a [viser](https://github.com/nerfstudio-project/viser) server to visualize the 3D HD map world with dynamic bounding boxes. You can use
+- `w a s d` to control camera's position
+- `q e` to control camera's z-axis coordinate
+
+## PostTraining
+
+We provide a conversion and rendering script for the Waymo Open Dataset as an example of how information from another AV source can interface with the model. Note that our model is not trained on the Waymo dataset, and this script is intended to help users better understand our data format. As a result, a drop in generative video quality is expected. Finetuning on the desired custom dataset would be beneficial to improve quality.
+
+### Waymo Open Dataset
+Parsing tfrecords from Waymo Open Dataset requires extra dependencies; install it with
+```bash
+pip install waymo-open-dataset-tf-2-11-0==1.6.1
+```
+
+#### Step 0: Check Our Provided Captions
+We provide auto-generated captions for the Waymo dataset at [`assets/waymo_caption.csv`](./assets/waymo_caption.csv). You will need these captions to run [**Cosmos-Transfer1-7B-Sample-AV**](https://github.com/nvidia-cosmos/cosmos-transfer1/blob/main/examples/inference_cosmos_transfer1_7b_sample_av.md).
+
+#### Step 1: Download Waymo Open Dataset
+
+Download the all the training & validation clips from [waymo perception dataset v1.4.2](https://waymo.com/open/download/) to the ``.
+
+If you have `sudo`, you can use [gcloud](https://cloud.google.com/storage/docs/discover-object-storage-gcloud) to download them from terminal.
+
+gcloud installation (need sudo) and downloading from terminal
+
+```bash
+sudo apt-get update
+sudo apt-get install apt-transport-https ca-certificates gnupg curl
+curl https://packages.cloud.google.com/apt/doc/apt-key.gpg | sudo gpg --dearmor -o /usr/share/keyrings/cloud.google.gpg
+echo "deb [signed-by=/usr/share/keyrings/cloud.google.gpg] https://packages.cloud.google.com/apt cloud-sdk main" | sudo tee -a /etc/apt/sources.list.d/google-cloud-sdk.list
+sudo apt-get update && sudo apt-get install google-cloud-cli
+```
+
+Then you can login your google account and download the above tfrecords via
+```bash
+# or use `gcloud init --no-launch-browser` if you are in a remote terminal session
+gcloud init
+bash download_waymo.sh config/waymo_all.json
+```
+
+
+After downloading tfrecord files, we expect a folder structure as follows:
+```bash
+
+|-- segment-10247954040621004675_2180_000_2200_000_with_camera_labels.tfrecord
+|-- segment-11379226583756500423_6230_810_6250_810_with_camera_labels.tfrecord
+|-- ...
+`-- segment-1172406780360799916_1660_000_1680_000_with_camera_labels.tfrecord
+```
+
+> [!NOTE]
+> If you download the tfrecord files from the console, you will have prefixes like `individual_files_training_` or `individual_files_validation_`. Make sure these prefixes are removed before further processing.
+
+
+#### Step 2: Convert Waymo Open Dataset to RDS-HQ format
+
+Next, convert the Waymo Open Dataset to RDS-HQ format. Suppose you have a folder with Waymo Open Dataset's tfrecords, you can convert it to RDS-HQ format by:
+```bash
+python convert_waymo_to_rds_hq.py -i -o /videos -n 16
+```
+Here `` can be set to any folder you want, and number of workers can be changed from `16`.
+
+#### Step 3: Render HD map + bounding box / LiDAR condition video from RDS-HQ format
+For single view post-training, run the following command:
+```bash
+python render_from_rds_hq.py -d waymo -i /videos -o -c pinhole -p True -n 8 -rl True
+```
+Or, for multiview training, run:
+```bash
+python render_from_rds_hq.py -d waymo_mv_short -i /videos -o -c pinhole -p True -n 8 -rl True
+```
+
+#### Step 4: Create T5 Text Embeddings
+Lastly, we need to create T5 text embeddings.
+Make sure to have completed Cosmos-transfer1 installation and use the cosmos-transfer1 environment for this step:
+```bash
+conda activate cosmos-transfer1
+
+python create_t5_embed.py --caption_file ./assets/waymo_caption.csv --data_root
+```
+The resulting folder structure should look like this if you are doing multiview training, or with only the `pinhole_front/` sub-folders if doing only front view training:
+```
+/waymo/
+├── cache/
+│ ├── prefix_pinhole_front.pkl
+│ ├── prefix_pinhole_front_left.pkl
+│ ├── prefix_pinhole_front_right.pkl
+│ ├── prefix_pinhole_side_left.pkl
+│ └── prefix_pinhole_side_right.pkl
+├── videos/
+│ ├── pinhole_front/
+│ │ └── *.mp4
+│ ├── pinhole_left/
+│ │ └── *.mp4
+│ ├── pinhole_front_right/
+│ │ └── *.mp4
+│ *
+├── hdmap/
+├── ├── pinhole_front/
+│ │ └── *.mp4
+│ *
+├── lidar/
+├── ├── pinhole_front/
+│ │ └── *.mp4
+│ *
+└── t5_xxl/
+ ├── pinhole_front
+ │ └── *.pkl
+ *
+```
+You are now ready to train cosmos-transfer models on Waymo!
\ No newline at end of file
diff --git a/packages/terasim-world/environment.yaml b/packages/terasim-world/environment.yaml
new file mode 100644
index 0000000..e922fbc
--- /dev/null
+++ b/packages/terasim-world/environment.yaml
@@ -0,0 +1,27 @@
+name: cosmos-av-toolkits
+channels:
+ - conda-forge
+dependencies:
+ - python=3.10
+ - conda-forge::ffmpeg
+ - numpy
+ - scipy
+ - click
+ - tqdm
+ - termcolor
+ - imageio
+ - trimesh
+ - pip:
+ - torch==2.4.0 --index-url=https://download.pytorch.org/whl/cu121
+ - opencv-python-headless
+ - pymap3d
+ - einops
+ - pyproj
+ - webdataset
+ - imageio[ffmpeg]
+ - decord
+ - viser
+ - python-pycg
+ - ray[default]
+ - shapely
+ - transformers
\ No newline at end of file
diff --git a/packages/terasim-cosmos/terasim_cosmos/render_from_rds_hq.py b/packages/terasim-world/render_from_rds_hq.py
similarity index 90%
rename from packages/terasim-cosmos/terasim_cosmos/render_from_rds_hq.py
rename to packages/terasim-world/render_from_rds_hq.py
index 305fd1c..6cce99e 100644
--- a/packages/terasim-cosmos/terasim_cosmos/render_from_rds_hq.py
+++ b/packages/terasim-world/render_from_rds_hq.py
@@ -20,13 +20,13 @@
from pathlib import Path
from termcolor import cprint, colored
from pathlib import Path
-from .utils.wds_utils import get_sample
-from .utils.bbox_utils import create_bbox_projection, interpolate_bbox, fix_static_objects
-from .utils.minimap_utils import create_minimap_projection, simplify_minimap
-from .utils.pcd_utils import batch_move_points_within_bboxes_sparse, forward_warp_multiframes_sparse_depth_only
-from .utils.camera.pinhole import PinholeCamera
-from .utils.camera.ftheta import FThetaCamera
-from .utils.ray_utils import ray_remote, wait_for_futures
+from utils.wds_utils import get_sample
+from utils.bbox_utils import create_bbox_projection, interpolate_bbox, fix_static_objects
+from utils.minimap_utils import create_minimap_projection, simplify_minimap
+from utils.pcd_utils import batch_move_points_within_bboxes_sparse, forward_warp_multiframes_sparse_depth_only
+from utils.camera.pinhole import PinholeCamera
+from utils.camera.ftheta import FThetaCamera
+from utils.ray_utils import ray_remote, wait_for_futures
USE_RAY = False
@@ -118,8 +118,7 @@ def prepare_input(input_root, clip_id, settings, camera_type, post_training, res
cprint(f"Ftheta intrinsic file does not exist: {intrinsic_file}", 'red')
cprint(f"===> So we will use default ftheta intrinsic for rendering", 'yellow', attrs=['bold'])
# intrinsic_file = 'config/default_ftheta_intrinsic.tar'
- package_root = Path(__file__).parent
- intrinsic_file = package_root / "assets/example/ftheta_intrinsic/2d23a1f4-c269-46aa-8e7d-1bb595d1e421_2445376400000_2445396400000.tar"
+ intrinsic_file = "assets/example/ftheta_intrinsic/2d23a1f4-c269-46aa-8e7d-1bb595d1e421_2445376400000_2445396400000.tar"
if 'CAMERAS_TO_RDS_HQ' in settings:
camera_name_in_rds_hq = settings['CAMERAS_TO_RDS_HQ'][camera_name]
else:
@@ -206,7 +205,6 @@ def prepare_output(
# save HD map condition video
output_writer = imageio_v1.get_writer(
output_root_p / render_name / camera_folder_name / f"{clip_id}_{cur_idx}.mp4",
- format='mp4',
fps=TARGET_RENDER_FPS,
codec="libx264",
macro_block_size=None, # This makes sure num_frames is correct (by default it is rounded to 16x).
@@ -233,9 +231,13 @@ def render_sample_hdmap(
resize_resolution: tuple[int, int] = (1280, 720),
cosmos_resolution: tuple[int, int] = (1280, 704),
resize_last: bool = False,
+ distance_threshold: float = 100.0, # Maximum distance to render in meters
):
"""
- Render HD map projections.
+ Render HD map projections with distance filtering.
+ Args:
+ ... (existing args) ...
+ distance_threshold: Maximum distance in meters to render. Objects beyond this distance will be filtered out.
"""
print(f"Processing {clip_id} {camera_type} from {input_root}...")
minimap_types = settings['MINIMAP_TYPES']
@@ -249,22 +251,51 @@ def render_sample_hdmap(
for camera_name, camera_model in camera_name_to_camera_model.items():
cprint(f"Processing minimap projection for {clip_id} {camera_name}...", 'green', attrs=['bold'])
pose_all_frames = camera_name_to_camera_poses[camera_name]
+ # camera_pose_initial_position = pose_all_frames[0][:3, 3]
+ camera_pose_last_position = pose_all_frames[-1][:3, 3]
+
minimaps_projection_merged = np.zeros((len(render_frame_ids), camera_model.height, camera_model.width, 3), dtype=np.uint8)
for minimap_wds_file in minimap_wds_files:
minimap_data_wo_meta_info, minimap_name = simplify_minimap(minimap_wds_file)
- # Create projection with all data
+ # Filter minimap data by distance
+ filtered_minimap_data = []
+ for point in minimap_data_wo_meta_info:
+ # Calculate distance from camera to each point
+ distances = np.linalg.norm(point - camera_pose_last_position, axis=1)
+ # Keep only points within distance threshold
+ mask = distances <= distance_threshold
+ if np.any(mask):
+ filtered_minimap_data.append(point[mask])
+
+ # Create projection with filtered data
minimap_projection = create_minimap_projection(
minimap_name,
- minimap_data_wo_meta_info,
+ filtered_minimap_data,
pose_all_frames[render_frame_ids],
camera_model
)
minimaps_projection_merged = np.maximum(minimaps_projection_merged, minimap_projection)
- # Add bounding box projection to the minimap
+ # Filter and add bounding box projection to the minimap
+ filtered_object_info = {}
+ for frame_id in render_frame_ids:
+ frame_key = f"{frame_id:06d}.all_object_info.json"
+ if frame_key in all_object_info:
+ frame_objects = all_object_info[frame_key]
+ filtered_objects = {}
+ for obj, obj_information in frame_objects.items():
+ # Extract object center from transformation matrix (last column, first 3 elements)
+ obj_center = np.array(obj_information['object_to_world'])[:3, 3]
+ # Calculate distance from camera to object center
+ distance = np.linalg.norm(obj_center - camera_pose_last_position)
+ if distance <= distance_threshold:
+ filtered_objects[obj] = obj_information
+ if filtered_objects:
+ filtered_object_info[frame_key] = filtered_objects
+
bounding_box_projection = create_bbox_projection(
- all_object_info,
+ filtered_object_info,
pose_all_frames,
render_frame_ids,
camera_model,
@@ -480,9 +511,7 @@ def main(input_root, output_root, dataset, camera_type, skip, post_training, res
print(f" -> 2. {colored('Center Crop', 'magenta', attrs=['bold'])} to {colored(cosmos_resolution, 'magenta', attrs=['bold'])}.")
# Load settings
- package_root = Path(__file__).parent
- config_path = package_root / f'config/dataset_{dataset}.json'
- with open(config_path, 'r') as file:
+ with open(f'config/dataset_{dataset}.json', 'r') as file:
settings = json.load(file)
# if novel_pose_folder is provided, we also change the output folder
diff --git a/packages/terasim-world/street_view_analysis.py b/packages/terasim-world/street_view_analysis.py
new file mode 100644
index 0000000..a87152e
--- /dev/null
+++ b/packages/terasim-world/street_view_analysis.py
@@ -0,0 +1,201 @@
+import sumolib
+import dotenv
+import os
+import requests
+from openai import OpenAI
+from pathlib import Path
+import base64
+import xml.etree.ElementTree as ET
+
+# Load environment variables
+dotenv.load_dotenv()
+
+class StreetViewRetrievalAndAnalysis:
+ """
+ Class for retrieving and analyzing street view images using Google Street View API and GPT-4 Vision
+ """
+
+ def __init__(self):
+ """
+ Initialize StreetViewRetrievalAndAnalysis
+
+ Args:
+ google_maps_api_key: Google Maps API key for street view retrieval
+ openai_api_key: OpenAI API key for image analysis
+ """
+ # Load API keys from environment if not provided
+ self.google_maps_api_key = os.getenv('GOOGLE_MAPS_API_KEY')
+ self.openai_api_key = os.getenv('OPENAI_API_KEY')
+
+ if not self.google_maps_api_key:
+ raise ValueError("Google Maps API key is required")
+ if not self.openai_api_key:
+ raise ValueError("OpenAI API key is required")
+
+ # Initialize OpenAI client
+ self.client = OpenAI(api_key=self.openai_api_key)
+
+ def get_street_view_image(self, latitude: float, longitude: float, heading: int = 0, pitch: int = 0, fov: int = 90) -> bytes:
+ """
+ Get a street view image from Google Street View API
+
+ Args:
+ latitude: Latitude coordinate
+ longitude: Longitude coordinate
+ heading: Heading angle in degrees (0-360)
+ pitch: Pitch angle in degrees (-90 to 90)
+ fov: Field of view in degrees (10-120)
+
+ Returns:
+ Image data as bytes
+ """
+ url = f"https://maps.googleapis.com/maps/api/streetview"
+ params = {
+ 'size': '600x400', # Image size
+ 'location': f'{latitude},{longitude}',
+ 'heading': heading,
+ 'pitch': pitch,
+ 'fov': fov,
+ 'key': self.google_maps_api_key
+ }
+
+ response = requests.get(url, params=params)
+ if response.status_code == 200:
+ return response.content
+ else:
+ raise Exception(f"Failed to get street view image: {response.status_code}")
+
+ def analyze_image_with_llm(self, image_data_list: list) -> str:
+ """
+ Analyze the image using GPT-4 Vision and generate environment description
+
+ Args:
+ image_data_list: List of image data as bytes
+
+ Returns:
+ Environment description as string
+ """
+ # Convert images to base64
+ base64_image_list = [base64.b64encode(image_data).decode('utf-8') for image_data in image_data_list]
+
+ # Construct message content with multiple images
+ content = [
+ {
+ "type": "text",
+ "text": "Please describe the environment and setting of this street view image. Focus on static elements like buildings, roads, vegetation, weather conditions, and overall atmosphere. Ignore any moving objects or people. This description will be used as a prompt for video generation."
+ },
+ ]
+
+ # Add each image to the content
+ for base64_image in base64_image_list:
+ content.append({
+ "type": "image_url",
+ "image_url": {
+ "url": f"data:image/jpeg;base64,{base64_image}"
+ }
+ })
+
+ response = self.client.chat.completions.create(
+ model="gpt-4o",
+ messages=[{
+ "role": "user",
+ "content": content
+ }],
+ max_tokens=500
+ )
+
+ return response.choices[0].message.content
+
+ def get_vehicle_position_view(self, path_to_fcd: Path,
+ vehicle_id: str,
+ timestep_start: int,
+ timestep_end: int,
+ timestep_interval: int = 10) -> list:
+ """
+ Get vehicle position and angle data from FCD file
+
+ Args:
+ path_to_fcd: Path to FCD XML file
+ vehicle_id: Vehicle ID to track
+ timestep_start: Starting timestep
+ timestep_end: Ending timestep
+ timestep_interval: Interval between timesteps
+
+ Returns:
+ List of tuples (x, y, angle) for vehicle positions
+ """
+ tree = ET.parse(path_to_fcd)
+ root = tree.getroot()
+ all_timesteps = list(root.findall("timestep"))
+ all_timesteps.sort(key=lambda x: float(x.get("time")))
+ target_timestep_list = all_timesteps[timestep_start:timestep_end:timestep_interval]
+
+ vehicle_position_angle_list = []
+ for target_timestep in target_timestep_list:
+ for vehicle in target_timestep.findall("vehicle"):
+ if vehicle.get("id") == vehicle_id:
+ vehicle_position_angle_list.append((
+ float(vehicle.get("x")),
+ float(vehicle.get("y")),
+ float(vehicle.get("angle"))
+ ))
+ return vehicle_position_angle_list
+
+ def get_streetview_image_and_description(self, path_to_output: Path,
+ path_to_fcd: Path,
+ path_to_map: Path,
+ vehicle_id: str = None,
+ timestep_start: int = -40,
+ timestep_end: int = -1,
+ fov: int = 120) -> str:
+ """
+ Get street view images and generate environment description
+
+ Args:
+ path_to_output: Output directory path
+ path_to_fcd: Path to FCD XML file
+ path_to_map: Path to SUMO network file
+ vehicle_id: Vehicle ID to track
+ timestep_start: Starting timestep
+ timestep_end: Ending timestep
+ fov: Field of view for street view images
+
+ Returns:
+ Environment description as string
+ """
+ # Load SUMO network
+ sumo_net = sumolib.net.readNet(path_to_map)
+
+ # Get vehicle positions
+ vehicle_position_angle_list = self.get_vehicle_position_view(
+ path_to_fcd=path_to_fcd,
+ vehicle_id=vehicle_id,
+ timestep_start=timestep_start,
+ timestep_end=timestep_end,
+ timestep_interval=timestep_end - timestep_start + 1
+ )
+
+ # Convert coordinates to lat/lon
+ vehicle_lon_lat_angle_list = []
+ for x, y, angle in vehicle_position_angle_list:
+ lon, lat = sumo_net.convertXY2LonLat(x, y)
+ vehicle_lon_lat_angle_list.append((lon, lat, angle))
+
+ # Get street view images
+ image_data_list = []
+ for i, (lon, lat, angle) in enumerate(vehicle_lon_lat_angle_list):
+ image_data = self.get_street_view_image(lat, lon, heading=angle, fov=fov)
+ image_data_list.append(image_data)
+
+ # Save image to file
+ with open(path_to_output / f"streetview_image_{i}.jpg", 'wb') as f:
+ f.write(image_data)
+ print(f"Street view image saved as {path_to_output} / streetview_image_{i}.jpg")
+
+ # Generate environment description
+ description = self.analyze_image_with_llm(image_data_list)
+ with open(path_to_output / f"streetview_description.txt", 'w') as f:
+ f.write(description)
+ print(f"Environment description saved as {path_to_output} / streetview_description.txt")
+
+ return description
diff --git a/packages/terasim-world/terasim_to_cosmos_input.py b/packages/terasim-world/terasim_to_cosmos_input.py
new file mode 100644
index 0000000..f25e5d1
--- /dev/null
+++ b/packages/terasim-world/terasim_to_cosmos_input.py
@@ -0,0 +1,90 @@
+from pathlib import Path
+import json
+
+from convert_terasim_to_rds_hq import convert_terasim_to_wds
+from render_from_rds_hq import render_sample_hdmap
+from street_view_analysis import StreetViewRetrievalAndAnalysis
+
+
+def terasim_to_cosmos_input(path_to_output: Path,
+ path_to_fcd: Path,
+ path_to_map: Path,
+ camera_setting_name: str = "default",
+ vehicle_id: str = None,
+ timestep_start: int = 0,
+ timestep_end: int = 100,
+ streetview_retrieval: bool = False):
+
+ print(f"Processing fcd: {path_to_fcd}")
+ print(f"Processing map: {path_to_map}")
+
+ if vehicle_id is None:
+ print("No vehicle id provided, trying to load from monitor.json")
+ try:
+ path_to_collition_record = path_to_output / "monitor.json"
+ with open(path_to_collition_record, "r") as f:
+ collision_record = json.load(f)
+ vehicle_id = collision_record["veh_1_id"]
+ print(f"Using vehicle id: {vehicle_id}")
+ except Exception as e:
+ raise Exception(f"Error loading monitor.json: {e}")
+
+ if streetview_retrieval:
+ # Use the new class for street view retrieval and analysis
+ streetview_analyzer = StreetViewRetrievalAndAnalysis()
+ streetview_analyzer.get_streetview_image_and_description(
+ path_to_output=path_to_output,
+ path_to_fcd=path_to_fcd,
+ path_to_map=path_to_map,
+ vehicle_id=vehicle_id,
+ timestep_start=timestep_start,
+ timestep_end=timestep_end
+ )
+
+ convert_terasim_to_wds(
+ terasim_record_root=path_to_output,
+ path_to_fcd=path_to_fcd,
+ path_to_map=path_to_map,
+ output_wds_path=path_to_output / "wds",
+ single_camera=False,
+ camera_setting_name=camera_setting_name,
+ av_id=vehicle_id,
+ timestep_start=timestep_start,
+ timestep_end=timestep_end
+ )
+
+ if camera_setting_name == "waymo":
+ settings = json.load(open("config/dataset_waymo_mv.json", "r"))
+ elif camera_setting_name == "default":
+ settings = json.load(open("config/dataset_rds_hq_mv_terasim.json", "r"))
+ else:
+ raise ValueError(f"Invalid camera setting name: {camera_setting_name}")
+
+ render_sample_hdmap(
+ input_root=path_to_output / "wds",
+ output_root=path_to_output / "render" / vehicle_id,
+ clip_id=path_to_output.stem,
+ settings=settings,
+ camera_type="ftheta",
+ )
+ return True
+
+
+if __name__ == "__main__":
+ # Example usage
+ path_to_output = Path("/path/to/terasim/output") # Replace with your TeraSim output directory
+ path_to_fcd = path_to_output / "fcd_all.xml"
+ path_to_map = path_to_output / "map.net.xml"
+ camera_setting_name = "default" # "waymo" or "default"
+ vehicle_id = None # Will auto-detect from monitor.json
+ timestep_start = -40
+ timestep_end = -1
+
+ terasim_to_cosmos_input(path_to_output=path_to_output,
+ path_to_fcd=path_to_fcd,
+ path_to_map=path_to_map,
+ camera_setting_name=camera_setting_name,
+ vehicle_id=vehicle_id,
+ timestep_start=timestep_start,
+ timestep_end=timestep_end,
+ streetview_retrieval=True)
diff --git a/packages/terasim-world/terasim_vis.py b/packages/terasim-world/terasim_vis.py
new file mode 100644
index 0000000..a9ef85a
--- /dev/null
+++ b/packages/terasim-world/terasim_vis.py
@@ -0,0 +1,31 @@
+import matplotlib
+import matplotlib.animation as animation
+import matplotlib.pyplot as plt
+
+import terasim_vis
+matplotlib.use('Agg')
+# Load net file and trajectory file
+net = terasim_vis.Net("path/to/road/directory/map.net.xml")
+trajectories = terasim_vis.Trajectories("path/to/road/directory/fcd_all.xml")
+# Set trajectory color for different vehicles
+for trajectory in trajectories:
+ if trajectory.id == "CAV":
+ trajectory.assign_colors_constant("#ff0000")
+ else:
+ trajectory.assign_colors_constant("#00FF00")
+
+# Show the generated trajectory video
+fig, ax = plt.subplots()
+ax.set_aspect('equal', adjustable='box') # same scale
+artist_collection = net.plot(ax=ax)
+plot_time_interaval = trajectories.timestep_range()[-100:] # only plot 10s before the end of the trajectories, can be modified later
+a = animation.FuncAnimation(
+ fig,
+ trajectories.plot_points,
+ frames=plot_time_interaval,
+ interval=1,
+ fargs=(ax, True, artist_collection.lanes),
+ blit=False,
+)
+# plt.show()
+a.save("test.mp4", writer=animation.FFMpegWriter(fps=10), dpi=300)
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/animation_utils.py b/packages/terasim-world/utils/animation_utils.py
similarity index 100%
rename from packages/terasim-cosmos/terasim_cosmos/utils/animation_utils.py
rename to packages/terasim-world/utils/animation_utils.py
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/bbox_utils.py b/packages/terasim-world/utils/bbox_utils.py
similarity index 99%
rename from packages/terasim-cosmos/terasim_cosmos/utils/bbox_utils.py
rename to packages/terasim-world/utils/bbox_utils.py
index 1e71739..6b0e72b 100644
--- a/packages/terasim-cosmos/terasim_cosmos/utils/bbox_utils.py
+++ b/packages/terasim-world/utils/bbox_utils.py
@@ -9,7 +9,7 @@
import numpy as np
from tqdm import tqdm
-from .minimap_utils import cuboid3d_to_polyline
+from utils.minimap_utils import cuboid3d_to_polyline
from scipy.spatial.transform import Rotation as R
from scipy.spatial.transform import Slerp
from pycg import Isometry
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/camera/base.py b/packages/terasim-world/utils/camera/base.py
similarity index 99%
rename from packages/terasim-cosmos/terasim_cosmos/utils/camera/base.py
rename to packages/terasim-world/utils/camera/base.py
index ff2b019..bbd97ec 100644
--- a/packages/terasim-cosmos/terasim_cosmos/utils/camera/base.py
+++ b/packages/terasim-world/utils/camera/base.py
@@ -9,7 +9,7 @@
import torch
import numpy as np
import cv2
-from ..pcd_utils import interpolate_polyline_to_points
+from utils.pcd_utils import interpolate_polyline_to_points
from typing import List, Union
from abc import abstractmethod
from shapely.geometry import Polygon, LineString
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/camera/ftheta.py b/packages/terasim-world/utils/camera/ftheta.py
similarity index 99%
rename from packages/terasim-cosmos/terasim_cosmos/utils/camera/ftheta.py
rename to packages/terasim-world/utils/camera/ftheta.py
index 3579e2b..a03ca25 100644
--- a/packages/terasim-cosmos/terasim_cosmos/utils/camera/ftheta.py
+++ b/packages/terasim-world/utils/camera/ftheta.py
@@ -15,7 +15,7 @@
from numpy.polynomial.polynomial import Polynomial
from scipy.optimize import curve_fit
-from .base import CameraBase
+from utils.camera.base import CameraBase
CropParams = TypeVar("CropParams")
ScaleParams = TypeVar("ScaleParams")
@@ -541,7 +541,7 @@ def pixel2ray_torch(self, pixels: torch.Tensor) -> torch.Tensor:
rays (torch.Tensor): ray direction vector. shape: (M, 3)
valid (torch.Tensor): bool flag indicating the validity of each backprojected pixel. shape: (M,)
"""
- # ensure the input shape is (n_points, 2)
+ # 确保输入形状为 (n_points, 2)
if pixels.dim() == 1:
pixels = pixels.unsqueeze(0)
@@ -565,7 +565,7 @@ def pixel2ray_torch(self, pixels: torch.Tensor) -> torch.Tensor:
rays[valid, 1] = sin_alpha[valid] * xd[valid, 1] / xd_norm[valid]
rays[valid, 2] = torch.cos(alpha[valid])
- # for the invalid point set to (0,0,1)
+ # For the invalid point set to (0,0,1)
rays[~valid, 0] = 0
rays[~valid, 1] = 0
rays[~valid, 2] = 1
@@ -588,7 +588,7 @@ def ray2pixel_np(self, rays: np.ndarray) -> np.ndarray:
# Fix the type
rays = rays.astype(np.float32)
- # TODO(restes) combine 2 and 3 column norm for rays? (optional)
+ # TODO(restes) combine 2 and 3 column norm for rays?
xy_norm = np.linalg.norm(rays[:, :2], axis=1, keepdims=True)
cos_alpha = rays[:, 2:] / np.linalg.norm(rays, axis=1, keepdims=True)
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/camera/pinhole.py b/packages/terasim-world/utils/camera/pinhole.py
similarity index 99%
rename from packages/terasim-cosmos/terasim_cosmos/utils/camera/pinhole.py
rename to packages/terasim-world/utils/camera/pinhole.py
index 218683e..4263e3b 100644
--- a/packages/terasim-cosmos/terasim_cosmos/utils/camera/pinhole.py
+++ b/packages/terasim-world/utils/camera/pinhole.py
@@ -9,7 +9,7 @@
import torch
import numpy as np
-from .base import CameraBase
+from utils.camera.base import CameraBase
class PinholeCamera(CameraBase):
def __init__(self, fx, fy, cx, cy, w, h, dtype=torch.float32, device=None):
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/deepmap_utils.py b/packages/terasim-world/utils/deepmap_utils.py
similarity index 100%
rename from packages/terasim-cosmos/terasim_cosmos/utils/deepmap_utils.py
rename to packages/terasim-world/utils/deepmap_utils.py
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/minimap_utils.py b/packages/terasim-world/utils/minimap_utils.py
similarity index 99%
rename from packages/terasim-cosmos/terasim_cosmos/utils/minimap_utils.py
rename to packages/terasim-world/utils/minimap_utils.py
index e7c466d..8b42014 100644
--- a/packages/terasim-cosmos/terasim_cosmos/utils/minimap_utils.py
+++ b/packages/terasim-world/utils/minimap_utils.py
@@ -12,7 +12,7 @@
from pathlib import Path
from scipy.spatial.transform import Rotation as R
from termcolor import cprint
-from .wds_utils import get_sample
+from utils.wds_utils import get_sample
MINIMAP_TO_RGB = json.load(open(Path(__file__).parent.parent / 'config' /'hdmap_color_config.json'))
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/pcd_utils.py b/packages/terasim-world/utils/pcd_utils.py
similarity index 100%
rename from packages/terasim-cosmos/terasim_cosmos/utils/pcd_utils.py
rename to packages/terasim-world/utils/pcd_utils.py
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/post_training_utils.py b/packages/terasim-world/utils/post_training_utils.py
similarity index 100%
rename from packages/terasim-cosmos/terasim_cosmos/utils/post_training_utils.py
rename to packages/terasim-world/utils/post_training_utils.py
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/ray_utils.py b/packages/terasim-world/utils/ray_utils.py
similarity index 100%
rename from packages/terasim-cosmos/terasim_cosmos/utils/ray_utils.py
rename to packages/terasim-world/utils/ray_utils.py
diff --git a/packages/terasim-cosmos/terasim_cosmos/utils/wds_utils.py b/packages/terasim-world/utils/wds_utils.py
similarity index 100%
rename from packages/terasim-cosmos/terasim_cosmos/utils/wds_utils.py
rename to packages/terasim-world/utils/wds_utils.py
diff --git a/packages/terasim-cosmos/terasim_cosmos/visualize_rds_hq.py b/packages/terasim-world/visualize_rds_hq.py
similarity index 89%
rename from packages/terasim-cosmos/terasim_cosmos/visualize_rds_hq.py
rename to packages/terasim-world/visualize_rds_hq.py
index 19a8996..1ee4bde 100644
--- a/packages/terasim-cosmos/terasim_cosmos/visualize_rds_hq.py
+++ b/packages/terasim-world/visualize_rds_hq.py
@@ -16,9 +16,9 @@
import json
from pycg import Isometry
-from .utils.wds_utils import write_to_tar, get_sample
-from .utils.minimap_utils import simplify_minimap
-from .utils.animation_utils import FreePoseAnimator, InterpType
+from utils.wds_utils import write_to_tar, get_sample
+from utils.minimap_utils import simplify_minimap
+from utils.animation_utils import FreePoseAnimator, InterpType
from pathlib import Path
MINIMAP_TO_TYPE = json.load(open(Path(__file__).parent / 'config' /'hdmap_type_config.json'))
@@ -214,33 +214,32 @@ def draw_bev_projection(gui_image_handler, client_camera_pose, label_to_line_seg
def add_camera_frustums(server, camera_poses, current_frame_idx, all_cameras):
"""
- Add camera frustums to the viser server for all cameras at the current frame.
- Different cameras are represented with different colors.
+ 为所有相机添加视锥体
Args:
- server: viser server
- camera_poses: camera poses
- current_frame_idx: current frame index
- all_cameras: all camera names
+ server: viser服务器
+ camera_poses: 相机姿态数据
+ current_frame_idx: 当前帧索引
+ all_cameras: 所有相机名称列表
"""
- # create frustums for each camera
+ # 为每个相机创建视锥体
for i, camera_name in enumerate(all_cameras):
camera_pose = camera_poses[f'{current_frame_idx:06d}.pose.{camera_name}.npy']
camera_wxyz = tf.SE3.from_matrix(camera_pose).wxyz_xyz[:4]
camera_position = camera_pose[:3, 3]
- # set different colors based on the camera position
- if i == 0: # front camera
- color = np.array([1.0, 0, 0]) # red
+ # 根据相机位置设置不同的颜色
+ if i == 0: # 前相机
+ color = np.array([1.0, 0, 0]) # 红色
fov = 60 / 180 * np.pi
- # elif i == 1: # left camera
- # color = np.array([0, 1.0, 0]) # green
+ # elif i == 1: # 左相机
+ # color = np.array([0, 1.0, 0]) # 绿色
# fov = 30 / 180 * np.pi
- elif i == len(all_cameras) - 1: # back camera
- color = np.array([0, 0, 1.0]) # blue
+ elif i == len(all_cameras) - 1: # 后相机
+ color = np.array([0, 0, 1.0]) # 蓝色
fov = 30 / 180 * np.pi
- else: # other cameras
- color = np.array([1.0, 1.0, 0]) # yellow
+ else: # 其他相机
+ color = np.array([1.0, 1.0, 0]) # 黄色
fov = 30 / 180 * np.pi
server.scene.add_camera_frustum(
@@ -257,18 +256,16 @@ def add_camera_frustums(server, camera_poses, current_frame_idx, all_cameras):
@click.command()
@click.option('--input_root', '-i', type=str, required=True,
- help='The root directory of the webdataset (wds) folder')
+ help='The root directory of the webdataset')
@click.option('--novel_pose_folder', '-np', type=str, default='novel_pose',
help='The folder name of the novel pose data. If provided, we will render the novel ego trajectory')
-@click.option('--dataset', '-d', type=str, default='rds_hq_mv_terasim',
+@click.option('--dataset', '-d', type=str, default='rds_hq',
help='The dataset name, "rds_hq" or "waymo"')
-@click.option('--clip_id', '-c', type=str, help='clip id to visualize', required=True)
+@click.option('--clip_id', '-c', type=str, help='clip id to visualize')
def main(input_root, novel_pose_folder, dataset, clip_id):
server = viser.ViserServer()
- package_root = Path(__file__).parent
- config_path = package_root / f'config/dataset_{dataset}.json'
- with open(config_path, 'r') as file:
+ with open(f'config/dataset_{dataset}.json', 'r') as file:
settings = json.load(file)
label_to_line_segments = {}
@@ -276,14 +273,14 @@ def main(input_root, novel_pose_folder, dataset, clip_id):
camera_poses = get_sample(os.path.join(input_root, f"pose/{clip_id}.tar"))
frame_num = len([x for x in camera_poses.keys() if x.endswith(f'pose.{all_cameras[0]}.npy')])
- # get the first camera position and pose of the 0th frame
+ # 获取第0帧的第一个相机位置和姿态
initial_camera_pose = camera_poses[f'000000.pose.{all_cameras[0]}.npy']
initial_camera_position = initial_camera_pose[:3, 3]
initial_camera_wxyz = tf.SE3.from_matrix(initial_camera_pose).wxyz_xyz[:4]
- # set the offset: add the offset to the first camera position
- # offset: x=0, y=-2(back), z=3(up) - so the user's view is behind the first camera
- camera_offset = np.array([0, -4, 3]) # can be adjusted as needed
+ # 设置偏移量：在第一个相机位置基础上添加偏移
+ # 偏移量：x=0, y=-2(后方), z=3(上方) - 这样用户视角在第一个相机后方上方
+ camera_offset = np.array([0, -4, 3]) # 可以根据需要调整
initial_camera_position_with_offset = initial_camera_position + camera_offset
all_camera_to_front_camera = [
@@ -324,7 +321,7 @@ def main(input_root, novel_pose_folder, dataset, clip_id):
)
gui_reset_to_first_camera_button = server.gui.add_button(
"Reset to First Camera View",
- hint="Reset camera view to current frame's first camera position and orientation",
+ hint="Reset camera view to first camera's initial position and orientation",
)
gui_image_handler = server.gui.add_image(
image = np.ones((bev_map_h_pixel, bev_map_w_pixel, 3), dtype=np.uint8) * 255,
@@ -439,25 +436,13 @@ def _(event: viser.GuiEvent):
assert client is not None
try:
- # get the current frame index
- current_frame_idx = gui_frame_slider.value
-
- # get the first camera pose of the current frame
- current_camera_pose = camera_poses[f'{current_frame_idx:06d}.pose.{all_cameras[0]}.npy']
- current_camera_position = current_camera_pose[:3, 3]
- current_camera_wxyz = tf.SE3.from_matrix(current_camera_pose).wxyz_xyz[:4]
-
- # set the offset: add the offset to the first camera position
- camera_offset = np.array([0, -4, 3]) # behind the camera
- current_camera_position_with_offset = current_camera_position + camera_offset
-
for client_id, client in server.get_clients().items():
- client.camera.position = current_camera_position_with_offset
- client.camera.wxyz = current_camera_wxyz
-
+ client.camera.position = initial_camera_position_with_offset
+ client.camera.wxyz = initial_camera_wxyz
+
client.add_notification(
title="Camera View Reset",
- body=f"Camera view reset to frame {current_frame_idx} first camera position: {current_camera_position_with_offset}",
+ body=f"Camera view reset to first camera's initial position: {initial_camera_position_with_offset}",
loading=False,
with_close_button=True,
auto_close=3000,
@@ -559,7 +544,7 @@ def _(event: viser.GuiEvent):
while-true loop for visualization
"""
prev_frame_idx = 0
- first_frame = True # mark if it is the first frame
+ first_frame = True # 标记是否是第一帧
while True:
current_frame_idx = gui_frame_slider.value
current_frame_idx_divisible_by_3 = current_frame_idx // 3 * 3
@@ -567,7 +552,7 @@ def _(event: viser.GuiEvent):
label_to_line_segments['current_dynamic_objects'] = frame_idx_to_dynamic_object_line_segments[current_frame_idx_divisible_by_3]
label_to_line_segments['ego_car'] = frame_idx_to_ego_car_line_segments[current_frame_idx]
- # create frustums for all cameras
+ # 为所有相机创建视锥体
add_camera_frustums(server, camera_poses, current_frame_idx, all_cameras)
# create bev projection for current frame
diff --git a/packages/terasim/terasim/simulator.py b/packages/terasim/terasim/simulator.py
index 79b5efb..17ed352 100644
--- a/packages/terasim/terasim/simulator.py
+++ b/packages/terasim/terasim/simulator.py
@@ -32,6 +32,7 @@ def __init__(
self,
sumo_config_file_path: str or Path,
sumo_net_file_path: str or Path,
+ sumo_additional_file_path: str or Path = None,
num_tries: int = 10,
gui_flag: bool = False,
output_path: str or Path = None,
@@ -59,6 +60,7 @@ def __init__(
"""
self.sumo_net_file_path = Path(sumo_net_file_path)
self.sumo_config_file_path = Path(sumo_config_file_path)
+ self.sumo_additional_file_path = Path(sumo_additional_file_path) if sumo_additional_file_path is not None else None
assert self.sumo_net_file_path.exists(), "sumo_net_file_path does not exist"
assert self.sumo_config_file_path.exists(), "sumo_config_file_path does not exist"
self.sumo_net = sumolib.net.readNet(str(self.sumo_net_file_path), withInternal=True, withPrograms=True)
@@ -79,7 +81,8 @@ def __init__(
self.additional_sumo_args = (
[additional_sumo_args]
if isinstance(additional_sumo_args, str)
- else additional_sumo_args
+ else additional_sumo_args if additional_sumo_args is not None
+ else []
)
if seed is not None:
self.additional_sumo_args += ["--seed", str(seed)]
@@ -143,6 +146,8 @@ def start(self):
"-c",
str(self.sumo_config_file_path),
]
+ if self.sumo_additional_file_path is not None:
+ sumo_cmd += ["--additional-files", str(self.sumo_additional_file_path)]
if self.step_length is not None:
sumo_cmd += ["--step-length", str(self.step_length)]
diff --git a/pyproject.toml b/pyproject.toml
index de6e4e4..6799c4b 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -8,9 +8,7 @@ authors = [
readme = "README.md"
license = "Apache-2.0"
requires-python = ">=3.10,<3.13"
-dependencies = [
- "terasim-cosmos",
-]
+dependencies = []
[tool.uv.workspace]
members = [
@@ -19,8 +17,7 @@ members = [
"packages/terasim-service",
"packages/terasim-envgen",
"packages/terasim-datazoo",
- "packages/terasim-vis",
- "packages/terasim-cosmos"
+ "packages/terasim-vis"
]
[tool.uv.sources]
@@ -31,7 +28,6 @@ terasim-service = { workspace = true }
terasim-envgen = { workspace = true }
terasim-datazoo = { workspace = true }
terasim-vis = { workspace = true }
-terasim-cosmos = { workspace = true }
[dependency-groups]
dev = [
@@ -91,4 +87,4 @@ exclude_lines = [
"raise NotImplementedError",
"if 0:",
"if __name__ == .__main__.:"
-]
+]
\ No newline at end of file
diff --git a/scripts/run_experiments.py b/run_experiments.py
similarity index 97%
rename from scripts/run_experiments.py
rename to run_experiments.py
index 9dfd9d6..508a826 100644
--- a/scripts/run_experiments.py
+++ b/run_experiments.py
@@ -103,10 +103,10 @@ def run_simulation(config_file="examples/scenarios/police_pullover_case.yaml", a
# Example 2: Run simulation with visualization
result = run_simulation(
- config_file="texas_example/test_configs/cutin.yaml",
+ config_file="/home/sdai/harry/TeraSim/jupiter/eb/test_config.yaml",
enable_viz=True, # Enable visualization
viz_port=8050, # Visualization port
viz_update_freq=2 # Update every 2 simulation steps (reduce load)
)
- print(f"Final simulation result: {result}")
+ print(f"Final simulation result: {result}")
\ No newline at end of file
diff --git a/scripts/run_experiments_debug.py b/run_experiments_debug.py
similarity index 63%
rename from scripts/run_experiments_debug.py
rename to run_experiments_debug.py
index ddfef20..a7b7732 100644
--- a/scripts/run_experiments_debug.py
+++ b/run_experiments_debug.py
@@ -1,8 +1,10 @@
import argparse
+import random
import hydra
from loguru import logger
from omegaconf import DictConfig, OmegaConf
from pathlib import Path
+from tqdm import tqdm
from terasim.logger.infoextractor import InfoExtractor
from terasim.simulator import Simulator
@@ -29,8 +31,8 @@ def main(config_path: str) -> None:
base_dir = Path(config.output.dir) / config.output.name / "raw_data" / config.output.nth
base_dir.mkdir(parents=True, exist_ok=True)
- env = NADEWithAV( # NADEWithAV or NADE
- av_cfg = config.environment.parameters.AV_cfg,
+ env = NADEWithAV(
+ av_cfg=config.environment.parameters.AV_cfg,
vehicle_factory=NDEVehicleFactory(cfg=config.environment.parameters),
vru_factory=NDEVulnerableRoadUserFactory(cfg=config.environment.parameters),
info_extractor=InfoExtractor,
@@ -46,16 +48,23 @@ def main(config_path: str) -> None:
# Paths already resolved in config
sumo_net_file = config.input.sumo_net_file
sumo_config_file = config.input.sumo_config_file
+ # sumo_additional_file = config.input.sumo_additional_file
+ sumo_additional_file = "./vTypeDistributions.add.xml"
sim = Simulator(
sumo_net_file_path=sumo_net_file,
sumo_config_file_path=sumo_config_file,
+ sumo_additional_file_path=sumo_additional_file,
num_tries=10,
gui_flag=config.simulator.parameters.gui_flag,
realtime_flag=config.simulator.parameters.realtime_flag,
output_path=base_dir,
- sumo_output_file_types=config.simulator.parameters.sumo_output_file_types,
- traffic_scale=config.simulator.parameters.traffic_scale if hasattr(config.simulator.parameters, "traffic_scale") else 1,
+ sumo_output_file_types=["collision"],
+ traffic_scale=(
+ config.simulator.parameters.traffic_scale
+ if hasattr(config.simulator.parameters, "traffic_scale")
+ else 1
+ ),
additional_sumo_args=[
"--device.bluelight.explicit",
"true",
@@ -70,25 +79,22 @@ def main(config_path: str) -> None:
if __name__ == "__main__":
- parser = argparse.ArgumentParser(description="Run TeraSim debug experiments with YAML configuration")
- parser.add_argument(
- "--config",
- type=str,
- default="configs/simulation/test.yaml",
- help="Path to YAML configuration file (default: configs/simulation/test.yaml)"
- )
-
- args = parser.parse_args()
-
- # Convert to Path object and validate
- config_path = Path(args.config)
- if not config_path.exists():
- logger.error(f"Configuration file not found: {config_path}")
- exit(1)
-
- if not config_path.suffix.lower() in ['.yaml', '.yml']:
- logger.error(f"Configuration file must be a YAML file: {config_path}")
- exit(1)
-
- logger.info(f"Running experiment with config: {config_path}")
- main(str(config_path))
+ # Get all yaml files in examples/scenarios directory
+ config_dir = Path(__file__).parent / "examples" / "scenarios"
+ # yaml_files = sorted(config_dir.glob("*.yaml"), key=lambda x: int(''.join(filter(str.isdigit, x.stem)) or '0'))
+ # yaml_files = ["examples/scenarios/cutin.yaml"]
+ yaml_files = [Path("/home/sdai/harry/TeraSim/jupiter/eb/test_config.yaml")]
+ # Randomly shuffle yaml files
+ random.shuffle(yaml_files)
+
+ # Run experiments for each yaml file
+ for yaml_file in tqdm(yaml_files):
+ print(yaml_file)
+ logger.info(f"Running experiment with config: {yaml_file}")
+ main(str(yaml_file))
+ # try:
+ # main(str(yaml_file))
+ # except Exception as e:
+ # logger.error(f"Error running {yaml_file}: {e}")
+ # # yaml_file.unlink() # Delete the yaml file
+ # continue
diff --git a/scripts/run_service.py b/run_service.py
similarity index 100%
rename from scripts/run_service.py
rename to run_service.py
diff --git a/scripts/convert_terasim_to_cosmos.py b/scripts/convert_terasim_to_cosmos.py
deleted file mode 100644
index 8e06c83..0000000
--- a/scripts/convert_terasim_to_cosmos.py
+++ /dev/null
@@ -1,61 +0,0 @@
-from pathlib import Path
-import sys
-import argparse
-
-from terasim_cosmos import TeraSimToCosmosConverter
-
-
-if __name__ == "__main__":
- parser = argparse.ArgumentParser(
- description="Convert TeraSim simulation data to Cosmos-Drive compatible format"
- )
-
- # Required parameters
- parser.add_argument("--path_to_output", type=Path, required=True,
- help="Output directory path")
- parser.add_argument("--path_to_fcd", type=Path, required=True,
- help="Path to FCD (Floating Car Data) XML file")
- parser.add_argument("--path_to_map", type=Path, required=True,
- help="Path to SUMO network map XML file")
- parser.add_argument("--time_start", type=float, required=True,
- help="Start time in seconds")
- parser.add_argument("--time_end", type=float, required=True,
- help="End time in seconds")
- parser.add_argument("--vehicle_id", type=str, required=True,
- help="ID of the ego vehicle")
-
- # Optional parameters
- parser.add_argument("--camera_setting_name", type=str, choices=["default", "waymo"],
- default="default",
- help="Camera configuration setting (default: %(default)s)")
- parser.add_argument("--agent_clip_distance", type=float, default=80.0,
- help="Distance threshold to show agents (default: %(default)s) meters")
- parser.add_argument("--map_clip_distance", type=float, default=100.0,
- help="Distance threshold to show map features (default: %(default)s) meters")
-
- # Processing options
- parser.add_argument("--streetview_retrieval", action="store_true",
- help="Retrieve street view imagery and generate text descriptions")
- parser.add_argument("--no_streetview_retrieval", dest="streetview_retrieval", action="store_false",
- help="Disable street view retrieval")
- parser.set_defaults(streetview_retrieval=True)
-
- args = parser.parse_args()
-
- # Create config dictionary from command line arguments
- config_dict = {
- "path_to_output": str(args.path_to_output),
- "path_to_fcd": str(args.path_to_fcd),
- "path_to_map": str(args.path_to_map),
- "camera_setting_name": args.camera_setting_name,
- "vehicle_id": args.vehicle_id,
- "time_start": args.time_start,
- "time_end": args.time_end,
- "agent_clip_distance": args.agent_clip_distance,
- "map_clip_distance": args.map_clip_distance,
- "streetview_retrieval": args.streetview_retrieval,
- }
-
- # Create converter and run conversion
- converter = TeraSimToCosmosConverter.from_config_dict(config_dict)
- converter.convert()
\ No newline at end of file
diff --git a/scripts/generate_experiments.py b/scripts/generate_experiments.py
deleted file mode 100755
index c47beed..0000000
--- a/scripts/generate_experiments.py
+++ /dev/null
@@ -1,384 +0,0 @@
-#!/usr/bin/env python3
-"""
-TeraSim Experiment Generator
-
-Generate complete simulation scenarios from latitude/longitude coordinates.
-This tool combines map downloading, format conversion, and traffic flow generation
-to create ready-to-use simulation environments for autonomous vehicle testing.
-
-Usage Examples:
- # Single scenario generation
- python generate_experiments.py --lat 42.2775 --lon -83.7347 --bbox 500 --name ann_arbor
-
- # Batch generation from file
- python generate_experiments.py --batch coordinates.txt --bbox 300
-
- # Custom traffic densities
- python generate_experiments.py --lat 30.2309 --lon -97.7335 --traffic low medium
-
- # Specify output directory and formats
- python generate_experiments.py --lat 37.7749 --lon -122.4194 --output experiments/sf --formats sumo opendrive
-"""
-
-import argparse
-import json
-import logging
-import sys
-from pathlib import Path
-from typing import List, Tuple, Optional, Dict
-import time
-
-try:
- from terasim_envgen import IntegratedScenarioGenerator
-except ImportError as e:
- print(f"Error importing terasim_envgen: {e}")
- print("Please ensure terasim-envgen is installed: poetry install")
- sys.exit(1)
-
-# Configure logging
-logging.basicConfig(
- level=logging.INFO,
- format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
-)
-logger = logging.getLogger(__name__)
-
-
-def parse_arguments():
- """Parse command line arguments."""
- parser = argparse.ArgumentParser(
- description="Generate TeraSim simulation scenarios from geographic coordinates",
- formatter_class=argparse.RawDescriptionHelpFormatter,
- epilog=__doc__
- )
-
- # Input options (mutually exclusive)
- input_group = parser.add_mutually_exclusive_group(required=True)
- input_group.add_argument(
- "--lat",
- type=float,
- help="Latitude of the center point for single scenario"
- )
- input_group.add_argument(
- "--batch",
- type=str,
- help="Path to file containing lat,lon pairs (one per line)"
- )
- input_group.add_argument(
- "--coordinates",
- nargs="+",
- type=float,
- help="List of coordinates as: lat1 lon1 lat2 lon2 ..."
- )
-
- # Longitude (required with --lat)
- parser.add_argument(
- "--lon",
- type=float,
- help="Longitude of the center point (required with --lat)"
- )
-
- # Common parameters
- parser.add_argument(
- "--bbox",
- type=int,
- default=500,
- help="Size of bounding box in meters (default: 500)"
- )
-
- parser.add_argument(
- "--output",
- type=str,
- default="generated_experiments",
- help="Output directory for generated files (default: generated_experiments)"
- )
-
- parser.add_argument(
- "--name",
- type=str,
- help="Name for the scenario (auto-generated if not provided)"
- )
-
- parser.add_argument(
- "--traffic",
- nargs="+",
- choices=["low", "medium", "high"],
- default=["low", "medium", "high"],
- help="Traffic density levels to generate (default: all)"
- )
-
- parser.add_argument(
- "--formats",
- nargs="+",
- choices=["sumo", "opendrive", "lanelet2"],
- default=["sumo"],
- help="Map formats to convert to (default: sumo)"
- )
-
- parser.add_argument(
- "--config",
- type=str,
- help="Path to configuration file for advanced settings"
- )
-
- parser.add_argument(
- "--route",
- type=str,
- help="Path to file containing AV route coordinates (lat,lon per line)"
- )
-
- parser.add_argument(
- "--verbose",
- action="store_true",
- help="Enable verbose logging"
- )
-
- parser.add_argument(
- "--dry-run",
- action="store_true",
- help="Show what would be generated without actually generating"
- )
-
- return parser.parse_args()
-
-
-def load_coordinates_from_file(filepath: str) -> List[Tuple[float, float]]:
- """
- Load coordinates from a text file.
-
- Expected format: lat,lon (one pair per line)
- Lines starting with # are treated as comments.
-
- Args:
- filepath: Path to the coordinates file
-
- Returns:
- List of (lat, lon) tuples
- """
- coordinates = []
-
- try:
- with open(filepath, 'r') as f:
- for line_num, line in enumerate(f, 1):
- line = line.strip()
-
- # Skip empty lines and comments
- if not line or line.startswith('#'):
- continue
-
- # Parse lat,lon
- try:
- parts = line.split(',')
- if len(parts) != 2:
- logger.warning(f"Line {line_num}: Invalid format (expected 'lat,lon'): {line}")
- continue
-
- lat = float(parts[0].strip())
- lon = float(parts[1].strip())
- coordinates.append((lat, lon))
-
- except ValueError as e:
- logger.warning(f"Line {line_num}: Could not parse coordinates: {line} ({e})")
- continue
-
- except FileNotFoundError:
- logger.error(f"Coordinates file not found: {filepath}")
- sys.exit(1)
- except Exception as e:
- logger.error(f"Error reading coordinates file: {e}")
- sys.exit(1)
-
- return coordinates
-
-
-def load_route_from_file(filepath: str) -> Optional[List[Tuple[float, float]]]:
- """Load AV route coordinates from file."""
- if not filepath:
- return None
-
- return load_coordinates_from_file(filepath)
-
-
-def generate_single_scenario(
- generator: IntegratedScenarioGenerator,
- lat: float,
- lon: float,
- args: argparse.Namespace,
- scenario_name: Optional[str] = None
-) -> Dict:
- """
- Generate a single scenario.
-
- Args:
- generator: The integrated scenario generator instance
- lat: Latitude of center point
- lon: Longitude of center point
- args: Command line arguments
- scenario_name: Optional name for the scenario
-
- Returns:
- Dictionary with generation results
- """
- # Load AV route if provided
- av_route = load_route_from_file(args.route) if hasattr(args, 'route') else None
-
- # Use provided name or generate one
- if not scenario_name:
- scenario_name = args.name if args.name else f"scenario_{lat:.6f}_{lon:.6f}"
-
- logger.info(f"Generating scenario: {scenario_name}")
- logger.info(f" Location: ({lat}, {lon})")
- logger.info(f" Bounding box: {args.bbox}m")
- logger.info(f" Traffic densities: {args.traffic}")
- logger.info(f" Map formats: {args.formats}")
-
- if args.dry_run:
- logger.info(" [DRY RUN] Would generate files to: %s/%s", args.output, scenario_name)
- return {"status": "dry_run", "scenario_name": scenario_name}
-
- # Generate the scenario
- start_time = time.time()
-
- try:
- result = generator.generate_from_latlon(
- lat=lat,
- lon=lon,
- bbox_size=args.bbox,
- output_dir=args.output,
- scenario_name=scenario_name,
- traffic_densities=args.traffic,
- convert_formats=args.formats,
- av_route=av_route
- )
-
- elapsed_time = time.time() - start_time
- logger.info(f" Generation completed in {elapsed_time:.2f} seconds")
-
- # Log generated files
- if result.get("status") == "success":
- logger.info(" Generated files:")
- for category, files in result.get("generated_files", {}).items():
- if isinstance(files, dict):
- logger.info(f" {category}:")
- for key, value in files.items():
- logger.info(f" {key}: {value}")
- else:
- logger.info(f" {category}: {files}")
- else:
- logger.error(f" Generation failed: {result.get('error', 'Unknown error')}")
-
- except Exception as e:
- logger.error(f" Error generating scenario: {e}")
- result = {"status": "error", "error": str(e)}
-
- return result
-
-
-def main():
- """Main function."""
- args = parse_arguments()
-
- # Set logging level
- if args.verbose:
- logging.getLogger().setLevel(logging.DEBUG)
-
- # Validate arguments
- if args.lat is not None and args.lon is None:
- logger.error("Longitude (--lon) is required when using --lat")
- sys.exit(1)
-
- # Initialize generator
- logger.info("Initializing TeraSim scenario generator...")
- try:
- generator = IntegratedScenarioGenerator(config_path=args.config)
- except Exception as e:
- logger.error(f"Failed to initialize generator: {e}")
- sys.exit(1)
-
- # Determine coordinates to process
- coordinates = []
-
- if args.lat is not None:
- # Single coordinate
- coordinates = [(args.lat, args.lon)]
-
- elif args.batch:
- # Load from file
- logger.info(f"Loading coordinates from: {args.batch}")
- coordinates = load_coordinates_from_file(args.batch)
- logger.info(f"Loaded {len(coordinates)} coordinate pairs")
-
- elif args.coordinates:
- # Parse from command line
- if len(args.coordinates) % 2 != 0:
- logger.error("Coordinates must be provided in lat,lon pairs")
- sys.exit(1)
-
- coordinates = [
- (args.coordinates[i], args.coordinates[i+1])
- for i in range(0, len(args.coordinates), 2)
- ]
- logger.info(f"Processing {len(coordinates)} coordinate pairs")
-
- if not coordinates:
- logger.error("No valid coordinates to process")
- sys.exit(1)
-
- # Generate scenarios
- logger.info("="*60)
- logger.info(f"Starting generation of {len(coordinates)} scenario(s)")
- logger.info("="*60)
-
- results = []
- success_count = 0
-
- for i, (lat, lon) in enumerate(coordinates, 1):
- logger.info(f"\n[{i}/{len(coordinates)}] Processing coordinates: ({lat}, {lon})")
-
- # Generate scenario name for batch processing
- scenario_name = None
- if len(coordinates) > 1:
- scenario_name = f"batch_{i:03d}_{lat:.6f}_{lon:.6f}"
-
- result = generate_single_scenario(generator, lat, lon, args, scenario_name)
- results.append(result)
-
- if result.get("status") == "success":
- success_count += 1
-
- # Summary
- logger.info("\n" + "="*60)
- logger.info("GENERATION SUMMARY")
- logger.info("="*60)
- logger.info(f"Total scenarios: {len(coordinates)}")
- logger.info(f"Successful: {success_count}")
- logger.info(f"Failed: {len(coordinates) - success_count}")
-
- if not args.dry_run:
- # Save summary to file
- summary_file = Path(args.output) / "generation_summary.json"
- summary_file.parent.mkdir(parents=True, exist_ok=True)
-
- summary_data = {
- "total_scenarios": len(coordinates),
- "successful": success_count,
- "failed": len(coordinates) - success_count,
- "parameters": {
- "bbox_size": args.bbox,
- "traffic_densities": args.traffic,
- "map_formats": args.formats,
- "output_directory": args.output
- },
- "results": results
- }
-
- with open(summary_file, 'w') as f:
- json.dump(summary_data, f, indent=2)
-
- logger.info(f"\nSummary saved to: {summary_file}")
-
- # Exit with appropriate code
- sys.exit(0 if success_count == len(coordinates) else 1)
-
-
-if __name__ == "__main__":
- main()
\ No newline at end of file
diff --git a/scripts/visualize_fcd.py b/scripts/visualize_fcd.py
deleted file mode 100644
index 53c57b6..0000000
--- a/scripts/visualize_fcd.py
+++ /dev/null
@@ -1,381 +0,0 @@
-#!/usr/bin/env python3
-import matplotlib.pyplot as plt
-import matplotlib.animation as animation
-import numpy as np
-from pathlib import Path
-import sys
-import yaml
-
-from terasim_vis import Net, Trajectories
-
-
-def load_config(config_path):
- """Load configuration from YAML file."""
- config_path = Path(config_path)
-
- if not config_path.exists():
- print(f"Error: Configuration file {config_path} not found.")
- sys.exit(1)
-
- try:
- with open(config_path, 'r') as f:
- config = yaml.safe_load(f)
-
- # Convert paths to Path objects
- if config.get('fcd'):
- config['fcd'] = Path(config['fcd'])
- if config.get('net'):
- config['net'] = Path(config['net'])
- if config.get('save'):
- config['save'] = Path(config['save'])
-
- # Handle output directory configuration
- if config.get('output'):
- output_config = config['output']
- if output_config.get('dir'):
- output_config['dir'] = Path(output_config['dir'])
-
- # Set defaults for missing values
- config.setdefault('start_time', 0)
- config.setdefault('end_time', None)
- config.setdefault('speed_colors', False)
- config.setdefault('lane_colors', False)
- config.setdefault('animate', False)
- config.setdefault('dpi', 150)
- config.setdefault('figsize', '12,8')
-
- return config
-
- except Exception as e:
- print(f"Error loading configuration file: {e}")
- sys.exit(1)
-
-
-class TrafficVisualizer:
- """Class for visualizing SUMO traffic simulation data."""
-
- def __init__(self, net, trajectories, config):
- """Initialize the visualizer with network, trajectories, and configuration."""
- self.net = net
- self.trajectories = trajectories
- self.config = config
- self.fig = None
- self.ax = None
- self.target_trajectory = None
-
- def setup_plot(self):
- """Set up the matplotlib figure and plot the network."""
- figsize = tuple(map(float, self.config['figsize'].split(",")))
- self.fig, self.ax = plt.subplots(figsize=figsize)
-
- # Get map styling configuration
- map_style = self.config.get('map_style', {})
- colors = map_style.get('colors', {})
-
- # Then change the colors of road elements
- lane_color = colors.get('lane_color', 'white')
- lane_marking_color = colors.get('lane_marking_color', 'black')
- # Plot the network with custom road colors
- # First plot with default settings to get the network structure
- self.net.plot(
- self.ax,
- style=map_style.get('style', 'EUR'),
- zoom_to_extents=map_style.get('zoom_to_extents', True),
- plot_stop_lines=map_style.get('plot_stop_lines', False),
- lane_kwargs=dict(color=lane_color),
- junction_kwargs=dict(color=lane_color),
- tl_phases=map_style.get('tl_phases', None)
- # lane_marking_kwargs=dict(color=lane_marking_color)
- )
-
-
- self.ax.set_aspect('equal')
-
- # Apply custom view settings if not zooming to extents
- if not map_style.get('zoom_to_extents', True):
- self._set_custom_view(map_style.get('view', {}))
-
- # Optional title and labels based on config
- if map_style.get('show_title', False):
- title = map_style.get('title', 'SUMO Traffic Simulation Visualization')
- self.ax.set_title(title)
-
- if map_style.get('show_labels', False):
- self.ax.set_xlabel("X coordinate (m)")
- self.ax.set_ylabel("Y coordinate (m)")
-
- # Hide ticks if not showing labels
- if not map_style.get('show_labels', False):
- self.ax.set_xticks([])
- self.ax.set_yticks([])
-
- # Remove margins and padding to fill the entire display area
- self.ax.margins(0) # Remove default margins around the plot
- self.ax.set_aspect('equal', adjustable='box') # Maintain aspect ratio
-
- # Adjust the subplot to remove whitespace
- plt.subplots_adjust(left=0, right=1, top=1, bottom=0)
-
- # Alternative: use tight_layout for automatic adjustment
- # plt.tight_layout(pad=0)
-
- return self.fig, self.ax
-
- def _set_custom_view(self, view_config):
- """Set custom view boundaries based on configuration."""
- width = view_config.get('width', 200)
- height = view_config.get('height', 200)
-
- if view_config.get('center_on_ego', True) and self.target_trajectory:
- # Center on ego vehicle's first position
- if hasattr(self.target_trajectory, 'x') and len(self.target_trajectory.x) > 0:
- center_x = self.target_trajectory.x[0]
- center_y = self.target_trajectory.y[0]
- else:
- # Fallback to fixed center if ego trajectory not available
- center_x = view_config.get('center_x', 0)
- center_y = view_config.get('center_y', 0)
- else:
- # Use fixed center coordinates
- center_x = view_config.get('center_x', 0)
- center_y = view_config.get('center_y', 0)
-
- # Set view limits
- half_width = width / 2
- half_height = height / 2
-
- self.ax.set_xlim(center_x - half_width, center_x + half_width)
- self.ax.set_ylim(center_y - half_height, center_y + half_height)
-
- def configure_trajectory_colors(self, ego_vehicle_id=None, adv_vehicle_id=None):
- """Configure colors and markers for all trajectories."""
- # Find the target vehicle for following
- if ego_vehicle_id:
- for trajectory in self.trajectories:
- if trajectory.id == ego_vehicle_id:
- self.target_trajectory = trajectory
- print(f"Found target vehicle (ego): {ego_vehicle_id}")
- break
- if not self.target_trajectory:
- print(f"Warning: Ego vehicle {ego_vehicle_id} not found in trajectories")
-
- # Set up color scheme and marker properties for different vehicle types
- for trajectory in self.trajectories:
- # Highlight the ego vehicle (green)
- if ego_vehicle_id and trajectory.id == ego_vehicle_id:
- trajectory.assign_colors_constant("blue") # Green for ego vehicle
- trajectory.point_plot_kwargs["color"] = "blue"
- trajectory.point_plot_kwargs["markeredgecolor"] = "black"
- trajectory.point_plot_kwargs["zorder"] = 300 # Higher z-order to show on top
- # Highlight the adversarial vehicle (red)
- elif adv_vehicle_id and trajectory.id == adv_vehicle_id:
- trajectory.assign_colors_constant("red") # Red for adversarial vehicle
- trajectory.point_plot_kwargs["color"] = "red"
- trajectory.point_plot_kwargs["markeredgecolor"] = "black"
- trajectory.point_plot_kwargs["zorder"] = 290 # High z-order but below ego
- else:
- # All other vehicles use gray color
- trajectory.assign_colors_constant("gray")
- trajectory.point_plot_kwargs["color"] = "gray"
- trajectory.point_plot_kwargs["markeredgecolor"] = "black"
- trajectory.point_plot_kwargs["zorder"] = 200
- # Set size, color for different vehicle types
- vehicle_type = getattr(trajectory, 'type', '')
- if 'ped' in vehicle_type.lower():
- trajectory.length = 1
- trajectory.width = 1
- trajectory.point_plot_kwargs["ms"] = 1
- elif 'bike' in vehicle_type.lower() or 'bicycle' in vehicle_type.lower():
- trajectory.length = 2 # Bicycles
- trajectory.width = 1
- trajectory.point_plot_kwargs["ms"] = 1
- else:
- trajectory.length = 4.8 # Regular vehicles
- trajectory.width = 2
- trajectory.point_plot_kwargs["ms"] = 2
-
- def plot_static_trajectories(self, speed_colors=False, lane_colors=False, ego_vehicle_id=None, adv_vehicle_id=None):
- """Plot all trajectories as static lines."""
- if not speed_colors and not lane_colors and (ego_vehicle_id or adv_vehicle_id):
- # Use special vehicle coloring scheme
- self.configure_trajectory_colors(ego_vehicle_id, adv_vehicle_id)
- else:
- # Use original coloring scheme for static plots
- for trajectory in self.trajectories:
- if speed_colors:
- trajectory.assign_colors_speed(cmap="viridis")
- elif lane_colors:
- trajectory.assign_colors_lane(cmap="tab10")
- else:
- # Assign colors based on vehicle type
- vehicle_type = getattr(trajectory, 'type', '')
- if 'bike' in vehicle_type.lower() or 'bicycle' in vehicle_type.lower():
- trajectory.assign_colors_constant("yellow") # Bicycles
- elif trajectory.id.startswith("VRU_"):
- trajectory.assign_colors_constant("blue") # Pedestrians
- else:
- trajectory.assign_colors_constant("red") # Regular vehicles
-
- for trajectory in self.trajectories:
- trajectory.plot(self.ax, linewidth=1.5, alpha=0.7)
-
- # Add colorbar if using speed colors
- if speed_colors and self.trajectories.mappables:
- # Use the first trajectory's mappable for the colorbar
- first_mappable = next(iter(self.trajectories.mappables.values()))
- cbar = plt.colorbar(first_mappable, ax=self.ax, shrink=0.6)
- cbar.set_label("Speed (m/s)")
-
- def create_animation(self, ego_vehicle_id=None, adv_vehicle_id=None):
- """Create animated visualization with optional camera following."""
- # Configure trajectory colors and find target vehicle
- self.configure_trajectory_colors(ego_vehicle_id, adv_vehicle_id)
-
- # Set trajectories timestep
- self.trajectories.timestep = 0.1
- print(f"Trajectories timestep: {self.trajectories.timestep}")
- print(f"Trajectories start: {self.trajectories.start}, end: {self.trajectories.end}")
-
- # Get timestep range
- timestep_range = self.trajectories.timestep_range()
-
- # Filter frames to desired time range
- start_time = self.config.get('start_time')
- end_time = self.config.get('end_time')
-
- if start_time is not None or end_time is not None:
- if start_time is None:
- start_time = self.trajectories.start
- if end_time is None:
- end_time = self.trajectories.end
- timestep_range = timestep_range[(timestep_range >= start_time) & (timestep_range <= end_time)]
-
- # Limit to max frames if desired
- max_frames = self.config.get('max_frames')
- if max_frames is not None:
- timestep_range = timestep_range[:max_frames]
-
- print(f"Creating animation with {len(timestep_range)} frames")
- print(f"Time range: {timestep_range[0]:.1f} to {timestep_range[-1]:.1f}")
-
- # Create animation function with camera following
- def animate_with_follow(frame):
- # Call original plot_points function
- self.trajectories.plot_points(frame, self.ax, True)
-
- # Follow target vehicle if specified
- if self.target_trajectory and frame in self.target_trajectory.time:
- # Get vehicle position at current frame
- frame_idx = self.target_trajectory.time.index(frame)
- vehicle_x = self.target_trajectory.x[frame_idx]
- vehicle_y = self.target_trajectory.y[frame_idx]
-
- # Set view to follow the vehicle with configured view size
- map_style = self.config.get('map_style', {})
- view_config = map_style.get('view', {})
- width = view_config.get('width', 200)
- height = view_config.get('height', 200)
-
- half_width = width / 2
- half_height = height / 2
-
- self.ax.set_xlim(vehicle_x - half_width, vehicle_x + half_width)
- self.ax.set_ylim(vehicle_y - half_height, vehicle_y + half_height)
-
- return self.ax.get_children() # Return artists for blitting
-
- if self.target_trajectory:
- # Use custom animation function for following
- anim = animation.FuncAnimation(
- self.fig, animate_with_follow,
- frames=timestep_range,
- repeat=False,
- interval=int(1000 * self.trajectories.timestep) if self.trajectories.timestep else 100,
- blit=False # Disable blitting when changing view limits
- )
- else:
- # Use original approach when not following
- anim = animation.FuncAnimation(
- self.fig, self.trajectories.plot_points,
- frames=timestep_range,
- repeat=False,
- interval=int(1000 * self.trajectories.timestep) if self.trajectories.timestep else 100,
- fargs=(self.ax, True),
- blit=True
- )
-
- # Store time_range for later use in saving
- anim.time_range = timestep_range
- return anim
-
-
-def main(path_to_config):
- """Main function."""
- config = load_config(path_to_config)
-
- print(f"Loading network from: {config['net']}")
- net = Net(str(config['net']))
-
- print(f"Loading trajectories from: {config['fcd']}")
- trajectories = Trajectories(str(config['fcd']),
- start_time=config['start_time'],
- end_time=config['end_time'],
- vehicle_only=config['vehicle_only'] if 'vehicle_only' in config else False)
- print(f"Loaded {len(trajectories.trajectories)} trajectories")
-
- # Create visualizer instance
- visualizer = TrafficVisualizer(net, trajectories, config)
- visualizer.setup_plot()
-
- output_dir = Path(config['output_dir'])
- output_dir.mkdir(parents=True, exist_ok=True)
-
- if config['animate']:
- print("Creating animation...")
- ego_vehicle_id = config.get('ego_vehicle_id')
- adv_vehicle_id = config.get('adv_vehicle_id')
- if ego_vehicle_id:
- print(f"Camera will follow ego vehicle: {ego_vehicle_id}")
- if adv_vehicle_id:
- print(f"Highlighting adversarial vehicle: {adv_vehicle_id}")
- anim = visualizer.create_animation(ego_vehicle_id, adv_vehicle_id)
-
- video_name = config.get('video_name', 'animation')
- max_frames = config.get('max_frames')
- if max_frames is not None:
- video_name = f"{video_name}_{max_frames}"
- save_path = output_dir/f"{video_name}.mp4"
-
- print(f"Saving animation to: {save_path}")
- Writer = animation.writers['ffmpeg']
- writer = Writer(fps=8, metadata=dict(artist='TeraSim-Vis'), bitrate=3600)
- print(f"Saving {len(anim.time_range)} frames at 8 fps...")
- anim.save(str(save_path), writer=writer, dpi=config['dpi'])
- print(f"Animation saved successfully!")
-
- else:
- print("Creating static visualization...")
- ego_vehicle_id = config.get('ego_vehicle_id')
- adv_vehicle_id = config.get('adv_vehicle_id')
- visualizer.plot_static_trajectories(
- config['speed_colors'], config['lane_colors'], ego_vehicle_id, adv_vehicle_id)
- save_path = output_dir/f"visualization.png"
- print(f"Saving plot to: {save_path}")
- plt.savefig(str(save_path), dpi=config['dpi'], bbox_inches='tight', pad_inches=0)
-
- print("Visualization complete!")
-
-
-if __name__ == "__main__":
- # path_to_config = "vis_configs/France_Paris_PedestrianCrossing.yaml"
- # path_to_config = "vis_configs/Germany_Rossfeld_AggressiveMerge.yaml"
- # path_to_config = "vis_configs/US_Arizona_HighwayCutin.yaml"
- # path_to_config = "vis_configs/US_AnnArbor_RoundaboutFailToYield.yaml"
- path_to_config = "vis_configs/US_SanDiego_RunRedLight.yaml"
- # path_to_config = "vis_configs/US_Chicago_UncoordinatedLeftTurn.yaml"
- # path_to_config = "vis_configs/Mcity_CyclistCrossing.yaml"
- # path_to_config = "vis_configs/Mcity_PedestrianCrossing.yaml"
- # path_to_config = "vis_configs/Mcity_UnprotectedLeftTurn.yaml"
- # path_to_config = "vis_configs/US_AnnArbor_RoundaboutFailToYield.yaml"
- # path_to_config = "vis_configs/Demo_AnnArborRoundabout.yaml"
- main(path_to_config)
\ No newline at end of file
diff --git a/scripts/visualize_xodr_region.py b/scripts/visualize_xodr_region.py
new file mode 100755
index 0000000..77a5e80
--- /dev/null
+++ b/scripts/visualize_xodr_region.py
@@ -0,0 +1,913 @@
+#!/usr/bin/env python3
+"""
+OpenDRIVE Map Region Visualizer (SUMO Coordinate Mode)
+
+This script takes SUMO x, y coordinates and visualizes the corresponding
+region in the original OpenDRIVE (.xodr) map file.
+
+The script reads the coordinate transformation (netOffset) from the SUMO
+network file to convert SUMO coordinates back to OpenDRIVE coordinates.
+
+Usage:
+ python visualize_xodr_region.py [options]
+
+Author: TeraSim Team
+License: MIT
+"""
+
+import xml.etree.ElementTree as ET
+import argparse
+import logging
+import math
+import sys
+from typing import List, Tuple, Optional
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from matplotlib.path import Path
+import numpy as np
+
+# Setup logging
+logging.basicConfig(level=logging.INFO, format='%(levelname)s: %(message)s')
+logger = logging.getLogger(__name__)
+
+# Try to import pyOpenDRIVE for enhanced geometry
+try:
+ from pyOpenDRIVE.OpenDriveMap import PyOpenDriveMap
+ PYOPENDRIVE_AVAILABLE = True
+ logger.info("pyOpenDRIVE is available - enhanced geometry processing enabled")
+except ImportError:
+ PYOPENDRIVE_AVAILABLE = False
+ logger.warning("pyOpenDRIVE not available. Using XML parsing for basic visualization.")
+
+
+class GeometryCalculator:
+ """Calculate positions along OpenDRIVE geometries"""
+
+ @staticmethod
+ def line(s: float, x: float, y: float, hdg: float, length: float) -> Tuple[float, float, float]:
+ """Calculate position and heading on a line geometry"""
+ dx = s * math.cos(hdg)
+ dy = s * math.sin(hdg)
+ return x + dx, y + dy, hdg
+
+ @staticmethod
+ def arc(s: float, x: float, y: float, hdg: float, length: float, curvature: float) -> Tuple[float, float, float]:
+ """Calculate position and heading on an arc geometry"""
+ if abs(curvature) < 1e-10:
+ return GeometryCalculator.line(s, x, y, hdg, length)
+
+ radius = 1.0 / curvature
+ # Arc center
+ cx = x - radius * math.sin(hdg)
+ cy = y + radius * math.cos(hdg)
+
+ # Angle along arc
+ angle = hdg - math.pi / 2 + s * curvature
+
+ # Point on arc
+ px = cx + radius * math.cos(angle)
+ py = cy + radius * math.sin(angle)
+
+ # Heading at this point
+ phdg = hdg + s * curvature
+
+ return px, py, phdg
+
+ @staticmethod
+ def spiral(s: float, x: float, y: float, hdg: float, length: float,
+ curv_start: float, curv_end: float) -> Tuple[float, float, float]:
+ """Calculate position and heading on a spiral (clothoid) - simplified approximation"""
+ # Simplified clothoid approximation using small segments
+ num_segments = max(10, int(length))
+ if num_segments == 0:
+ return x, y, hdg
+
+ segment_length = s / num_segments
+
+ px, py = x, y
+ current_hdg = hdg
+
+ for i in range(num_segments):
+ # Linear interpolation of curvature
+ t = i / num_segments
+ curvature = curv_start + t * (curv_end - curv_start)
+
+ # Move along current heading
+ px += segment_length * math.cos(current_hdg)
+ py += segment_length * math.sin(current_hdg)
+
+ # Update heading based on curvature
+ current_hdg += curvature * segment_length
+
+ return px, py, current_hdg
+
+ @staticmethod
+ def get_lateral_offset(x: float, y: float, hdg: float, lateral_offset: float) -> Tuple[float, float]:
+ """
+ Calculate position offset laterally from centerline
+ Positive offset = left side, Negative offset = right side
+ """
+ # Perpendicular direction (90 degrees to the left of heading)
+ perp_hdg = hdg + math.pi / 2
+ offset_x = x + lateral_offset * math.cos(perp_hdg)
+ offset_y = y + lateral_offset * math.sin(perp_hdg)
+ return offset_x, offset_y
+
+
+class XODRVisualizer:
+ """Visualize a region of an OpenDRIVE map"""
+
+ def __init__(self, xodr_file: str, sumo_net_file: Optional[str] = None):
+ """
+ Initialize visualizer with OpenDRIVE file
+
+ Args:
+ xodr_file: Path to OpenDRIVE map file
+ sumo_net_file: Optional SUMO network file for coordinate transformation
+ """
+ self.xodr_file = xodr_file
+ self.sumo_net_file = sumo_net_file
+ self.roads = []
+ self.junction_roads = [] # Roads that are part of junctions
+ self.junctions = []
+
+ # Lane-level data
+ self.lanes = [] # List of lane geometries
+ self.lane_connections = [] # List of junction lane connections
+
+ # Coordinate transformation from SUMO to OpenDRIVE
+ self.net_offset = (0.0, 0.0) # Default: no offset
+
+ # Read offset from SUMO network file if provided
+ if sumo_net_file:
+ self._read_sumo_offset()
+
+ # Parse the map
+ if PYOPENDRIVE_AVAILABLE:
+ self._parse_with_pyopendrive()
+ else:
+ self._parse_with_xml()
+
+ def _read_sumo_offset(self):
+ """Read netOffset from SUMO network file"""
+ try:
+ tree = ET.parse(self.sumo_net_file)
+ root = tree.getroot()
+
+ # Find location element with netOffset
+ location = root.find('location')
+ if location is not None:
+ net_offset_str = location.get('netOffset')
+ if net_offset_str:
+ parts = net_offset_str.split(',')
+ self.net_offset = (float(parts[0]), float(parts[1]))
+ logger.info(f"Read netOffset from SUMO file: ({self.net_offset[0]:.2f}, {self.net_offset[1]:.2f})")
+
+ # Also read boundaries for reference
+ orig_boundary = location.get('origBoundary')
+ conv_boundary = location.get('convBoundary')
+ if orig_boundary:
+ logger.info(f" Original OpenDRIVE boundary: {orig_boundary}")
+ if conv_boundary:
+ logger.info(f" Converted SUMO boundary: {conv_boundary}")
+ else:
+ logger.warning("No netOffset found in SUMO network file")
+ else:
+ logger.warning("No location element found in SUMO network file")
+ except Exception as e:
+ logger.error(f"Error reading SUMO network file: {e}")
+ logger.info("Using default offset (0, 0)")
+
+ def sumo_to_xodr(self, sumo_x: float, sumo_y: float) -> Tuple[float, float]:
+ """
+ Convert SUMO coordinates to OpenDRIVE coordinates
+
+ The conversion formula is:
+ xodr_x = sumo_x - offset_x
+ xodr_y = sumo_y - offset_y
+
+ Args:
+ sumo_x: X coordinate in SUMO system
+ sumo_y: Y coordinate in SUMO system
+
+ Returns:
+ Tuple of (xodr_x, xodr_y) in OpenDRIVE coordinate system
+ """
+ xodr_x = sumo_x - self.net_offset[0]
+ xodr_y = sumo_y - self.net_offset[1]
+ return xodr_x, xodr_y
+
+ def _parse_with_pyopendrive(self):
+ """Parse OpenDRIVE using pyOpenDRIVE library"""
+ try:
+ logger.info(f"Parsing {self.xodr_file} with pyOpenDRIVE...")
+ odr_map = PyOpenDriveMap.load_from_file(self.xodr_file)
+
+ # Extract road geometries
+ for road_id in range(odr_map.get_num_roads()):
+ road = odr_map.get_road(road_id)
+ road_data = {
+ 'id': road.get_id(),
+ 'points': self._sample_road_centerline_pyodr(road)
+ }
+ self.roads.append(road_data)
+
+ logger.info(f"Loaded {len(self.roads)} roads using pyOpenDRIVE")
+ except Exception as e:
+ logger.error(f"Error parsing with pyOpenDRIVE: {e}")
+ logger.info("Falling back to XML parsing...")
+ self._parse_with_xml()
+
+ def _sample_road_centerline_pyodr(self, road, num_samples: int = 50) -> List[Tuple[float, float]]:
+ """Sample points along road centerline using pyOpenDRIVE"""
+ points = []
+ length = road.get_length()
+
+ for i in range(num_samples + 1):
+ s = (i / num_samples) * length
+ try:
+ x, y = road.get_xy(s)
+ points.append((x, y))
+ except:
+ pass
+
+ return points
+
+ def _parse_with_xml(self):
+ """Parse OpenDRIVE using XML parsing"""
+ try:
+ logger.info(f"Parsing {self.xodr_file} with XML parser...")
+ tree = ET.parse(self.xodr_file)
+ root = tree.getroot()
+
+ # First, collect all junction connecting road IDs
+ junction_road_ids = set()
+ for junction_elem in root.findall('.//junction'):
+ for connection in junction_elem.findall('connection'):
+ connecting_road_id = connection.get('connectingRoad')
+ if connecting_road_id:
+ junction_road_ids.add(connecting_road_id)
+
+ # Parse roads
+ for road_elem in root.findall('.//road'):
+ road_id = road_elem.get('id', 'unknown')
+ junction_id = road_elem.get('junction', '-1')
+
+ # Parse plan view geometries
+ points = []
+ plan_view = road_elem.find('planView')
+ if plan_view is not None:
+ for geom in plan_view.findall('geometry'):
+ geom_points = self._parse_geometry(geom)
+ points.extend(geom_points)
+
+ if points:
+ road_data = {
+ 'id': road_id,
+ 'points': points,
+ 'junction_id': junction_id
+ }
+
+ # Separate junction roads from regular roads
+ if junction_id != '-1' or road_id in junction_road_ids:
+ self.junction_roads.append(road_data)
+ else:
+ self.roads.append(road_data)
+
+ logger.info(f"Loaded {len(self.roads)} regular roads and {len(self.junction_roads)} junction roads using XML parser")
+
+ # Parse lane-level geometries
+ self._parse_lanes_from_xml(root)
+
+ # Parse junction connections for lane links
+ self._parse_junction_connections(root)
+
+ except Exception as e:
+ logger.error(f"Error parsing XML: {e}")
+ raise
+
+ def _parse_lanes_from_xml(self, root):
+ """Parse individual lane geometries from OpenDRIVE XML"""
+ try:
+ for road_elem in root.findall('.//road'):
+ road_id = road_elem.get('id', 'unknown')
+ junction_id = road_elem.get('junction', '-1')
+
+ # Get road plan view geometries
+ plan_view = road_elem.find('planView')
+ if plan_view is None:
+ continue
+
+ geometries = []
+ for geom in plan_view.findall('geometry'):
+ geometries.append({
+ 's': float(geom.get('s', 0)),
+ 'x': float(geom.get('x', 0)),
+ 'y': float(geom.get('y', 0)),
+ 'hdg': float(geom.get('hdg', 0)),
+ 'length': float(geom.get('length', 0)),
+ 'type': self._get_geometry_type(geom),
+ 'elem': geom
+ })
+
+ if not geometries:
+ continue
+
+ # Get road length
+ road_length = float(road_elem.get('length', 0))
+
+ # Parse lanes from lane sections
+ lanes_elem = road_elem.find('lanes')
+ if lanes_elem is None:
+ continue
+
+ for lane_section in lanes_elem.findall('laneSection'):
+ s_start = float(lane_section.get('s', 0))
+
+ # Find s_end (start of next section or road end)
+ s_end = road_length
+
+ # Parse right lanes (negative IDs)
+ right_elem = lane_section.find('right')
+ if right_elem is not None:
+ for lane_elem in right_elem.findall('lane'):
+ lane_id = int(lane_elem.get('id', 0))
+ lane_type = lane_elem.get('type', 'none')
+
+ if lane_type in ['driving', 'biking', 'sidewalk']: # Only visualize drivable lanes
+ lane_data = self._create_lane_geometry(
+ road_id, lane_id, lane_type, junction_id,
+ geometries, lane_elem, s_start, s_end
+ )
+ if lane_data:
+ self.lanes.append(lane_data)
+
+ # Parse left lanes (positive IDs)
+ left_elem = lane_section.find('left')
+ if left_elem is not None:
+ for lane_elem in left_elem.findall('lane'):
+ lane_id = int(lane_elem.get('id', 0))
+ lane_type = lane_elem.get('type', 'none')
+
+ if lane_type in ['driving', 'biking', 'sidewalk']:
+ lane_data = self._create_lane_geometry(
+ road_id, lane_id, lane_type, junction_id,
+ geometries, lane_elem, s_start, s_end
+ )
+ if lane_data:
+ self.lanes.append(lane_data)
+
+ logger.info(f"Parsed {len(self.lanes)} individual lanes")
+ except Exception as e:
+ logger.error(f"Error parsing lanes: {e}")
+ # Don't raise - fall back to road-level visualization
+
+ def _get_geometry_type(self, geom_elem):
+ """Determine geometry type from element"""
+ if geom_elem.find('line') is not None:
+ return 'line'
+ elif geom_elem.find('arc') is not None:
+ return 'arc'
+ elif geom_elem.find('spiral') is not None:
+ return 'spiral'
+ return 'unknown'
+
+ def _create_lane_geometry(self, road_id, lane_id, lane_type, junction_id, geometries, lane_elem, s_start, s_end):
+ """Create lane boundary geometry"""
+ # Calculate actual lane boundaries
+ left_boundary = []
+ right_boundary = []
+
+ # Get lane width elements
+ width_elements = lane_elem.findall('width')
+ if not width_elements:
+ return None
+
+ # Sample points along the lane
+ num_samples = 20
+ total_length = s_end - s_start
+
+ for i in range(num_samples + 1):
+ # Position along lane section
+ s_local = (i / num_samples) * total_length
+ s_global = s_start + s_local
+
+ # Find which geometry this s belongs to
+ geom = self._find_geometry_at_s(geometries, s_global)
+ if not geom:
+ continue
+
+ # Calculate position and heading on centerline
+ s_in_geom = s_global - geom['s']
+ px, py, phdg = self._evaluate_geometry(geom, s_in_geom)
+
+ # Calculate lane width at this position
+ width = self._calculate_lane_width(width_elements, s_local)
+
+ # Calculate cumulative offset for this lane
+ # For right lanes (negative ID), offset is negative (to the right)
+ # For left lanes (positive ID), offset is positive (to the left)
+ offset = self._calculate_lane_offset(lane_elem, lane_id, s_local)
+
+ # Calculate lane boundaries
+ # Inner boundary (closer to centerline)
+ inner_x, inner_y = GeometryCalculator.get_lateral_offset(px, py, phdg, offset)
+ # Outer boundary (farther from centerline)
+ outer_x, outer_y = GeometryCalculator.get_lateral_offset(px, py, phdg, offset + width * (-1 if lane_id < 0 else 1))
+
+ if lane_id < 0: # Right side lanes
+ right_boundary.append((inner_x, inner_y))
+ left_boundary.append((outer_x, outer_y))
+ else: # Left side lanes
+ left_boundary.append((inner_x, inner_y))
+ right_boundary.append((outer_x, outer_y))
+
+ if not left_boundary or not right_boundary:
+ return None
+
+ return {
+ 'road_id': road_id,
+ 'lane_id': lane_id,
+ 'type': lane_type,
+ 'junction_id': junction_id,
+ 's_start': s_start,
+ 's_end': s_end,
+ 'left_boundary': left_boundary,
+ 'right_boundary': right_boundary
+ }
+
+ def _find_geometry_at_s(self, geometries, s):
+ """Find which geometry element contains position s"""
+ for geom in geometries:
+ if geom['s'] <= s < geom['s'] + geom['length']:
+ return geom
+ # Return last geometry if s is beyond end
+ return geometries[-1] if geometries else None
+
+ def _evaluate_geometry(self, geom, s):
+ """Evaluate geometry at position s"""
+ geom_elem = geom['elem']
+ x, y, hdg = geom['x'], geom['y'], geom['hdg']
+ length = geom['length']
+
+ line = geom_elem.find('line')
+ arc = geom_elem.find('arc')
+ spiral = geom_elem.find('spiral')
+
+ if line is not None:
+ return GeometryCalculator.line(s, x, y, hdg, length)
+ elif arc is not None:
+ curvature = float(arc.get('curvature', 0))
+ return GeometryCalculator.arc(s, x, y, hdg, length, curvature)
+ elif spiral is not None:
+ curv_start = float(spiral.get('curvStart', 0))
+ curv_end = float(spiral.get('curvEnd', 0))
+ return GeometryCalculator.spiral(s, x, y, hdg, length, curv_start, curv_end)
+ else:
+ return x, y, hdg
+
+ def _calculate_lane_width(self, width_elements, s):
+ """Calculate lane width at position s using polynomial"""
+ # Find applicable width element
+ applicable_width = None
+ for width_elem in width_elements:
+ s_offset = float(width_elem.get('sOffset', 0))
+ if s >= s_offset:
+ applicable_width = width_elem
+
+ if not applicable_width:
+ applicable_width = width_elements[0]
+
+ # Get polynomial coefficients
+ a = float(applicable_width.get('a', 0))
+ b = float(applicable_width.get('b', 0))
+ c = float(applicable_width.get('c', 0))
+ d = float(applicable_width.get('d', 0))
+ s_offset = float(applicable_width.get('sOffset', 0))
+
+ # Calculate width: width(ds) = a + b*ds + c*ds^2 + d*ds^3
+ ds = s - s_offset
+ width = a + b*ds + c*ds**2 + d*ds**3
+
+ return abs(width) # Ensure positive width
+
+ def _calculate_lane_offset(self, lane_elem, lane_id, s):
+ """Calculate cumulative offset from centerline to inner edge of this lane"""
+ # This is simplified - assumes we know offsets
+ # In reality, would need to sum widths of inner lanes
+
+ # For simplicity, use a basic offset based on lane ID
+ # Full implementation would traverse all inner lanes
+ base_offset = abs(lane_id) * 3.5 # Assume ~3.5m per lane
+
+ if lane_id < 0: # Right side
+ return -base_offset
+ else: # Left side
+ return base_offset
+
+ def _generate_junction_colors(self, num_junctions):
+ """Generate distinct colors for each junction using HSV color space"""
+ import colorsys
+ colors = []
+ for i in range(num_junctions):
+ # Use golden ratio for better color distribution
+ hue = (i * 0.618033988749895) % 1.0
+ # Keep saturation high and value/brightness high for visibility
+ saturation = 0.7 + (i % 3) * 0.1 # Vary between 0.7-1.0
+ value = 0.8 + (i % 2) * 0.2 # Vary between 0.8-1.0
+ rgb = colorsys.hsv_to_rgb(hue, saturation, value)
+ # Convert to hex color
+ hex_color = '#{:02x}{:02x}{:02x}'.format(
+ int(rgb[0] * 255),
+ int(rgb[1] * 255),
+ int(rgb[2] * 255)
+ )
+ colors.append(hex_color)
+ return colors
+
+ def _parse_junction_connections(self, root):
+ """Parse junction connections to understand lane links"""
+ try:
+ for junction_elem in root.findall('.//junction'):
+ junction_id = junction_elem.get('id', 'unknown')
+
+ for connection in junction_elem.findall('connection'):
+ incoming_road = connection.get('incomingRoad')
+ connecting_road = connection.get('connectingRoad')
+ contact_point = connection.get('contactPoint', 'start')
+
+ for lane_link in connection.findall('laneLink'):
+ from_lane = int(lane_link.get('from', 0))
+ to_lane = int(lane_link.get('to', 0))
+
+ self.lane_connections.append({
+ 'junction_id': junction_id,
+ 'from_road': incoming_road,
+ 'from_lane': from_lane,
+ 'to_road': connecting_road,
+ 'to_lane': to_lane,
+ 'contact_point': contact_point
+ })
+
+ logger.info(f"Parsed {len(self.lane_connections)} lane connections in junctions")
+ except Exception as e:
+ logger.error(f"Error parsing junction connections: {e}")
+
+ def _parse_geometry(self, geom_elem, num_samples: int = 20) -> List[Tuple[float, float]]:
+ """Parse a geometry element and sample points"""
+ s = float(geom_elem.get('s', 0))
+ x = float(geom_elem.get('x', 0))
+ y = float(geom_elem.get('y', 0))
+ hdg = float(geom_elem.get('hdg', 0))
+ length = float(geom_elem.get('length', 0))
+
+ points = []
+
+ # Check geometry type
+ line = geom_elem.find('line')
+ arc = geom_elem.find('arc')
+ spiral = geom_elem.find('spiral')
+
+ if line is not None:
+ # Sample line
+ for i in range(num_samples + 1):
+ t = (i / num_samples) * length
+ px, py, phdg = GeometryCalculator.line(t, x, y, hdg, length)
+ points.append((px, py))
+
+ elif arc is not None:
+ # Sample arc
+ curvature = float(arc.get('curvature', 0))
+ for i in range(num_samples + 1):
+ t = (i / num_samples) * length
+ px, py, phdg = GeometryCalculator.arc(t, x, y, hdg, length, curvature)
+ points.append((px, py))
+
+ elif spiral is not None:
+ # Sample spiral
+ curv_start = float(spiral.get('curvStart', 0))
+ curv_end = float(spiral.get('curvEnd', 0))
+ for i in range(num_samples + 1):
+ t = (i / num_samples) * length
+ px, py, phdg = GeometryCalculator.spiral(t, x, y, hdg, length, curv_start, curv_end)
+ points.append((px, py))
+ else:
+ # Unknown geometry type, just add start point
+ points.append((x, y))
+
+ return points
+
+ def visualize_region(self, center_x: float, center_y: float,
+ output_file: str = 'map_region.png',
+ size_pixels: int = 1000,
+ meters_per_pixel: float = 1.0,
+ use_sumo_coords: bool = False,
+ show_lanes: bool = False):
+ """
+ Visualize a region of the map centered on (center_x, center_y)
+
+ Args:
+ center_x: X coordinate of center point (meters, in SUMO or OpenDRIVE coords)
+ center_y: Y coordinate of center point (meters, in SUMO or OpenDRIVE coords)
+ output_file: Output PNG file path
+ size_pixels: Size of output image (width and height)
+ meters_per_pixel: Scale factor (default: 1 meter per pixel)
+ use_sumo_coords: If True, input coordinates are SUMO coords (convert to OpenDRIVE)
+ show_lanes: If True, show individual lanes and connections (experimental)
+ """
+ # Convert SUMO coordinates to OpenDRIVE if needed
+ if use_sumo_coords:
+ xodr_x, xodr_y = self.sumo_to_xodr(center_x, center_y)
+ logger.info(f"Converting SUMO coords ({center_x:.2f}, {center_y:.2f}) "
+ f"to OpenDRIVE coords ({xodr_x:.2f}, {xodr_y:.2f})")
+ center_x, center_y = xodr_x, xodr_y
+ # Calculate bounds
+ half_size_meters = (size_pixels / 2.0) * meters_per_pixel
+ x_min = center_x - half_size_meters
+ x_max = center_x + half_size_meters
+ y_min = center_y - half_size_meters
+ y_max = center_y + half_size_meters
+
+ logger.info(f"Visualizing region: ({x_min:.1f}, {y_min:.1f}) to ({x_max:.1f}, {y_max:.1f})")
+ logger.info(f"Center: ({center_x:.1f}, {center_y:.1f})")
+ logger.info(f"Size: {size_pixels}x{size_pixels} pixels ({half_size_meters*2:.1f}m x {half_size_meters*2:.1f}m)")
+
+ # Create figure
+ dpi = 100
+ fig_size = size_pixels / dpi
+ fig, ax = plt.subplots(figsize=(fig_size, fig_size), dpi=dpi)
+
+ # Set limits
+ ax.set_xlim(x_min, x_max)
+ ax.set_ylim(y_min, y_max)
+ ax.set_aspect('equal')
+
+ if show_lanes and self.lanes:
+ # Lane-level visualization
+ lanes_in_view = 0
+ junction_lanes_in_view = 0
+
+ # Generate unique colors for each junction
+ unique_junctions = set()
+ for lane in self.lanes:
+ if lane['junction_id'] != '-1':
+ unique_junctions.add(lane['junction_id'])
+
+ junction_colors = self._generate_junction_colors(len(unique_junctions))
+ junction_color_map = {jid: color for jid, color in zip(sorted(unique_junctions), junction_colors)}
+
+ # Define colors for different lane types
+ lane_colors = {
+ 'driving': '#888888', # Gray
+ 'biking': '#90EE90', # Light green
+ 'sidewalk': '#D3D3D3', # Light gray
+ }
+
+ # Track junctions in view for legend
+ junctions_in_view = set()
+
+ for lane in self.lanes:
+ # Check if lane has boundaries
+ if 'left_boundary' not in lane or 'right_boundary' not in lane:
+ continue
+
+ left_boundary = lane['left_boundary']
+ right_boundary = lane['right_boundary']
+
+ if not left_boundary or not right_boundary:
+ continue
+
+ # Check if any points are in view
+ in_view = any(
+ x_min <= x <= x_max and y_min <= y <= y_max
+ for x, y in left_boundary + right_boundary
+ )
+
+ if not in_view:
+ continue
+
+ # Determine color based on lane type and junction status
+ is_junction = lane['junction_id'] != '-1'
+ lane_type = lane['type']
+ junction_id = lane['junction_id']
+
+ if is_junction:
+ # Use unique color for this junction
+ color = junction_color_map.get(junction_id, '#6495ED')
+ junctions_in_view.add(junction_id)
+ else:
+ # Use regular lane type color
+ color = lane_colors.get(lane_type, '#888888')
+
+ # Create polygon: left boundary + reversed right boundary
+ polygon_points = left_boundary + list(reversed(right_boundary))
+
+ # Draw filled polygon
+ xs, ys = zip(*polygon_points)
+ ax.fill(xs, ys, color=color, alpha=0.7, edgecolor='black', linewidth=0.5)
+
+ lanes_in_view += 1
+ if is_junction:
+ junction_lanes_in_view += 1
+
+ logger.info(f"Drew {lanes_in_view} lanes ({junction_lanes_in_view} in junctions)")
+ logger.info(f"Unique junctions in view: {len(junctions_in_view)}")
+
+ # Store junction info for legend
+ junctions_for_legend = junctions_in_view
+ junction_color_map_for_legend = junction_color_map
+ else:
+ # Initialize empty sets if not showing lanes
+ junctions_for_legend = set()
+ junction_color_map_for_legend = {}
+
+ # Skip road-level visualization if showing lanes
+ if show_lanes and self.lanes:
+ roads_in_view = 0
+ junctions_in_view_roads = 0
+ else:
+ # Plot regular roads
+ roads_in_view = 0
+ for road in self.roads:
+ points = road['points']
+ if not points:
+ continue
+
+ # Check if any points are in the view
+ in_view = any(
+ x_min <= x <= x_max and y_min <= y <= y_max
+ for x, y in points
+ )
+
+ if in_view:
+ roads_in_view += 1
+ xs, ys = zip(*points)
+ ax.plot(xs, ys, 'k-', linewidth=2, alpha=0.7, label='Regular roads' if roads_in_view == 1 else '')
+
+ # Plot junction roads with different color
+ junctions_in_view = 0
+ for road in self.junction_roads:
+ points = road['points']
+ if not points:
+ continue
+
+ # Check if any points are in the view
+ in_view = any(
+ x_min <= x <= x_max and y_min <= y <= y_max
+ for x, y in points
+ )
+
+ if in_view:
+ junctions_in_view += 1
+ xs, ys = zip(*points)
+ ax.plot(xs, ys, 'b-', linewidth=2, alpha=0.6, label='Junction roads' if junctions_in_view == 1 else '')
+
+ # Draw lane connections if enabled
+ if show_lanes and self.lane_connections:
+ connections_drawn = 0
+ for conn in self.lane_connections:
+ # Draw a line showing lane connectivity
+ # This would require finding the actual lane endpoints
+ connections_drawn += 1
+
+ if connections_drawn > 0:
+ logger.info(f"Drew {connections_drawn} lane connection indicators")
+
+ # Mark center point
+ ax.plot(center_x, center_y, 'r+', markersize=15, markeredgewidth=2,
+ label=f'Center ({center_x:.1f}, {center_y:.1f})')
+
+ # Add grid
+ ax.grid(True, alpha=0.3, linestyle='--')
+
+ # Labels and title
+ ax.set_xlabel('X (m)', fontsize=8)
+ ax.set_ylabel('Y (m)', fontsize=8)
+
+ # Update title based on visualization mode
+ if show_lanes and self.lanes:
+ title = f'OpenDRIVE Lane-Level View\n{size_pixels}x{size_pixels}px @ {meters_per_pixel}m/px'
+ else:
+ title = f'OpenDRIVE Map Region\n{size_pixels}x{size_pixels}px @ {meters_per_pixel}m/px'
+ ax.set_title(title, fontsize=10)
+
+ # Create legend
+ if show_lanes and self.lanes and junctions_for_legend:
+ # Add junction color patches to legend
+ from matplotlib.patches import Patch
+ legend_elements = [Patch(facecolor='#888888', edgecolor='black', label='Regular lanes', alpha=0.7)]
+
+ # Add up to 5 junctions to legend to avoid clutter
+ for i, jid in enumerate(sorted(junctions_for_legend)):
+ if i >= 5: # Limit to 5 junctions in legend
+ legend_elements.append(Patch(facecolor='gray', edgecolor='black',
+ label=f'... +{len(junctions_for_legend)-5} more junctions', alpha=0.5))
+ break
+ color = junction_color_map_for_legend.get(jid, '#6495ED')
+ legend_elements.append(Patch(facecolor=color, edgecolor='black',
+ label=f'Junction {jid}', alpha=0.7))
+
+ ax.legend(handles=legend_elements, fontsize=7, loc='upper right')
+ else:
+ ax.legend(fontsize=7, loc='upper right')
+
+ # Add scale bar
+ scale_length = 50 # meters
+ scale_x = x_min + (x_max - x_min) * 0.1
+ scale_y = y_min + (y_max - y_min) * 0.05
+ ax.plot([scale_x, scale_x + scale_length], [scale_y, scale_y], 'k-', linewidth=3)
+ ax.text(scale_x + scale_length/2, scale_y + (y_max-y_min)*0.02,
+ f'{scale_length}m', ha='center', fontsize=7)
+
+ # Tight layout
+ plt.tight_layout(pad=0.5)
+
+ # Save
+ plt.savefig(output_file, dpi=dpi, bbox_inches='tight', pad_inches=0.1)
+ logger.info(f"✓ Saved visualization to {output_file}")
+ logger.info(f" Regular roads visible: {roads_in_view}")
+ logger.info(f" Junction roads visible: {junctions_in_view}")
+
+ plt.close()
+
+
+def main():
+ """Main entry point"""
+ parser = argparse.ArgumentParser(
+ description='Visualize OpenDRIVE map region using SUMO coordinates',
+ formatter_class=argparse.RawDescriptionHelpFormatter,
+ epilog="""
+Examples:
+ # Using SUMO coordinates (reads netOffset from SUMO network file)
+ python visualize_xodr_region.py map.xodr map.net.xml 100 200
+
+ # Direct OpenDRIVE coordinates (no SUMO network file)
+ python visualize_xodr_region.py map.xodr - 384.6 0
+
+ # Custom output file and size
+ python visualize_xodr_region.py map.xodr map.net.xml 100 200 --output region.png --size 400
+
+ # Different scale (0.5 meters per pixel = zoomed in, more detail)
+ python visualize_xodr_region.py map.xodr map.net.xml 100 200 --scale 0.5
+ """
+ )
+
+ parser.add_argument('xodr_file', help='OpenDRIVE map file (.xodr)')
+ parser.add_argument('sumo_net_file', help='SUMO network file (.net.xml) for coordinate conversion, or "-" for direct OpenDRIVE coords')
+ parser.add_argument('x', type=float, help='X coordinate of center point (SUMO coords if net file provided, else OpenDRIVE coords)')
+ parser.add_argument('y', type=float, help='Y coordinate of center point (SUMO coords if net file provided, else OpenDRIVE coords)')
+ parser.add_argument('--output', '-o', default='map_region.png',
+ help='Output PNG file (default: map_region.png)')
+ parser.add_argument('--size', '-s', type=int, default=1000,
+ help='Image size in pixels (width=height, default: 1000)')
+ parser.add_argument('--scale', type=float, default=1.0,
+ help='Meters per pixel (default: 1.0)')
+ parser.add_argument('--lanes', action='store_true',
+ help='Show individual lanes and junction connections (experimental)')
+ parser.add_argument('--verbose', '-v', action='store_true',
+ help='Enable verbose logging')
+
+ args = parser.parse_args()
+
+ # Set logging level
+ if args.verbose:
+ logging.getLogger().setLevel(logging.DEBUG)
+
+ # Validate inputs
+ if args.size <= 0:
+ logger.error("Size must be positive")
+ sys.exit(1)
+
+ if args.scale <= 0:
+ logger.error("Scale must be positive")
+ sys.exit(1)
+
+ # Determine if using SUMO coordinates
+ use_sumo_coords = args.sumo_net_file != '-'
+ sumo_net_file = args.sumo_net_file if use_sumo_coords else None
+
+ # Create visualizer
+ try:
+ visualizer = XODRVisualizer(args.xodr_file, sumo_net_file)
+ visualizer.visualize_region(
+ center_x=args.x,
+ center_y=args.y,
+ output_file=args.output,
+ size_pixels=args.size,
+ meters_per_pixel=args.scale,
+ use_sumo_coords=use_sumo_coords,
+ show_lanes=args.lanes
+ )
+ except FileNotFoundError as e:
+ logger.error(f"File not found: {e}")
+ sys.exit(1)
+ except Exception as e:
+ logger.error(f"Error: {e}")
+ if args.verbose:
+ import traceback
+ traceback.print_exc()
+ sys.exit(1)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/scripts/xodr_to_sumo_converter.py b/scripts/xodr_to_sumo_converter.py
index d1bd3b4..b877b5b 100644
--- a/scripts/xodr_to_sumo_converter.py
+++ b/scripts/xodr_to_sumo_converter.py
@@ -383,48 +383,61 @@ def _get_lane_width(self, lane_elem: ET.Element) -> float:
def _is_highway_merge(self, junction_id: str, internal_road_ids: List[str]) -> bool:
"""
Check if a junction represents a highway merge scenario
-
+
Highway merge criteria:
1. Exactly 2 incoming roads (main line + ramp)
2. Exactly 1 outgoing road (merged highway)
- 3. Connecting road length > 150m (sufficient merge distance)
-
+ 3. Different lane counts on incoming roads (asymmetric merge)
+
Returns:
True if this is a highway merge, False otherwise
"""
incoming_roads = set()
outgoing_roads = set()
max_connecting_length = 0
-
+ incoming_lane_counts = {}
+
for road_id in internal_road_ids:
if road_id not in self.road_map:
continue
-
+
road = self.road_map[road_id]
-
+
# Track maximum connecting road length
max_connecting_length = max(max_connecting_length, road.length)
-
+
# Collect incoming roads (predecessors of connecting roads)
if road.predecessor and road.predecessor['elementType'] == 'road':
- incoming_roads.add(road.predecessor['elementId'])
-
+ inc_road_id = road.predecessor['elementId']
+ incoming_roads.add(inc_road_id)
+ # Track lane count of this connecting road (proxy for incoming road lanes)
+ lane_count = len(road.lanes_right) + len(road.lanes_left)
+ incoming_lane_counts[inc_road_id] = lane_count
+
# Collect outgoing roads (successors of connecting roads)
if road.successor and road.successor['elementType'] == 'road':
outgoing_roads.add(road.successor['elementId'])
-
+
# Check highway merge criteria
+ # A merge is when 2 roads join into 1, regardless of length
+ # Additional check: asymmetric lane counts (e.g., 1-lane ramp merging with 3-lane highway)
+ is_asymmetric = False
+ if len(incoming_lane_counts) == 2:
+ lane_counts = list(incoming_lane_counts.values())
+ is_asymmetric = lane_counts[0] != lane_counts[1]
+
is_merge = (
- len(incoming_roads) == 2 and
- len(outgoing_roads) == 1 and
- max_connecting_length > 150
+ len(incoming_roads) == 2 and
+ len(outgoing_roads) == 1 and
+ (max_connecting_length > 50 or is_asymmetric) # Relaxed threshold: >50m OR asymmetric lanes
)
-
+
if is_merge:
logger.info(f"Junction {junction_id} identified as highway merge: "
- f"incoming={incoming_roads}, outgoing={outgoing_roads}, "
- f"max_length={max_connecting_length:.1f}m")
-
+ f"incoming={incoming_roads} (lanes={incoming_lane_counts}), "
+ f"outgoing={outgoing_roads}, max_length={max_connecting_length:.1f}m, "
+ f"asymmetric={is_asymmetric}")
+
return is_merge
def _determine_junction_type(self, junction_id: str, internal_road_ids: List[str]) -> str:
@@ -479,12 +492,12 @@ def _create_nodes(self):
# Collect all junction IDs referenced by roads
referenced_junctions = set()
junction_road_endpoints = {} # junction_id -> list of (x, y, road_id, position)
-
+
# First pass: Identify junction connections and collect connection points
for road_id, road in self.road_map.items():
if road.junction != '-1':
continue # Skip junction internal roads
-
+
# Check predecessor
if road.predecessor and road.predecessor['elementType'] == 'junction':
junction_id = road.predecessor['elementId']
@@ -494,11 +507,28 @@ def _create_nodes(self):
if junction_id not in junction_road_endpoints:
junction_road_endpoints[junction_id] = []
junction_road_endpoints[junction_id].append((start_pos[0], start_pos[1], road_id, 'start'))
+ elif road.predecessor and road.predecessor['elementType'] == 'road':
+ # Road-to-road connection - establish node mapping
+ pred_road_id = road.predecessor['elementId']
+ contact_point = road.predecessor.get('contactPoint', 'start')
+ pred_node_key = f"{pred_road_id}_{contact_point}"
+ current_node_key = f"{road_id}_start"
+
+ # Create node if it doesn't exist yet
+ if pred_node_key not in self.node_map:
+ pred_road = self.road_map.get(pred_road_id)
+ if pred_road:
+ node = self._create_road_endpoint_node(pred_road, contact_point)
+ self.node_map[pred_node_key] = node
+ self.node_map[current_node_key] = node
+ else:
+ # Node already exists, just link current road to it
+ self.node_map[current_node_key] = self.node_map[pred_node_key]
else:
# Create regular start node
start_node = self._create_road_endpoint_node(road, 'start')
self.node_map[f"{road_id}_start"] = start_node
-
+
# Check successor
if road.successor and road.successor['elementType'] == 'junction':
junction_id = road.successor['elementId']
@@ -508,6 +538,21 @@ def _create_nodes(self):
if junction_id not in junction_road_endpoints:
junction_road_endpoints[junction_id] = []
junction_road_endpoints[junction_id].append((end_pos[0], end_pos[1], road_id, 'end'))
+ elif road.successor and road.successor['elementType'] == 'road':
+ # Road-to-road connection - establish node mapping
+ succ_road_id = road.successor['elementId']
+ contact_point = road.successor.get('contactPoint', 'start')
+ succ_node_key = f"{succ_road_id}_{contact_point}"
+ current_node_key = f"{road_id}_end"
+
+ # Create node if it doesn't exist yet
+ if current_node_key not in self.node_map:
+ node = self._create_road_endpoint_node(road, 'end')
+ self.node_map[current_node_key] = node
+ self.node_map[succ_node_key] = node
+ # If current_node_key already exists, link successor to it
+ elif succ_node_key not in self.node_map:
+ self.node_map[succ_node_key] = self.node_map[current_node_key]
else:
# Create regular end node
end_node = self._create_road_endpoint_node(road, 'end')
@@ -701,58 +746,54 @@ def _calculate_junction_center_from_internal_roads(self, internal_road_ids: List
def _handle_highway_merge(self, junction_id: str, internal_road_ids: List[str]):
"""
- Handle highway merge by creating edges instead of junction
- Creates: Junction A (merge start) -> Merge Edge (4 lanes) -> Junction B (merge end)
+ Handle highway merge by creating separate entry nodes and internal edges
+ Creates: Main Entry -> Internal Edge (main) -> Merge End
+ Ramp Entry -> Internal Edge (ramp) -> Merge End
"""
logger.info(f"Handling highway merge for junction {junction_id}")
-
+
# Analyze the merge configuration
merge_info = self._analyze_merge_roads(junction_id, internal_road_ids)
if not merge_info:
logger.error(f"Failed to analyze merge roads for junction {junction_id}")
return
-
- # Create Junction A (merge start)
- junction_a = self._create_merge_start_junction(junction_id, merge_info)
- if junction_a:
- self.nodes.append(junction_a)
- self.node_map[f"merge_start_{junction_id}"] = junction_a.id
-
- # Create Junction B (merge end)
- junction_b = self._create_merge_end_junction(junction_id, merge_info)
- if junction_b:
- self.nodes.append(junction_b)
- self.node_map[f"merge_end_{junction_id}"] = junction_b.id
-
- # Mark junction as handled - but we need to be smarter about the mapping
- # Incoming roads (main and ramp) should map to merge start
- # Outgoing road should map to merge end
-
- # For roads ending at this junction (incoming), use merge start
- for road_id, road in self.road_map.items():
- if road.successor and road.successor.get('elementId') == junction_id:
- # This road ends at the junction - it's an incoming road
- # It should connect to the merge start node
- pass # Will be handled by the updated _get_road_to_node logic
- if road.predecessor and road.predecessor.get('elementId') == junction_id:
- # This road starts from the junction - it's an outgoing road
- # It should connect to the merge end node
- pass # Will be handled by the updated _get_road_from_node logic
-
- # Store a special marker for this junction
+
+ # Create separate entry nodes for main road and ramp
+ main_entry = self._create_main_entry_node(junction_id, merge_info)
+ if main_entry:
+ self.nodes.append(main_entry)
+ self.node_map[f"main_entry_{junction_id}"] = main_entry.id
+ logger.info(f"Created main entry node: {main_entry.id}")
+
+ ramp_entry = self._create_ramp_entry_node(junction_id, merge_info)
+ if ramp_entry:
+ self.nodes.append(ramp_entry)
+ self.node_map[f"ramp_entry_{junction_id}"] = ramp_entry.id
+ logger.info(f"Created ramp entry node: {ramp_entry.id}")
+
+ # Create merge end junction (where both internal edges converge)
+ merge_end = self._create_merge_end_junction(junction_id, merge_info)
+ if merge_end:
+ self.nodes.append(merge_end)
+ self.node_map[f"merge_end_{junction_id}"] = merge_end.id
+ logger.info(f"Created merge end node: {merge_end.id}")
+
+ # Store junction information
self.node_map[junction_id] = f"merge_zone_{junction_id}"
-
- # Also store specific mappings for incoming and outgoing
+
+ # Store detailed merge information
self.highway_merges = getattr(self, 'highway_merges', {})
self.highway_merges[junction_id] = {
- 'start_node': junction_a.id if junction_a else None,
- 'end_node': junction_b.id if junction_b else None,
+ 'main_entry_node': main_entry.id if main_entry else None,
+ 'ramp_entry_node': ramp_entry.id if ramp_entry else None,
+ 'merge_end_node': merge_end.id if merge_end else None,
'merge_info': merge_info
}
-
- logger.info(f"Created merge zone for junction {junction_id}: "
- f"{junction_a.id if junction_a else 'None'} -> merge_zone_{junction_id} -> "
- f"{junction_b.id if junction_b else 'None'}")
+
+ logger.info(f"Created merge structure for junction {junction_id}: "
+ f"Main({main_entry.id if main_entry else 'None'}) + "
+ f"Ramp({ramp_entry.id if ramp_entry else 'None'}) -> "
+ f"End({merge_end.id if merge_end else 'None'})")
def _analyze_merge_roads(self, junction_id: str, internal_road_ids: List[str]) -> Optional[dict]:
"""
@@ -820,8 +861,10 @@ def _analyze_merge_roads(self, junction_id: str, internal_road_ids: List[str]) -
def _create_merge_start_junction(self, junction_id: str, merge_info: dict) -> Optional[PlainNode]:
"""Create the junction node at the merge start (where main road and ramp meet)"""
+ # This method is deprecated - we now create separate entry nodes for main and ramp
+ # Keeping for backward compatibility, but it won't be used
main_road = merge_info['main_road']
-
+
# Use main road's end position as junction location
if main_road.successor and main_road.successor['elementType'] == 'junction':
end_pos = self._calculate_road_end(main_road)
@@ -832,9 +875,45 @@ def _create_merge_start_junction(self, junction_id: str, merge_info: dict) -> Op
y=end_pos[1],
type="priority"
)
-
+
logger.warning(f"Could not determine merge start position for junction {junction_id}")
return None
+
+ def _create_main_entry_node(self, junction_id: str, merge_info: dict) -> Optional[PlainNode]:
+ """Create the entry node for the main road"""
+ main_road = merge_info['main_road']
+
+ # Use main road's end position
+ if main_road.successor and main_road.successor['elementType'] == 'junction':
+ end_pos = self._calculate_road_end(main_road)
+ if end_pos:
+ return PlainNode(
+ id=f"j_main_entry_{junction_id}",
+ x=end_pos[0],
+ y=end_pos[1],
+ type="priority"
+ )
+
+ logger.warning(f"Could not determine main entry position for junction {junction_id}")
+ return None
+
+ def _create_ramp_entry_node(self, junction_id: str, merge_info: dict) -> Optional[PlainNode]:
+ """Create the entry node for the ramp"""
+ ramp_road = merge_info['ramp_road']
+
+ # Use ramp road's end position
+ if ramp_road.successor and ramp_road.successor['elementType'] == 'junction':
+ end_pos = self._calculate_road_end(ramp_road)
+ if end_pos:
+ return PlainNode(
+ id=f"j_ramp_entry_{junction_id}",
+ x=end_pos[0],
+ y=end_pos[1],
+ type="priority"
+ )
+
+ logger.warning(f"Could not determine ramp entry position for junction {junction_id}")
+ return None
def _create_merge_end_junction(self, junction_id: str, merge_info: dict) -> Optional[PlainNode]:
"""Create the junction node at the merge end (where merge zone meets outgoing road)"""
@@ -1118,12 +1197,12 @@ def _get_road_from_node(self, road: OpenDriveRoad) -> Optional[str]:
if road.predecessor['elementType'] == 'junction':
# Road starts from a junction - elementId is the junction ID directly
junction_id = road.predecessor['elementId']
-
+
# Check if this is a highway merge junction
if hasattr(self, 'highway_merges') and junction_id in self.highway_merges:
# This road is outgoing from a merge - use the merge end node
- return self.highway_merges[junction_id]['end_node']
-
+ return self.highway_merges[junction_id]['merge_end_node']
+
# Check if this junction has been converted to merge nodes
if junction_id in self.node_map:
mapped_node = self.node_map[junction_id]
@@ -1138,21 +1217,33 @@ def _get_road_from_node(self, road: OpenDriveRoad) -> Optional[str]:
# Road connects to another road - find the shared connection point
pred_road_id = road.predecessor['elementId']
contact_point = road.predecessor.get('contactPoint', 'start')
- if contact_point == 'end':
- shared_node_key = f"{pred_road_id}_end"
+
+ # Determine the canonical node key - use the predecessor road's contact point as primary
+ pred_node_key = f"{pred_road_id}_{contact_point}"
+ current_node_key = f"{road.id}_start"
+
+ # Check if either node already exists
+ if pred_node_key in self.node_map:
+ # Predecessor's node already exists - use it and link current to it
+ shared_node = self.node_map[pred_node_key]
+ if current_node_key not in self.node_map:
+ self.node_map[current_node_key] = shared_node
+ return shared_node
+ elif current_node_key in self.node_map:
+ # Current road's start node already exists - use it and link predecessor to it
+ shared_node = self.node_map[current_node_key]
+ self.node_map[pred_node_key] = shared_node
+ return shared_node
else:
- shared_node_key = f"{pred_road_id}_start"
-
- # Use the shared node from the predecessor road
- shared_node = self.node_map.get(shared_node_key)
- if not shared_node:
- # If the shared node doesn't exist, create it based on the predecessor road
+ # Neither exists - create node at predecessor road's contact point
pred_road = self.road_map.get(pred_road_id)
if pred_road:
- position = 'end' if contact_point == 'end' else 'start'
+ position = contact_point
shared_node = self._create_road_endpoint_node(pred_road, position)
- self.node_map[shared_node_key] = shared_node
- return shared_node
+ self.node_map[pred_node_key] = shared_node
+ self.node_map[current_node_key] = shared_node
+ return shared_node
+ return None
# Road has no predecessor - use stored endpoint node (ensure it exists and is unique)
node_key = f"{road.id}_start"
if node_key not in self.node_map:
@@ -1163,16 +1254,35 @@ def _get_road_from_node(self, road: OpenDriveRoad) -> Optional[str]:
def _get_road_to_node(self, road: OpenDriveRoad) -> Optional[str]:
"""Get the to node for a road"""
+ # First check if this road's end node was already created by another road's predecessor link
+ # This handles cases where road A claims to end at a junction, but road B claims road A is its predecessor
+ current_node_key = f"{road.id}_end"
+ if current_node_key in self.node_map:
+ # Already mapped - use existing node (likely from another road's predecessor)
+ return self.node_map[current_node_key]
+
if road.successor:
if road.successor['elementType'] == 'junction':
# Road ends at a junction - elementId is the junction ID directly
junction_id = road.successor['elementId']
-
+
# Check if this is a highway merge junction
if hasattr(self, 'highway_merges') and junction_id in self.highway_merges:
- # This road is incoming to a merge - use the merge start node
- return self.highway_merges[junction_id]['start_node']
-
+ # This road is incoming to a merge - determine which entry node to use
+ merge_data = self.highway_merges[junction_id]
+ merge_info = merge_data['merge_info']
+ main_road = merge_info.get('main_road')
+ ramp_road = merge_info.get('ramp_road')
+
+ # Check if this road is the main road or ramp
+ if main_road and road.id == main_road.id:
+ return merge_data['main_entry_node']
+ elif ramp_road and road.id == ramp_road.id:
+ return merge_data['ramp_entry_node']
+ else:
+ logger.warning(f"Road {road.id} ends at merge junction {junction_id} but is neither main nor ramp")
+ return merge_data.get('main_entry_node', f"junction_{junction_id}")
+
# Check if this junction has been converted to merge nodes
if junction_id in self.node_map:
mapped_node = self.node_map[junction_id]
@@ -1187,21 +1297,29 @@ def _get_road_to_node(self, road: OpenDriveRoad) -> Optional[str]:
# Road connects to another road - find the shared connection point
succ_road_id = road.successor['elementId']
contact_point = road.successor.get('contactPoint', 'start')
- if contact_point == 'end':
- shared_node_key = f"{succ_road_id}_end"
+
+ # Determine the canonical node key - use the current road's end as primary
+ current_node_key = f"{road.id}_end"
+ succ_node_key = f"{succ_road_id}_{contact_point}"
+
+ # Check if either node already exists
+ if current_node_key in self.node_map:
+ # Current road's end node already exists - use it and link successor to it
+ shared_node = self.node_map[current_node_key]
+ if succ_node_key not in self.node_map:
+ self.node_map[succ_node_key] = shared_node
+ return shared_node
+ elif succ_node_key in self.node_map:
+ # Successor's node already exists - use it and link current to it
+ shared_node = self.node_map[succ_node_key]
+ self.node_map[current_node_key] = shared_node
+ return shared_node
else:
- shared_node_key = f"{succ_road_id}_start"
-
- # Use the shared node from the successor road
- shared_node = self.node_map.get(shared_node_key)
- if not shared_node:
- # If the shared node doesn't exist, create it based on the successor road
- succ_road = self.road_map.get(succ_road_id)
- if succ_road:
- position = 'end' if contact_point == 'end' else 'start'
- shared_node = self._create_road_endpoint_node(succ_road, position)
- self.node_map[shared_node_key] = shared_node
- return shared_node
+ # Neither exists - create node at current road's end
+ shared_node = self._create_road_endpoint_node(road, 'end')
+ self.node_map[current_node_key] = shared_node
+ self.node_map[succ_node_key] = shared_node
+ return shared_node
# Road has no successor - use stored endpoint node (ensure it exists and is unique)
node_key = f"{road.id}_end"
if node_key not in self.node_map:
@@ -1395,47 +1513,33 @@ def _create_edges(self):
self._create_merge_edges()
def _create_merge_edges(self):
- """Create edges for highway merge zones"""
+ """Create edges for highway merge zones - now creates individual edges for each internal connecting road"""
for junction_id, internal_road_ids in self.junction_roads.items():
# Only process highway merges
if not self._is_highway_merge(junction_id, internal_road_ids):
continue
-
- logger.info(f"Creating merge edge for highway merge junction {junction_id}")
-
+
+ logger.info(f"Creating internal lane edges for highway merge junction {junction_id}")
+
# Get merge configuration
merge_info = self._analyze_merge_roads(junction_id, internal_road_ids)
if not merge_info:
- logger.error(f"Failed to create merge edge for junction {junction_id}")
+ logger.error(f"Failed to create merge edges for junction {junction_id}")
continue
-
- # Find the main connecting road (the one from main road, not ramp)
- # We should use the connecting road that comes from the main road (higher lane count)
- main_connecting = None
- main_road = merge_info.get('main_road')
-
- # Try to find the connecting road that follows the main road
- for road_id, road in merge_info['connecting_roads'].items():
- # Check if this connecting road comes from the main road
- if road.predecessor and main_road:
- pred_id = road.predecessor.get('elementId')
- if pred_id == main_road.id:
- main_connecting = road
- logger.debug(f"Selected connecting road {road_id} from main road {main_road.id}")
- break
-
- if not main_connecting:
- logger.error(f"No connecting road found for merge zone {junction_id}")
- continue
-
- # Create 4-lane merge edge
- merge_edge = self._build_merge_edge(junction_id, main_connecting, merge_info)
- if merge_edge:
- self.edges.append(merge_edge)
- logger.info(f"Created merge edge {merge_edge.id} with {merge_edge.num_lanes} lanes")
-
- # # Create lane mappings for junction internal roads to merge zone
- # self._create_merge_zone_lane_mappings(junction_id, merge_info)
+
+ # Create individual edges for each internal connecting road (lane)
+ connecting_roads = merge_info.get('connecting_roads', {})
+ edges_created = 0
+
+ for road_id, connecting_road in connecting_roads.items():
+ # Create an edge for this internal connecting road
+ internal_edge = self._build_internal_lane_edge(junction_id, connecting_road, merge_info)
+ if internal_edge:
+ self.edges.append(internal_edge)
+ edges_created += 1
+ logger.info(f"Created internal edge {internal_edge.id} with {internal_edge.num_lanes} lane(s)")
+
+ logger.info(f"Created {edges_created} internal lane edges for junction {junction_id}")
def _build_merge_edge(self, junction_id: str, main_connecting: OpenDriveRoad, merge_info: dict) -> Optional[PlainEdge]:
"""Build a 4-lane merge edge from connecting road geometry"""
@@ -1559,7 +1663,98 @@ def trim_shape(points, trim_dist):
shape=shape_points if len(shape_points) >= 2 else None,
lane_data=lane_data
)
-
+
+ def _build_internal_lane_edge(self, junction_id: str, connecting_road: OpenDriveRoad, merge_info: dict) -> Optional[PlainEdge]:
+ """Build an edge for an internal connecting road (lane) within a junction"""
+
+ # Determine which incoming road this connecting road comes from
+ predecessor_id = connecting_road.predecessor.get('elementId') if connecting_road.predecessor else None
+ successor_id = connecting_road.successor.get('elementId') if connecting_road.successor else None
+
+ main_road = merge_info.get('main_road')
+ ramp_road = merge_info.get('ramp_road')
+ outgoing_road = merge_info.get('outgoing_road')
+
+ # Determine the from node based on which road this connecting road comes from
+ from_node = None
+ if predecessor_id:
+ if main_road and predecessor_id == main_road.id:
+ # This connecting road comes from the main road
+ from_node = f"j_main_entry_{junction_id}"
+ logger.debug(f"Connecting road {connecting_road.id} starts from main entry")
+ elif ramp_road and predecessor_id == ramp_road.id:
+ # This connecting road comes from the ramp
+ from_node = f"j_ramp_entry_{junction_id}"
+ logger.debug(f"Connecting road {connecting_road.id} starts from ramp entry")
+ else:
+ logger.warning(f"Connecting road {connecting_road.id} has unexpected predecessor {predecessor_id}")
+
+ # Determine the to node - all internal edges end at the merge end
+ to_node = None
+ if successor_id:
+ if outgoing_road and successor_id == outgoing_road.id:
+ to_node = f"j_merge_end_{junction_id}"
+ logger.debug(f"Connecting road {connecting_road.id} ends at merge end")
+ else:
+ logger.warning(f"Connecting road {connecting_road.id} has unexpected successor {successor_id}")
+ to_node = f"j_merge_end_{junction_id}"
+
+ # Fallback if nodes not determined
+ if not from_node:
+ logger.warning(f"Could not determine from_node for connecting road {connecting_road.id}, using main entry")
+ from_node = f"j_main_entry_{junction_id}"
+ if not to_node:
+ logger.warning(f"Could not determine to_node for connecting road {connecting_road.id}, using merge end")
+ to_node = f"j_merge_end_{junction_id}"
+
+ # Check if nodes exist
+ if from_node not in [n.id for n in self.nodes] or to_node not in [n.id for n in self.nodes]:
+ logger.error(f"Nodes not found for connecting road {connecting_road.id} in junction {junction_id}: from={from_node}, to={to_node}")
+ return None
+
+ # Get geometry from connecting road
+ if self.use_pyopendrive and connecting_road.id in self.py_roads:
+ shape_points = self._get_road_centerline_pyopendrive(connecting_road.id, eps=0.5)
+ else:
+ shape_points = self._generate_road_shape(connecting_road)
+
+ # Get lane information from the connecting road
+ num_lanes = len(connecting_road.lanes_right) if connecting_road.lanes_right else 1
+
+ # Prepare lane data
+ lane_data = []
+ for i, lane in enumerate(connecting_road.lanes_right):
+ lane_dict = {
+ 'width': lane.get('width', 3.66),
+ }
+ lane_type = self._decode_if_bytes(lane.get('type', 'driving'))
+ if lane_type == 'shoulder':
+ lane_dict['type'] = 'shoulder'
+ lane_dict['disallow'] = 'all'
+ lane_data.append(lane_dict)
+
+ # Create lane mapping
+ opendrive_lane_id = lane['id']
+ edge_id = f"internal_{junction_id}_{connecting_road.id}"
+ mapping_key = (connecting_road.id, opendrive_lane_id, 'forward')
+ self.lane_mapping[mapping_key] = (edge_id, i)
+
+ # Get speed from the connecting road or use default
+ speed = connecting_road.speed_limit if hasattr(connecting_road, 'speed_limit') else 13.89
+
+ # Create the internal edge
+ return PlainEdge(
+ id=f"internal_{junction_id}_{connecting_road.id}",
+ from_node=from_node,
+ to_node=to_node,
+ num_lanes=num_lanes,
+ speed=speed,
+ name=f"Internal lane {connecting_road.id} in junction {junction_id}",
+ type="highway_merge_internal",
+ shape=shape_points if shape_points and len(shape_points) >= 2 else None,
+ lane_data=lane_data if lane_data else None
+ )
+
def _create_connections(self):
"""Create Plain XML connections using connecting road geometry as via points"""
total_connections = 0
@@ -1811,125 +2006,144 @@ def _trace_lane_successor(self, road: OpenDriveRoad, lane_id: int) -> Optional[i
return lane_id
def _create_merge_connections(self, junction_id: str):
- """Create connections for highway merge zones using OpenDRIVE definitions"""
+ """Create connections for highway merge zones using individual internal edges"""
logger.info(f"Creating merge connections for junction {junction_id}")
-
+
# Get merge configuration
if junction_id not in self.junction_roads:
logger.error(f"No junction roads found for merge {junction_id}")
return
-
+
merge_info = self._analyze_merge_roads(junction_id, self.junction_roads[junction_id])
if not merge_info:
logger.error(f"Failed to analyze merge for connections {junction_id}")
return
-
+
main_road = merge_info['main_road']
ramp_road = merge_info['ramp_road']
outgoing_road = merge_info['outgoing_road']
-
+
# Build connection chains from OpenDRIVE data
connection_chains = self._build_junction_connection_chains(junction_id)
-
- # Create merge zone edge reference
- merge_edge = f"merge_zone_{junction_id}"
-
- # Process connections using established lane mappings
+
+ # Create connections: incoming roads -> internal edges -> outgoing road
for incoming_road_id, chains in connection_chains.items():
# Get the incoming edge
incoming_edge_id = f"{incoming_road_id}.0"
incoming_edge_obj = next((e for e in self.edges if e.id == incoming_edge_id), None)
-
+
if not incoming_edge_obj:
logger.warning(f"Incoming edge {incoming_edge_id} not found for merge connections")
continue
-
+
for chain in chains:
from_lane_id = chain['from_lane']
connecting_road_id = chain['connecting_road']
connecting_lane_id = chain['connecting_lane']
-
- # Get lane mappings using the established system
- # 1. From incoming road to connecting road (via normal mapping)
+ outgoing_road_id = chain['outgoing_road']
+ outgoing_lane_id = chain['final_lane']
+
+ # Get the internal edge for this connecting road
+ internal_edge_id = f"internal_{junction_id}_{connecting_road_id}"
+ internal_edge_obj = next((e for e in self.edges if e.id == internal_edge_id), None)
+
+ if not internal_edge_obj:
+ logger.warning(f"Internal edge {internal_edge_id} not found for connecting road {connecting_road_id}")
+ continue
+
+ # Get lane mappings
+ # 1. From incoming road lane to SUMO lane
from_mapping = self._get_sumo_lane_index(incoming_road_id, from_lane_id, 'forward')
-
- # 2. From connecting road to merge zone (via our new mapping)
- to_mapping = self._get_sumo_lane_index(connecting_road_id, connecting_lane_id, 'forward')
-
+
+ # 2. From connecting road lane to internal edge lane
+ internal_mapping = self._get_sumo_lane_index(connecting_road_id, connecting_lane_id, 'forward')
+
+ # 3. From outgoing road lane to SUMO lane
+ to_mapping = self._get_sumo_lane_index(outgoing_road_id, outgoing_lane_id, 'forward')
+
if not from_mapping:
logger.error(f"No lane mapping found for incoming road {incoming_road_id} lane {from_lane_id}")
continue
-
- if not to_mapping:
+
+ if not internal_mapping:
logger.error(f"No lane mapping found for connecting road {connecting_road_id} lane {connecting_lane_id}")
continue
-
+
+ if not to_mapping:
+ logger.error(f"No lane mapping found for outgoing road {outgoing_road_id} lane {outgoing_lane_id}")
+ continue
+
from_edge, from_lane_idx = from_mapping
+ internal_edge, internal_lane_idx = internal_mapping
to_edge, to_lane_idx = to_mapping
-
- # Verify the to_edge is our merge zone
- if to_edge != merge_edge:
- logger.error(f"Expected merge zone edge {merge_edge}, but got {to_edge}")
- continue
-
- # Determine connection direction
+
+ # Determine connection direction and state
incoming_road = self.road_map.get(incoming_road_id)
- main_road = merge_info['main_road']
-
-
- # Create the connection
+ is_from_main = incoming_road and incoming_road.id == main_road.id
+
+ # Connection 1: incoming road -> internal edge
self.connections.append(PlainConnection(
from_edge=from_edge,
- to_edge=to_edge,
+ to_edge=internal_edge,
from_lane=from_lane_idx,
+ to_lane=internal_lane_idx,
+ ))
+ logger.debug(f"Connected incoming to internal: {from_edge}:{from_lane_idx} -> {internal_edge}:{internal_lane_idx}")
+
+ # Connection 2: internal edge -> outgoing road
+ if is_from_main:
+ direction = 's' # straight
+ state = 'M' # major
+ else:
+ # Ramp lane connections
+ direction = 'r' # right merge
+ state = 'm' # minor
+
+ # Only override for the rightmost ramp lane (lane 0 of internal edge)
+ # Multi-lane ramps: lane 0 → rightmost outgoing, lane 1 → lane 1, etc.
+ if internal_lane_idx == 0:
+ # This is the rightmost ramp lane - ensure it connects to rightmost outgoing lane
+ outgoing_edge_obj = next((e for e in self.edges if e.id == to_edge), None)
+ if outgoing_edge_obj:
+ # Check if there are any shoulder lanes we should skip
+ rightmost_driving_lane = 0
+ if hasattr(outgoing_edge_obj, 'lane_data') and outgoing_edge_obj.lane_data:
+ # Find the rightmost driving lane (skip shoulders)
+ driving_lanes = [i for i, lane in enumerate(outgoing_edge_obj.lane_data)
+ if lane.get('type', 'driving') != 'shoulder']
+ if driving_lanes:
+ rightmost_driving_lane = min(driving_lanes)
+
+ if to_lane_idx != rightmost_driving_lane:
+ logger.info(f"Ramp override: junction {junction_id}, internal {internal_edge} lane {internal_lane_idx} -> {to_edge} lane {to_lane_idx} changed to lane {rightmost_driving_lane} (rightmost)")
+ to_lane_idx = rightmost_driving_lane
+ else:
+ # For other ramp lanes, ensure proper lane offset from rightmost
+ # internal_lane_idx 1 should go to rightmost + 1, etc.
+ outgoing_edge_obj = next((e for e in self.edges if e.id == to_edge), None)
+ if outgoing_edge_obj:
+ rightmost_driving_lane = 0
+ if hasattr(outgoing_edge_obj, 'lane_data') and outgoing_edge_obj.lane_data:
+ driving_lanes = [i for i, lane in enumerate(outgoing_edge_obj.lane_data)
+ if lane.get('type', 'driving') != 'shoulder']
+ if driving_lanes:
+ rightmost_driving_lane = min(driving_lanes)
+
+ expected_lane = rightmost_driving_lane + internal_lane_idx
+ if to_lane_idx != expected_lane:
+ logger.info(f"Ramp lane offset correction: junction {junction_id}, internal {internal_edge} lane {internal_lane_idx} -> {to_edge} lane {to_lane_idx} changed to lane {expected_lane}")
+ to_lane_idx = expected_lane
+
+ self.connections.append(PlainConnection(
+ from_edge=internal_edge,
+ to_edge=to_edge,
+ from_lane=internal_lane_idx,
to_lane=to_lane_idx,
- # dir=direction,
- # state=state
+ dir=direction,
+ state=state
))
- logger.debug(f"Connected via mapping: {from_edge}:{from_lane_idx} -> {to_edge}:{to_lane_idx} "
- f"(Road {incoming_road_id} lane {from_lane_id} -> Road {connecting_road_id} lane {connecting_lane_id})")
-
- # Create connections for Junction B (merge end)
- # Merge zone to outgoing road (4 lanes to 3 lanes)
- if outgoing_road:
- outgoing_edge = f"{outgoing_road.id}.0"
- outgoing_edge_obj = next((e for e in self.edges if e.id == outgoing_edge), None)
-
- if outgoing_edge_obj:
- for chain in chains:
-
- connecting_road_id = chain['connecting_road']
- connecting_lane_id = chain['connecting_lane']
- outgoing_road_id = chain['outgoing_road']
- outgoing_lane_id = chain['final_lane']
- from_mapping = self._get_sumo_lane_index(connecting_road_id, connecting_lane_id, 'forward')
- to_mapping = self._get_sumo_lane_index(outgoing_road_id, outgoing_lane_id, 'forward')
-
- if incoming_road and incoming_road.id == main_road.id:
- # Main road - straight
- direction = 's'
- state = 'M'
- else:
- # Ramp - right merge
- direction = 'r'
- state = 'm'
-
- if from_mapping is None or to_mapping is None:
- logger.error(f"Missing lane mapping for merge end: from {connecting_road_id} lane {connecting_lane_id} or to {outgoing_road_id} lane {outgoing_lane_id}")
- continue
-
- from_edge, from_lane_idx = from_mapping
- to_edge, to_lane_idx = to_mapping
- self.connections.append(PlainConnection(
- from_edge=from_edge,
- to_edge=to_edge,
- from_lane=from_lane_idx, # From lanes 1,2,3
- to_lane=to_lane_idx, # To lanes 0,1,2
- dir=direction,
- state=state
- ))
- logger.debug(f"Connected merge to outgoing: {from_edge}:{from_lane_idx} -> {to_edge}:{to_lane_idx}")
-
+ logger.debug(f"Connected internal to outgoing: {internal_edge}:{internal_lane_idx} -> {to_edge}:{to_lane_idx} (dir={direction})")
+
logger.info(f"Created merge connections for junction {junction_id}")
def _get_outgoing_road_from_connecting(self, connecting_road: OpenDriveRoad, contact_point: str) -> Optional[str]:
diff --git a/setup_environment.sh b/setup_environment.sh
index bd4174c..c913042 100755
--- a/setup_environment.sh
+++ b/setup_environment.sh
@@ -1,3 +1,16 @@
+check_gcc_gpp() {
+ log_info "Checking for gcc and g++ compilers..."
+ if ! check_command gcc; then
+ log_error "gcc compiler not found. Please install gcc before proceeding."
+ exit 1
+ fi
+ if ! check_command g++; then
+ log_error "g++ compiler not found. Please install g++ before proceeding."
+ exit 1
+ fi
+ log_info "gcc version: $(gcc --version | head -n1)"
+ log_info "g++ version: $(g++ --version | head -n1)"
+}
#!/bin/bash
# TeraSim Monorepo Environment Setup Script (improved from original version)
@@ -32,11 +45,11 @@ check_python() {
if check_command python3; then
PYTHON_VERSION=$(python3 --version | cut -d' ' -f2)
log_info "Python version: $PYTHON_VERSION"
-
+
# Check if Python version meets requirements
PYTHON_MAJOR=$(echo $PYTHON_VERSION | cut -d. -f1)
PYTHON_MINOR=$(echo $PYTHON_VERSION | cut -d. -f2)
-
+
if [ "$PYTHON_MAJOR" -lt 3 ] || ([ "$PYTHON_MAJOR" -eq 3 ] && [ "$PYTHON_MINOR" -lt 10 ]); then
log_error "Python 3.10+ required, found $PYTHON_VERSION"
exit 1
@@ -47,20 +60,6 @@ check_python() {
fi
}
-check_gcc_gpp() {
- log_info "Checking for gcc and g++ compilers..."
- if ! check_command gcc; then
- log_error "gcc compiler not found. Please install gcc before proceeding."
- exit 1
- fi
- if ! check_command g++; then
- log_error "g++ compiler not found. Please install g++ before proceeding."
- exit 1
- fi
- log_info "gcc version: $(gcc --version | head -n1)"
- log_info "g++ version: $(g++ --version | head -n1)"
-}
-
check_redis() {
log_info "Checking Redis service..."
@@ -125,7 +124,6 @@ setup_monorepo() {
pip install -e packages/terasim-envgen
pip install -e packages/terasim-datazoo
pip install -e packages/terasim-vis
- pip install -e packages/terasim-cosmos
# Install development dependencies
pip install "pytest>=7.4.0" "pytest-cov>=4.1.0" "black>=23.7.0" "ruff>=0.1.0" "mypy>=1.5.1" "isort>=5.12.0"
@@ -162,12 +160,6 @@ try:
except ImportError:
print('⚠️ TeraSim Visualization not available (optional)')
-try:
- import terasim_cosmos
- print('✅ TeraSim Cosmos imported successfully')
-except ImportError:
- print('⚠️ TeraSim Cosmos not available (optional)')
-
print(f'TeraSim version: 0.2.0')
"
}
@@ -199,9 +191,6 @@ setup_sumo_tools() {
log_info "✅ SUMO tools updated"
fi
- # Export SUMO_HOME for current session
- export SUMO_HOME="${SUMO_HOME}"
-
# Save SUMO_HOME path for reference
echo "SUMO_HOME=${SUMO_HOME}" > "${DEPS_DIR}/.sumo_home"
@@ -215,89 +204,6 @@ create_output_directories() {
log_info "Output directories created"
}
-setup_environment_variables() {
- log_info "Setting up environment variables..."
-
- # Get SUMO_HOME from the saved file
- DEPS_DIR="${HOME}/.terasim/deps"
- if [ -f "${DEPS_DIR}/.sumo_home" ]; then
- source "${DEPS_DIR}/.sumo_home"
- log_info "📍 Found SUMO_HOME: ${SUMO_HOME}"
- else
- log_warning "SUMO_HOME not found. Please run SUMO setup first."
- return 1
- fi
-
- # Ask user if they want to add environment variables to shell config
- echo
- log_info "Environment variable setup options:"
- echo "1. Add to ~/.bashrc (recommended for bash users)"
- echo "2. Add to ~/.profile (recommended for all shells)"
- echo "3. Add to ~/.zshrc (for zsh users)"
- echo "4. Skip (environment variables will only be available in current session)"
- echo
- read -p "Choose an option (1-4): " -n 1 -r
- echo
-
- case $REPLY in
- 1)
- SHELL_CONFIG="${HOME}/.bashrc"
- ;;
- 2)
- SHELL_CONFIG="${HOME}/.profile"
- ;;
- 3)
- SHELL_CONFIG="${HOME}/.zshrc"
- ;;
- 4)
- log_info "Skipping persistent environment variable setup"
- log_info "To use SUMO in other sessions, run: source ${DEPS_DIR}/.sumo_home"
- return 0
- ;;
- *)
- log_warning "Invalid option. Skipping persistent environment variable setup"
- return 0
- ;;
- esac
-
- # Check if SUMO_HOME is already in the config file
- if grep -q "SUMO_HOME=" "${SHELL_CONFIG}" 2>/dev/null; then
- log_warning "SUMO_HOME already exists in ${SHELL_CONFIG}"
- read -p "Update existing SUMO_HOME? (y/n) " -n 1 -r
- echo
- if [[ $REPLY =~ ^[Yy]$ ]]; then
- # Remove existing SUMO_HOME lines
- sed -i '/SUMO_HOME=/d' "${SHELL_CONFIG}"
- else
- log_info "Keeping existing SUMO_HOME configuration"
- return 0
- fi
- fi
-
- # Add environment variables to shell config
- echo "" >> "${SHELL_CONFIG}"
- echo "# TeraSim environment variables" >> "${SHELL_CONFIG}"
- echo "export SUMO_HOME=\"${SUMO_HOME}\"" >> "${SHELL_CONFIG}"
- echo "export PATH=\"\$SUMO_HOME/bin:\$PATH\"" >> "${SHELL_CONFIG}"
-
- log_info "✅ Environment variables added to ${SHELL_CONFIG}"
- log_info "📍 SUMO_HOME: ${SUMO_HOME}"
- log_info "📍 PATH updated to include SUMO tools"
-
- # Verify the setup
- log_info "Verifying environment variable setup..."
- if [ -f "${SUMO_HOME}/bin/sumo" ] || [ -f "${SUMO_HOME}/tools/sumo" ]; then
- log_info "✅ SUMO tools found in ${SUMO_HOME}"
- else
- log_warning "⚠️ SUMO tools not found in expected location"
- fi
-
- echo
- log_info "To apply changes in current session, run:"
- log_info " source ${SHELL_CONFIG}"
- log_info "Or restart your terminal"
-}
-
main() {
log_info "Starting TeraSim monorepo setup..."
@@ -306,7 +212,6 @@ main() {
check_gcc_gpp
check_redis
setup_sumo_tools
- setup_environment_variables
setup_monorepo
create_output_directories
@@ -325,8 +230,7 @@ packages = [
('terasim_vis', 'Visualization tools'),
('terasim_envgen', 'Environment generation tools'),
('terasim_datazoo', 'Data processing tools'),
- ('terasim_service', 'Service API'),
- ('terasim_cosmos', 'Cosmos-Drive integration')
+ ('terasim_service', 'Service API')
]
for pkg_name, description in packages:
@@ -337,19 +241,12 @@ for pkg_name, description in packages:
print(f' ❌ {pkg_name:<18} - {description} (failed)')
"
- echo
- echo "Environment variables:"
- echo " SUMO_HOME is set to: ${SUMO_HOME:-'Not set'}"
- echo " To verify: echo \$SUMO_HOME"
echo
echo "Development commands:"
echo " pytest # Run tests"
echo " black . # Format code"
echo " python # Start Python shell"
echo
- echo "If SUMO_HOME is not set in new terminal sessions:"
- echo " source ~/.terasim/deps/.sumo_home # Load environment variables"
- echo
}
# Run main function only if script is executed directly
diff --git a/test_config.yaml b/test_config.yaml
new file mode 100644
index 0000000..b811945
--- /dev/null
+++ b/test_config.yaml
@@ -0,0 +1,19 @@
+map_search:
+ bbox_size: 1000
+ default_city: Ann Arbor, Michigan, USA
+ max_results: 5
+output:
+ base_dir: test_output
+visualization:
+ preview:
+ dpi: 300
+ figure_height: 6
+ figure_width: 8
+ interactive_map: false
+ randomize:
+ enabled: true
+ map_style: true
+ node_selection: true
+ seed: 33
+ visualization: true
+ satellite_view: true
diff --git a/vTypeDistributions.add.xml b/vTypeDistributions.add.xml
new file mode 100644
index 0000000..e96a704
--- /dev/null
+++ b/vTypeDistributions.add.xml
@@ -0,0 +1,110 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+