Skip to content
Snippets Groups Projects
Commit c4b74285 authored by sacardoz's avatar sacardoz Committed by Eli Henry Dykhne
Browse files

angle diff fix

parent 1ec2d197
No related branches found
No related tags found
2 merge requests!21Major changes. convert nuplan devkit to handle occlusions for project,!18Left turn filter
from collections import deque
from copy import deepcopy
import math
from typing import Dict, List, Optional, Tuple, Type
import numpy as np
......@@ -17,6 +18,8 @@ from nuplan.common.geometry.transform import translate_longitudinally
from nuplan.common.maps.abstract_map import AbstractMap
from nuplan.common.maps.abstract_map_objects import LaneGraphEdgeMapObject
from nuplan.database.utils.measure import angle_diff
from nuplan.planning.scenario_builder.abstract_scenario import AbstractScenario
from nuplan.planning.script.builders.occlusion_manager_builder import build_occlusion_manager
from nuplan.planning.simulation.controller.motion_model.abstract_motion_model import AbstractMotionModel
......@@ -510,7 +513,7 @@ class MLPlannerAgents(AbstractObservation):
ego_state_at_start = scenario.get_ego_state_at_iteration(0)
if self._optimization_cfg.mixed_agent_heading_check:
if abs(ego_state_at_start.rear_axle.heading - agent.center.heading) <= self._optimization_cfg.mixed_agent_heading_check_range:
if abs(angle_diff(ego_state_at_start.rear_axle.heading, agent.center.heading, math.pi*2)) <= self._optimization_cfg.mixed_agent_heading_check_range:
return "idm"
for index in range(scenario.get_number_of_iterations()):
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment