Merge XODR import, construction zone enhancement to the upstream

packages/terasim-nde-nade/terasim_nde_nade/adversity/static/stalled_object.py

@@ -74,7 +74,7 @@ def is_effective(self):
         Returns:
             bool: Flag to indicate if the adversarial event is effective.
         """
-        
+
         if self._placement_mode == "lane_position":
             if self._lane_id == "":
                 logger.warning("Lane ID is not provided.")
@@ -97,8 +97,15 @@ def is_effective(self):
             if self._angle is None:
                 logger.warning("Angle is not provided for xy_angle placement mode.")
                 return False
+        elif self._placement_mode == "latlon_degree":
+            if self._lon is None or self._lat is None:
+                logger.warning("Longitude and latitude are not provided for latlon_degree placement mode.")
+                return False
+            if self._degree is None:
+                logger.warning("Degree (heading angle) is not provided for latlon_degree placement mode.")
+                return False
         else:
-            logger.warning(f"Invalid placement mode: {self._placement_mode}. Must be 'lane_position' or 'xy_angle'.")
+            logger.warning(f"Invalid placement mode: {self._placement_mode}. Must be 'lane_position', 'xy_angle', or 'latlon_degree'.")
             return False
 
         if self._object_type == "":
@@ -124,36 +131,59 @@ def set_vehicle_feature(self, vehicle_id: str):
     def add_vehicle(self, vehicle_id: str):
         if self._placement_mode == "lane_position":
             stalled_object_route_id = self.set_vehicle_route(vehicle_id)
+            # Handle optional _vclass attribute
+            add_kwargs = {
+                "vehID": vehicle_id,
+                "routeID": stalled_object_route_id,
+                "typeID": self._object_type,
+            }
+            if hasattr(self, '_vclass') and self._vclass is not None:
+                add_kwargs["vclass"] = self._vclass
+            traci.vehicle.add(**add_kwargs)
+            self.set_vehicle_feature(vehicle_id)
+            traci.vehicle.moveTo(vehicle_id, self._lane_id, self._lane_position)
+            traci.vehicle.setSpeed(vehicle_id, 0)
+        elif self._placement_mode == "xy_angle":
+            edge_id = self._get_edge_from_xy()
+            stalled_object_route_id = self.set_vehicle_route_for_xy(vehicle_id, edge_id)
             traci.vehicle.add(
                 vehicle_id,
                 routeID=stalled_object_route_id,
                 typeID=self._object_type,
-                vclass=self._vclass
             )
             self.set_vehicle_feature(vehicle_id)
-            traci.vehicle.moveTo(vehicle_id, self._lane_id, self._lane_position)
+            traci.vehicle.moveToXY(vehicle_id, "", -1, self._x, self._y, self._angle, keepRoute=2)
             traci.vehicle.setSpeed(vehicle_id, 0)
-        elif self._placement_mode == "xy_angle":
-            edge_id = self._get_edge_from_xy()
+        elif self._placement_mode == "latlon_degree":
+            # Convert lat/lon to x/y coordinates
+            x, y = self._convert_latlon_to_xy()
+            if x is None or y is None:
+                logger.error(f"Failed to convert lat/lon to x/y coordinates. Cannot place vehicle {vehicle_id}.")
+                return
+
+            edge_id = self._get_edge_from_latlon()
             stalled_object_route_id = self.set_vehicle_route_for_xy(vehicle_id, edge_id)
             traci.vehicle.add(
                 vehicle_id,
                 routeID=stalled_object_route_id,
                 typeID=self._object_type,
             )
             self.set_vehicle_feature(vehicle_id)
-            traci.vehicle.moveToXY(vehicle_id, "", -1, self._x, self._y, self._angle, keepRoute=2)
+            # Use moveToXY with converted coordinates and degree as angle
+            traci.vehicle.moveToXY(vehicle_id, "", -1, x, y, self._degree, keepRoute=2)
             traci.vehicle.setSpeed(vehicle_id, 0)
 
     def set_vehicle_route(self, vehicle_id: str):
         edge_id = traci.lane.getEdgeID(self._lane_id)
-        stalled_object_route_id = f"r_stalled_object"
+        # Use edge_id in route name to allow different routes for different edges
+        stalled_object_route_id = f"r_stalled_object_{edge_id}"
         if stalled_object_route_id not in traci.route.getIDList():
             traci.route.add(stalled_object_route_id, [edge_id])
         return stalled_object_route_id
-    
+
     def set_vehicle_route_for_xy(self, vehicle_id: str, edge_id: str):
-        stalled_object_route_id = f"r_stalled_object_xy"
+        # Use edge_id in route name to allow different routes for different edges
+        stalled_object_route_id = f"r_stalled_object_xy_{edge_id}"
         if stalled_object_route_id not in traci.route.getIDList():
             traci.route.add(stalled_object_route_id, [edge_id])
         return stalled_object_route_id
@@ -165,15 +195,47 @@ def _get_edge_from_xy(self):
         except:
             logger.warning(f"Failed to get edge from coordinates ({self._x}, {self._y}). Using default edge.")
             return "1"
+
+    def _convert_latlon_to_xy(self):
+        """Convert latitude/longitude to SUMO x/y coordinates.
+
+        Returns:
+            tuple: (x, y) coordinates in SUMO coordinate system
+        """
+        try:
+            x, y = traci.simulation.convertGeo(self._lon, self._lat, fromGeo=True)
+            return x, y
+        except Exception as e:
+            logger.warning(f"Failed to convert lat/lon ({self._lat}, {self._lon}) to x/y coordinates: {e}")
+            return None, None
+
+    def _get_edge_from_latlon(self):
+        """Get edge ID from latitude/longitude coordinates.
+
+        Returns:
+            str: Edge ID
+        """
+        try:
+            x, y = self._convert_latlon_to_xy()
+            if x is None or y is None:
+                logger.warning("Failed to convert lat/lon to x/y. Using default edge.")
+                return "1"
+            edge_id = traci.simulation.convertRoad(x, y, isGeo=False)[0]
+            return edge_id
+        except Exception as e:
+            logger.warning(f"Failed to get edge from lat/lon ({self._lat}, {self._lon}): {e}. Using default edge.")
+            return "1"
 
     def initialize(self, time: float):
         """Initialize the adversarial event.
         """
         assert self.is_effective(), "Adversarial event is not effective."
+        # Use unique adversity_id to avoid conflicts when multiple stalled objects share the same object_type
+        unique_suffix = str(self._adversity_id).replace("-", "")[:8]  # Use first 8 chars of UUID
         if self._object_type == "PEDESTRIAN":
-            stalled_object_id = f"VRU_{self._object_type}_stalled_object"
+            stalled_object_id = f"VRU_{self._object_type}_stalled_object_{unique_suffix}"
         else:
-            stalled_object_id = f"BV_{self._object_type}_stalled_object"
+            stalled_object_id = f"BV_{self._object_type}_stalled_object_{unique_suffix}"
         self._static_adversarial_object_id_list.append(stalled_object_id)
 
         if self._placement_mode == "lane_position":
@@ -186,7 +248,12 @@ def initialize(self, time: float):
             self.edge_id = edge_id
             self.lane_index = 0
             self.lane_position = None
-
+        elif self._placement_mode == "latlon_degree":
+            edge_id = self._get_edge_from_latlon()
+            self.edge_id = edge_id
+            self.lane_index = 0
+            self.lane_position = None
+
         self.add_vehicle(stalled_object_id)
 
         self._duration=0
@@ -206,6 +273,11 @@ def update(self, time: float):
             elif self._placement_mode == "xy_angle":
                 edge_id = self._get_edge_from_xy()
                 traci.vehicle.moveToXY(self.stalled_object_id, "", -1, self._x, self._y, self._angle, keepRoute=2)
+            elif self._placement_mode == "latlon_degree":
+                # Convert lat/lon to x/y coordinates for each update to maintain position
+                x, y = self._convert_latlon_to_xy()
+                if x is not None and y is not None:
+                    traci.vehicle.moveToXY(self.stalled_object_id, "", -1, x, y, self._degree, keepRoute=2)
             traci.vehicle.setSpeed(self.stalled_object_id, 0)
 
 

packages/terasim-nde-nade/terasim_nde_nade/utils/adversity/abstract_adversity.py

@@ -73,7 +73,10 @@ def __init__(
         placement_mode="lane_position",
         x=None,
         y=None,
-        angle=None
+        angle=None,
+        lon=None,
+        lat=None,
+        degree=None
     ):
         """Initialize the AbstractStaticAdversity class. This class is an abstract class that defines the interface for the different types of adversities that can be triggered in the simulation.
 
@@ -84,10 +87,13 @@ def __init__(
             end_time (float): End time of the adversarial event. Default is -1 (infinite).
             object_type (str): Type of the object. Default is an empty string.
             other_settings (dict): Other settings for the adversarial event. Default is None.
-            placement_mode (str): Placement mode - "lane_position" or "xy_angle". Default is "lane_position".
+            placement_mode (str): Placement mode - "lane_position", "xy_angle", or "latlon_degree". Default is "lane_position".
             x (float): X coordinate for xy_angle placement mode. Default is None.
             y (float): Y coordinate for xy_angle placement mode. Default is None.
             angle (float): Angle for xy_angle placement mode. Default is None.
+            lon (float): Longitude for latlon_degree placement mode. Default is None.
+            lat (float): Latitude for latlon_degree placement mode. Default is None.
+            degree (float): Degree (heading angle) for latlon_degree placement mode. Default is None.
         """
         self._adversity_id = uuid.uuid4()
         self._lane_id = lane_id
@@ -101,6 +107,9 @@ def __init__(
         self._x = x
         self._y = y
         self._angle = angle
+        self._lon = lon
+        self._lat = lat
+        self._degree = degree
 
     @property
     def start_time(self):
@@ -133,4 +142,4 @@ def initialize(self, time: float):
     def update(self, time: float):
         """Update the adversarial event.
         """
-        pass
+        pass

scripts/run_experiments.py

@@ -109,4 +109,4 @@ def run_simulation(config_file="examples/scenarios/police_pullover_case.yaml", a
         viz_update_freq=2 # Update every 2 simulation steps (reduce load)
     )
 
-    print(f"Final simulation result: {result}")
+    print(f"Final simulation result: {result}")

scripts/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
 
@@ -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",
@@ -91,4 +100,4 @@ def main(config_path: str) -> None:
         exit(1)
 
     logger.info(f"Running experiment with config: {config_path}")
-    main(str(config_path))
+    main(str(config_path))