diff --git a/packages/terasim-nde-nade/terasim_nde_nade/adversity/static/construction.py b/packages/terasim-nde-nade/terasim_nde_nade/adversity/static/construction.py index 6927695..92fe227 100644 --- a/packages/terasim-nde-nade/terasim_nde_nade/adversity/static/construction.py +++ b/packages/terasim-nde-nade/terasim_nde_nade/adversity/static/construction.py @@ -25,6 +25,25 @@ def create_construction_cone_type(): traci.vehicletype.setColor(custom_type_id, (255, 140, 0, 255)) # Orange color return custom_type_id +def create_invisible_cone_type(): + """Create a custom vehicle type for invisible cones. + + Returns: + str: The ID of the custom vehicle type. + """ + custom_type_id = "INVISIBLE_CONE" + + if custom_type_id not in traci.vehicletype.getIDList(): + traci.vehicletype.copy("DEFAULT_VEHTYPE", custom_type_id) + traci.vehicletype.setVehicleClass(custom_type_id, "passenger") + traci.vehicletype.setShapeClass(custom_type_id, "passenger") + traci.vehicletype.setLength(custom_type_id, 0.3) # Smaller cone + traci.vehicletype.setWidth(custom_type_id, 0.3) # Smaller cone + traci.vehicletype.setHeight(custom_type_id, 0.7) # Cone height + traci.vehicletype.setMinGap(custom_type_id, 0.1) # Minimal gap + traci.vehicletype.setColor(custom_type_id, (0, 255, 0, 255)) # Green color + return custom_type_id + def create_construction_barrier_type(): """Create a custom vehicle type for construction barriers. @@ -68,6 +87,12 @@ def create_construction_sign_type(): class ConstructionAdversity(AbstractStaticAdversity): def __init__(self, **kwargs): + # Extract lane_ids parameter (backward compatible with lane_id) + self._lane_ids = kwargs.pop("lane_ids", None) + + # Extract closure direction for multiple lanes + self._closure_direction = kwargs.pop("closure_direction", "right") # "left" or "right" + # Extract our custom parameters before passing to parent self._construction_mode = kwargs.pop("construction_mode", "full_lane") # "full_lane" or "partial_lane" self._start_position = kwargs.pop("start_position", None) @@ -97,14 +122,93 @@ def __init__(self, **kwargs): # Call parent constructor with remaining kwargs super().__init__(**kwargs) - + + # Handle backward compatibility for lane_id/lane_ids + if self._lane_ids is None: + # Check if parent class set _lane_id from kwargs + if hasattr(self, '_lane_id') and self._lane_id: + self._lane_ids = [self._lane_id] + else: + self._lane_ids = [] + elif not isinstance(self._lane_ids, list): + # Convert single lane_id string to list + self._lane_ids = [self._lane_ids] + + # For backward compatibility, set _lane_id to first lane if available + if self._lane_ids and len(self._lane_ids) > 0: + self._lane_id = self._lane_ids[0] + elif not hasattr(self, '_lane_id'): + self._lane_id = "" + # Initialize other attributes self._construction_object_ids = [] - + + # Dictionary to store lane information for multiple lanes + self._lane_info = {} # {lane_id: {'length': float, 'width': float}} + # If work_zone_offset not specified, use lane_offset if self._work_zone_offset is None: self._work_zone_offset = self._lane_offset - + + def get_boundary_lane(self): + """Get the boundary lane ID based on closure direction. + + For multiple lane closures, returns the lane at the boundary of the closure: + - If closure_direction is "right", returns the leftmost closed lane (highest index) + - If closure_direction is "left", returns the rightmost closed lane (lowest index) + + In SUMO: Lane 0 is rightmost, higher numbers are further left. + + Returns: + str: The lane ID at the closure boundary, or empty string if no lanes. + """ + if not self._lane_ids: + return "" + + if len(self._lane_ids) == 1: + return self._lane_ids[0] + + # Sort lanes by their index (number at the end of the ID) + sorted_lanes = sorted(self._lane_ids, key=lambda x: int(x.split('_')[-1])) + + if self._closure_direction == "right": + # Right closure (closing right lanes): return leftmost closed lane (highest index) + # Example: closing lanes 0,1,2 -> return lane 2 (leftmost of the closed lanes) + return sorted_lanes[-1] + else: # "left" + # Left closure (closing left lanes): return rightmost closed lane (lowest index) + # Example: closing lanes 2,3 -> return lane 2 (rightmost of the closed lanes) + return sorted_lanes[0] + + def _calculate_other_lanes_width(self, exclude_lane_id): + """Calculate the total width of all lanes in the construction zone except the specified lane. + + Args: + exclude_lane_id (str): Lane ID to exclude from the calculation + + Returns: + float: Total width of other construction zone lanes in meters. + """ + # Sort lanes by their index to ensure consistent results regardless of input order + sorted_lanes = sorted(self._lane_ids, key=lambda x: int(x.split('_')[-1])) + + total_width = 0.0 + for lane_id in sorted_lanes: + if lane_id == exclude_lane_id: + continue # Skip the excluded lane + + if lane_id in self._lane_info: + total_width += self._lane_info[lane_id]['width'] + else: + # Fallback to querying SUMO if not in cache + try: + width = traci.lane.getWidth(lane_id) + total_width += width + except: + logger.warning(f"Could not get width for lane {lane_id}, using default 3.2m") + total_width += 3.2 # Standard lane width fallback + return total_width + def is_effective(self): """Check if the adversarial event is effective. @@ -114,14 +218,35 @@ def is_effective(self): if self._lane_id == "": logger.warning("Lane ID is not provided.") return False - try: - allowed_type_list = traci.lane.getAllowed(self._lane_id) - lane_length = traci.lane.getLength(self._lane_id) - self._lane_width = traci.lane.getWidth(self._lane_id) - except: - logger.warning(f"Failed to get lane information for {self._lane_id}.") + + # Populate lane information dictionary for all configured lanes + for lane_id in self._lane_ids: + try: + allowed_type_list = traci.lane.getAllowed(lane_id) + lane_length = traci.lane.getLength(lane_id) + lane_width = traci.lane.getWidth(lane_id) + + # Store in dictionary + self._lane_info[lane_id] = { + 'length': lane_length, + 'width': lane_width + } + + # Set backward compatibility attribute for primary lane + if lane_id == self._lane_id: + self._lane_width = lane_width + + except: + logger.warning(f"Failed to get lane information for {lane_id}.") + return False + + # Get primary lane info for validation + if self._lane_id in self._lane_info: + lane_length = self._lane_info[self._lane_id]['length'] + else: + logger.warning(f"Primary lane {self._lane_id} not found in lane info.") return False - + # Additional validation for partial lane mode if self._construction_mode == "partial_lane": if self._start_position is None or self._end_position is None: @@ -133,7 +258,7 @@ def is_effective(self): if self._start_position >= self._end_position: logger.warning("Start position must be less than end position.") return False - + return True def _calculate_zone_positions(self): @@ -201,7 +326,8 @@ def _calculate_lateral_offset(self, position, zone_type, zone_start, zone_end, o float: Lateral offset in meters """ lane_index = int(self._lane_id.split('_')[-1]) - is_left_lane = lane_index > 1 + # Use closure direction to determine placement side + is_left_closure = self._closure_direction == "left" # Special handling for warning signs - place on shoulder if object_type == 'sign' and zone_type in ['warning', 'termination']: return self._warning_sign_offset # Negative value places on right shoulder @@ -211,21 +337,28 @@ def _calculate_lateral_offset(self, position, zone_type, zone_start, zone_end, o return 0.0 elif zone_type == 'taper_in': - # Gradual offset increase from right edge to work zone + # Gradual offset increase from edge of current lane plus other lanes to work zone zone_length = zone_end - zone_start + # Use closure direction to determine placement side + is_left_closure = self._closure_direction == "left" + + # Get boundary lane for reference + boundary_lane = self.get_boundary_lane() + other_lanes_width = self._calculate_other_lanes_width(boundary_lane) + if zone_length <= 0: - # Start at appropriate edge based on lane type - if is_left_lane: - return self._lane_width / 2 - 0.3 # Start at left edge (positive = left) + # Start at appropriate edge based on closure direction and other lanes + if is_left_closure: + return self._lane_width / 2 - 0.3 + other_lanes_width # Current lane edge + other lanes else: - return -(self._lane_width / 2 - 0.3) # Start at right edge (negative = right) + return -(self._lane_width / 2 - 0.3 + other_lanes_width) # Current lane edge + other lanes progress = (position - zone_start) / zone_length - - if is_left_lane: - edge_offset = self._lane_width / 2 - 0.3 # Positive for left side + # Calculate edge offset: current lane's half-width + width of other lanes in construction zone + if is_left_closure: + edge_offset = self._lane_width / 2 - 0.3 + other_lanes_width # Positive for left side else: - edge_offset = -(self._lane_width / 2 - 0.3) # Negative for right side + edge_offset = -(self._lane_width / 2 - 0.3 + other_lanes_width) # Negative for right side if self._taper_type == 'linear': offset = edge_offset + progress * (self._work_zone_offset - edge_offset) @@ -235,29 +368,33 @@ def _calculate_lateral_offset(self, position, zone_type, zone_start, zone_end, o offset = edge_offset + s_curve * (self._work_zone_offset - edge_offset) else: offset = edge_offset + progress * (self._work_zone_offset - edge_offset) - - # Ensure offset stays within lane boundaries - max_left_offset = self._lane_width / 2 - 0.3 # Leave 0.3m margin - max_right_offset = -(self._lane_width / 2 - 0.3) - return max(max_right_offset, min(offset, max_left_offset)) + + + return offset elif zone_type in ['buffer', 'work']: - # Full offset in work zone, but ensure within lane boundaries - max_left_offset = self._lane_width / 2 - 0.3 # Leave 0.3m margin on left - max_right_offset = -(self._lane_width / 2 - 0.3) # Leave 0.3m margin on right - return max(max_right_offset, min(self._work_zone_offset, max_left_offset)) + # Full offset in work zone (already validated during initialization) + return self._work_zone_offset elif zone_type == 'taper_out': - # Gradual offset decrease from work zone to right edge + # Gradual offset decrease from work zone to edge of current lane plus other lanes zone_length = zone_end - zone_start + # Use closure direction to determine placement side + is_left_closure = self._closure_direction == "left" + + # Get boundary lane for reference + boundary_lane = self.get_boundary_lane() + other_lanes_width = self._calculate_other_lanes_width(boundary_lane) + if zone_length <= 0: return self._work_zone_offset progress = (position - zone_start) / zone_length - - if is_left_lane: - edge_offset = self._lane_width / 2 - 0.3 # Positive for left side + + # Calculate edge offset: current lane's half-width + width of other lanes in construction zone + if is_left_closure: + edge_offset = self._lane_width / 2 - 0.3 + other_lanes_width # Positive for left side else: - edge_offset = -(self._lane_width / 2 - 0.3) # Negative for right side + edge_offset = -(self._lane_width / 2 - 0.3 + other_lanes_width) # Negative for right side if self._taper_type == 'linear': offset = self._work_zone_offset + progress * (edge_offset - self._work_zone_offset) @@ -267,11 +404,8 @@ def _calculate_lateral_offset(self, position, zone_type, zone_start, zone_end, o offset = self._work_zone_offset + s_curve * (edge_offset - self._work_zone_offset) else: offset = self._work_zone_offset + progress * (edge_offset - self._work_zone_offset) - - # Ensure offset stays within lane boundaries - max_left_offset = self._lane_width / 2 - 0.3 # Leave 0.3m margin - max_right_offset = -(self._lane_width / 2 - 0.3) - return max(max_right_offset, min(offset, max_left_offset)) + + return offset return 0.0 @@ -288,7 +422,8 @@ def _calculate_shoulder_coordinates(self, lane_position): edge_id = traci.lane.getEdgeID(self._lane_id) lane_index = int(self._lane_id.split('_')[-1]) # Extract lane index from lane ID x_center, y_center = traci.simulation.convert2D(edge_id, lane_position, lane_index) - is_left_lane = lane_index > 1 + # Use closure direction to determine placement side + is_left_closure = self._closure_direction == "left" # Get lane angle at this position lane_angle = traci.lane.getAngle(self._lane_id, lane_position) @@ -303,28 +438,30 @@ def _calculate_shoulder_coordinates(self, lane_position): # Calculate shoulder coordinates # Note: SUMO uses a different coordinate system where y increases northward - if is_left_lane: - # For left lane, place sign on left shoulder (subtract offset) + if is_left_closure: + # For left closure, place sign on left shoulder (subtract offset) x_shoulder = x_center - offset_distance * math.cos(perpendicular_rad) y_shoulder = y_center - offset_distance * math.sin(perpendicular_rad) else: - # For right lane, place sign on right shoulder (add offset) + # For right closure, place sign on right shoulder (add offset) x_shoulder = x_center + offset_distance * math.cos(perpendicular_rad) y_shoulder = y_center + offset_distance * math.sin(perpendicular_rad) return x_shoulder, y_shoulder, lane_angle - def _place_object(self, position, lateral_offset, object_type, zone_type): + def _place_object(self, position, lateral_offset, object_type, zone_type, lane_id=None, visible_to_AV=True): """Place a single construction object at the specified position. - + Args: position: Longitudinal position on the lane lateral_offset: Lateral offset from lane center object_type: Type of object ('cone', 'barrier', 'sign') zone_type: Zone type for logging and ID generation + lane_id: Optional lane ID to place object on (defaults to self._lane_id) """ + current_lane = lane_id or self._lane_id # Create unique object ID - object_id = f"CONSTRUCTION_{zone_type}_{self._lane_id}_{len(self._construction_object_ids)}" + object_id = f"CONSTRUCTION_{zone_type}_{current_lane}_{len(self._construction_object_ids)}" self._construction_object_ids.append(object_id) # Add vehicle to simulation @@ -360,7 +497,7 @@ def _place_object(self, position, lateral_offset, object_type, zone_type): logger.debug(f"Placed warning sign {object_id} on shoulder at ({x_shoulder:.1f}, {y_shoulder:.1f})") else: # Normal placement for cones and barriers - traci.vehicle.moveTo(object_id, self._lane_id, position) + traci.vehicle.moveTo(object_id, current_lane, position) # Apply lateral offset for non-sign objects if lateral_offset != 0: @@ -404,97 +541,172 @@ def _calculate_dynamic_spacing(self, zone_type): def _create_construction_objects(self): """Create construction objects with proper zone-based placement.""" - edge_id = traci.lane.getEdgeID(self._lane_id) - - # Create route for construction objects - self._route_id = f"r_construction_{self._lane_id}" - if self._route_id not in traci.route.getIDList(): - traci.route.add(self._route_id, [edge_id]) - + # Get boundary lane for cone placement in work zone + boundary_lane = self.get_boundary_lane() + # Create object types and store them as instance variables for comparison self._cone_type = create_construction_cone_type() + self._invisible_cone_type = create_invisible_cone_type() self._barrier_type = create_construction_barrier_type() self._sign_type = create_construction_sign_type() - - # Calculate zones + + # Calculate zones based on first lane (they should be same for all lanes) zones = self._calculate_zone_positions() - + # Process each zone for zone_type, (zone_start, zone_end) in zones.items(): # Calculate dynamic spacing based on speed limit spacing = self._calculate_dynamic_spacing(zone_type) - - # Determine object type for this zone - if zone_type == 'warning': - object_types = ['sign'] # Only warning signs in warning zone - elif zone_type in ['taper_in', 'taper_out']: - object_types = ['cone'] - elif zone_type == 'buffer': - object_types = ['cone', 'cone', 'barrier'] # Mostly cones, some barriers - elif zone_type == 'work': - if self._construction_type == 'mixed': - object_types = ['cone', 'cone', 'barrier'] - else: - object_types = [self._construction_type] - elif zone_type == 'termination': - object_types = ['sign'] + + # For work zone, only place cones on boundary lane + if zone_type == 'work': + if not boundary_lane: + continue + + # Create route for this lane if not exists + edge_id = traci.lane.getEdgeID(boundary_lane) + route_id = f"r_construction_{boundary_lane}" + if route_id not in traci.route.getIDList(): + traci.route.add(route_id, [edge_id]) + self._route_id = route_id + + # Place cones only on boundary lane + current_pos = zone_start + while current_pos < zone_end: + # Calculate lateral offset for this position + lateral_offset = self._calculate_lateral_offset( + current_pos, zone_type, zone_start, zone_end, 'cone' + ) + + # Place cone on boundary lane + object_id = f"CONSTRUCTION_{zone_type}_{boundary_lane}_{len(self._construction_object_ids)}" + self._construction_object_ids.append(object_id) + + traci.vehicle.add( + object_id, + routeID=route_id, + typeID=self._cone_type, + ) + + traci.vehicle.setSpeedMode(object_id, 0) + traci.vehicle.setLaneChangeMode(object_id, 0) + traci.vehicle.moveTo(object_id, boundary_lane, current_pos) + + if lateral_offset != 0: + try: + traci.vehicle.changeSublane(object_id, lateral_offset) + except: + logger.debug(f"Could not apply lateral offset {lateral_offset} to {object_id}") + + traci.vehicle.setSpeed(object_id, 0) + current_pos += spacing + else: - continue - - # Place objects in this zone - current_pos = zone_start - object_index = 0 - - while current_pos < zone_end: - # Select object type - obj_type_name = object_types[object_index % len(object_types)] - if obj_type_name == 'cone': - type_id = self._cone_type - elif obj_type_name == 'barrier': - type_id = self._barrier_type - elif obj_type_name == 'sign': - type_id = self._sign_type - - # Calculate lateral offset for this position - lateral_offset = self._calculate_lateral_offset( - current_pos, zone_type, zone_start, zone_end, obj_type_name - ) - - # Place the object - self._place_object(current_pos, lateral_offset, type_id, zone_type) - - current_pos += spacing - object_index += 1 - - logger.info(f"Created {len(self._construction_object_ids)} construction objects in {len(zones)} zones on lane {self._lane_id}") + # For non-work zones, also place on boundary lane for consistency + if not boundary_lane: + continue + + edge_id = traci.lane.getEdgeID(boundary_lane) + route_id = f"r_construction_{boundary_lane}" + if route_id not in traci.route.getIDList(): + traci.route.add(route_id, [edge_id]) + self._route_id = route_id + + # Determine object type for this zone + if zone_type == 'warning': + object_types = ['sign'] # Only warning signs in warning zone + elif zone_type in ['taper_in', 'taper_out']: + object_types = ['cone'] + elif zone_type == 'buffer': + object_types = ['cone', 'cone', 'barrier'] # Mostly cones, some barriers + elif zone_type == 'termination': + object_types = ['sign'] + else: + continue + + # Place objects in this zone + current_pos = zone_start + object_index = 0 + + while current_pos < zone_end: + # Select object type + obj_type_name = object_types[object_index % len(object_types)] + if obj_type_name == 'cone': + type_id = self._cone_type + elif obj_type_name == 'barrier': + type_id = self._barrier_type + elif obj_type_name == 'sign': + type_id = self._sign_type + + # Calculate lateral offset for this position + lateral_offset = self._calculate_lateral_offset( + current_pos, zone_type, zone_start, zone_end, obj_type_name + ) + + # Place the object on boundary lane + self._place_object(current_pos, lateral_offset, type_id, zone_type, boundary_lane, visible_to_AV=True) + # Only place extra invisible cones in taper_in, taper_out, work, and buffer zones + if self._spacing > 0 and zone_type in ['taper_in', 'taper_out', 'work', 'buffer']: + next_visible_pos = current_pos + spacing + intermediate_pos = current_pos + self._spacing + while intermediate_pos < next_visible_pos and intermediate_pos < zone_end: + additional_offset = self._calculate_lateral_offset( + intermediate_pos, zone_type, zone_start, zone_end, obj_type_name) + self._place_object( + intermediate_pos, + additional_offset, + self._invisible_cone_type, + zone_type, + boundary_lane, + visible_to_AV=False, + ) + intermediate_pos += self._spacing + current_pos += spacing + object_index += 1 + + logger.info(f"Created {len(self._construction_object_ids)} construction objects in zones, with work zone cones on boundary lane {boundary_lane}") def initialize(self, time: float): """Initialize the adversarial event. """ assert self.is_effective(), "Adversarial event is not effective." + # Validate and correct work zone offset at initialization + max_left_offset = self._lane_width / 2 - 0.3 # Leave 0.3m margin on left + max_right_offset = -(self._lane_width / 2 - 0.3) # Leave 0.3m margin on right + + # Clamp work zone offset to valid range + if self._work_zone_offset > max_left_offset: + logger.warning(f"Work zone offset {self._work_zone_offset} exceeds max left offset {max_left_offset}, clamping to max") + self._work_zone_offset = max_left_offset + elif self._work_zone_offset < max_right_offset: + logger.warning(f"Work zone offset {self._work_zone_offset} exceeds max right offset {max_right_offset}, clamping to max") + self._work_zone_offset = max_right_offset + # Check for and remove vehicles in the construction zone (except stalled vehicle) if self._start_position is not None and self._end_position is not None: - # Get all vehicles on the lane - vehicles_on_lane = traci.lane.getLastStepVehicleIDs(self._lane_id) - - for vehicle_id in vehicles_on_lane: - # Skip if this is a stalled vehicle (check if it's marked as stalled) - # Stalled vehicles typically have "stalled" or "STALLED" in their ID - if "stalled" in vehicle_id.lower() or "STALLED" in vehicle_id: - logger.debug(f"Skipping stalled vehicle {vehicle_id} in construction zone") - continue + # Check all lanes in the construction zone + for lane_id in self._lane_ids: + # Get all vehicles on each construction lane + vehicles_on_lane = traci.lane.getLastStepVehicleIDs(lane_id) - # Get vehicle position on the lane - try: - vehicle_pos = traci.vehicle.getLanePosition(vehicle_id) - - # Check if vehicle is inside the construction zone - if self._start_position <= vehicle_pos <= self._end_position: - logger.info(f"Removing vehicle {vehicle_id} from construction zone at position {vehicle_pos}") - traci.vehicle.remove(vehicle_id) - except Exception as e: - logger.debug(f"Could not check/remove vehicle {vehicle_id}: {e}") + for vehicle_id in vehicles_on_lane: + # Skip if this is a stalled vehicle (check if it's marked as stalled) + # Stalled vehicles typically have "stalled" or "STALLED" in their ID + if "stalled" in vehicle_id.lower() or "STALLED" in vehicle_id: + logger.debug(f"Skipping stalled vehicle {vehicle_id} in construction zone on lane {lane_id}") + continue + + # Get vehicle position on the lane + try: + vehicle_pos = traci.vehicle.getLanePosition(vehicle_id) + + # Check if vehicle is inside the construction zone + if self._start_position <= vehicle_pos <= self._end_position: + logger.info(f"Removing vehicle {vehicle_id} from construction zone at position {vehicle_pos} on lane {lane_id}") + traci.vehicle.remove(vehicle_id) + except Exception as e: + logger.debug(f"Could not check/remove vehicle {vehicle_id} on lane {lane_id}: {e}") if self._construction_mode == "full_lane": # Original behavior: block entire lane diff --git a/packages/terasim-nde-nade/terasim_nde_nade/adversity/static/stalled_object.py b/packages/terasim-nde-nade/terasim_nde_nade/adversity/static/stalled_object.py index 3b46743..b03382f 100644 --- a/packages/terasim-nde-nade/terasim_nde_nade/adversity/static/stalled_object.py +++ b/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,17 +131,37 @@ 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, @@ -142,18 +169,21 @@ def add_vehicle(self, vehicle_id: str): 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) \ No newline at end of file diff --git a/packages/terasim-nde-nade/terasim_nde_nade/utils/adversity/abstract_adversity.py b/packages/terasim-nde-nade/terasim_nde_nade/utils/adversity/abstract_adversity.py index 0281f23..4b3f3d0 100644 --- a/packages/terasim-nde-nade/terasim_nde_nade/utils/adversity/abstract_adversity.py +++ b/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 \ No newline at end of file diff --git a/scripts/run_experiments.py b/scripts/run_experiments.py index 9dfd9d6..9251414 100644 --- a/scripts/run_experiments.py +++ b/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}") \ No newline at end of file diff --git a/scripts/run_experiments_debug.py b/scripts/run_experiments_debug.py index 5cb7f54..c86ac6f 100644 --- a/scripts/run_experiments_debug.py +++ b/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)) \ No newline at end of file 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/vTypeDistributions.add.xml b/vTypeDistributions.add.xml new file mode 100644 index 0000000..e96a704 --- /dev/null +++ b/vTypeDistributions.add.xml @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +