def test_database_multiprocessing_history(self): dbs = DatabaseSerializer(test_scenarios=4, test_world_steps=5, num_serialize_scenarios=2) dbs.process("data/database1") local_release_filename = dbs.release(version="test") db = BenchmarkDatabase(database_root=local_release_filename) evaluators = {"success" : "EvaluatorGoalReached", "collision" : "EvaluatorCollisionEgoAgent", "max_steps": "EvaluatorStepCount"} terminal_when = {"collision" :lambda x: x, "max_steps": lambda x : x>2} params = ParameterServer() # only for evaluated agents not passed to scenario! behaviors_tested = {"IDM": BehaviorIDMClassic(params), "Const" : BehaviorConstantAcceleration(params)} benchmark_runner = BenchmarkRunnerMP(benchmark_database=db, evaluators=evaluators, terminal_when=terminal_when, behaviors=behaviors_tested, log_eval_avg_every=10) rst = benchmark_runner.run(maintain_history=True) self.assertEqual(len(rst.get_histories()), 2*2*2) rst = benchmark_runner.run_benchmark_config(3, viewer=None, maintain_history=True) scenario_history = rst.get_histories()[3] print(scenario_history) params = ParameterServer() viewer = MPViewer( params=params, x_range=[5060, 5160], y_range=[5070,5150], use_world_bounds=True) viewer.drawWorld(world=scenario_history[1].GetWorldState(), eval_agent_ids=scenario_history[1].eval_agent_ids) viewer.show(block=True)
def _test_scenario(self, scenario_idx, num_steps, visualize, viewer): logging.info("Running scenario {} of {} in set {}".format(scenario_idx, self._scenario_generator.num_scenarios, self._set_name)) try: scenario = self._scenario_generator.get_scenario(scenario_idx) except Exception as e: logging.error("Deserialization failed with {}.".format(e)) return False try: world_state = scenario.GetWorldState() except Exception as e: logging.error("Get world state failed with {}.".format(e)) return False if visualize: if not viewer: viewer = MPViewer( params=ParameterServer(), x_range=[5060, 5160], y_range=[5070, 5150], use_world_bounds=True) sim_step_time = 0.2 sim_real_time_factor = 1 try: for _ in range(0, num_steps): # run a few steps for each scenario if visualize: info_text = "SetName: {} | ScenarioIdx: {}".format( self._set_name, scenario_idx) viewer.drawText(position=(0.5, 1.05), text=info_text) viewer.drawWorld( world_state, scenario._eval_agent_ids, scenario_idx=scenario_idx) viewer.show(block=False) time.sleep(sim_step_time/sim_real_time_factor) viewer.clear() world_state.Step(sim_step_time) return True except Exception as e: logging.error("Simulation failed with {}.".format(e)) return False
def SimulateWorld(self, world, local_tracer, N=5, **kwargs): """Simulates the world for N steps.""" self.ml_behavior.set_action_externally = False eval_id = self._scenario._eval_agent_ids[0] self._world.agents[eval_id].behavior_model = self.ml_behavior replaced_agent_id = kwargs.get("replaced_agent", 0) if replaced_agent_id not in self._cf_axs and self._visualize_cf_worlds: self._cf_axs[replaced_agent_id] = {"ax": plt.subplots(3, 1, constrained_layout=True)[1], "count": 0} for i in range(0, N): if i == N - 1 and kwargs.get("num_virtual_world", 0) is not None and \ self._visualize_cf_worlds and replaced_agent_id is not None: # NOTE: outsource for ftype in [".png", ".pgf"]: viewer = MPViewer( params=self._params, x_range=[-35, 35], y_range=[-35, 35], follow_agent_id=True, axis=self._cf_axs[replaced_agent_id]["ax"][self._cf_axs[replaced_agent_id]["count"]]) # se for agent_id in world.agents.keys(): viewer.agent_color_map[agent_id] = "gray" viewer.agent_color_map[replaced_agent_id] = (127/255, 205/255, 187/255) viewer.agent_color_map[eval_id] = (34/255, 94/255, 168/255) if replaced_agent_id == 1: viewer.drawWorld( world, eval_agent_ids=self._scenario._eval_agent_ids, filename=self._results_folder + "cf_%03d_replaced_" % self._count + str(replaced_agent_id)+ftype, debug_text=False) self._cf_axs[replaced_agent_id]["count"] += 1 observed_world = world.Observe([eval_id])[0] eval_state = observed_world.Evaluate() agent_states = CaptureAgentStates(observed_world) eval_state = {**eval_state, **agent_states} # TODO: break at collision local_tracer.Trace(eval_state, **kwargs) if eval_state["collision"] or eval_state["drivable_area"]: break world.Step(self._step_time) self.ml_behavior.set_action_externally = True
def test_dr_chn_merging(self): # threeway_intersection xodr_parser = XodrParser( os.path.join( os.path.dirname(__file__), "../../../runtime/tests/data/DR_CHN_Merging_ZS_partial_v02.xodr" )) # World Definition params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.map) world.SetMap(map_interface) roads = [0, 1] driving_direction = XodrDrivingDirection.forward map_interface.GenerateRoadCorridor(roads, driving_direction) road_corridor = map_interface.GetRoadCorridor(roads, driving_direction) # Draw map viewer = MPViewer(params=params, use_world_bounds=True) viewer.drawWorld(world) viewer.drawPolygon2d(road_corridor.lane_corridors[0].polygon, color="blue", alpha=0.5) viewer.drawPolygon2d(road_corridor.lane_corridors[1].polygon, color="blue", alpha=0.5) viewer.drawPolygon2d(road_corridor.lane_corridors[2].polygon, color="blue", alpha=0.5) viewer.show(block=False) self.assertTrue(road_corridor.lane_corridors[0].polygon.Valid()) self.assertTrue(road_corridor.lane_corridors[1].polygon.Valid()) self.assertTrue(road_corridor.lane_corridors[2].polygon.Valid()) self.assertTrue(road_corridor.polygon.Valid())
def test_curved_road(self): params = ParameterServer() world = World(params) xodr_map = MakeXodrMapCurved(50, 0.1) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_map) world.SetMap(map_interface) roads = [100] driving_direction = XodrDrivingDirection.forward map_interface.GenerateRoadCorridor(roads, driving_direction) road_corr = map_interface.GetRoadCorridor(roads, driving_direction) viewer = MPViewer(params=params, use_world_bounds=True) viewer.drawWorld(world) viewer.drawRoadCorridor(road_corr) #viewer.show(block=True) # if this is not here, the second unit test is not executed (maybe parsing takes too long?) time.sleep(1)
def test_two_roads_one_lane(self): params = ParameterServer() world = World(params) xodr_map = MakeXodrMapOneRoadTwoLanes() map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_map) world.SetMap(map_interface) start_point = Point2d(0, -11) lanes_near_start = map_interface.find_nearest_lanes(start_point, 1) assert(len(lanes_near_start) == 1) goal_point = Point2d(-191.789, -50.1725) lanes_near_goal = map_interface.find_nearest_lanes(goal_point, 1) assert(len(lanes_near_goal) == 1) viewer = MPViewer(params=params, use_world_bounds=True) viewer.drawWorld(world) # if this is not here, the second unit test is not executed (maybe parsing takes too long?) time.sleep(2)
ego_agent = get_ego_agent(world) apply_action_to_ego_agent(world, 0) # applies action with idx 0 (keep velocity) params = ParameterServer() viewer = MPViewer(params=params, use_world_bounds=True) # World Simulation sim_step_time = params["simulation"]["step_time", "Step-time in simulation", 0.2] sim_real_time_factor = params["simulation"]["real_time_factor", "execution in real-time or faster", 1] # Draw map video_renderer = VideoRenderer(renderer=viewer, world_step_time=sim_step_time) for _ in range(0, 20): world.Step(sim_step_time) viewer.clear() viewer.drawWorld(world) viewer.drawGoalDefinition(ego_agent.goal_definition, "red", 0.5, "red") print(ego_agent.state) # applies action with idx 1 (break) apply_action_to_ego_agent(world, 1) time.sleep(sim_step_time / sim_real_time_factor)
def test_road_corridor_forward(self): xodr_parser = XodrParser( os.path.join( os.path.dirname(__file__), "../../../runtime/tests/data/road_corridor_test.xodr")) # World Definition params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.map) world.SetMap(map_interface) open_drive_map = world.map.GetOpenDriveMap() viewer = MPViewer(params=params, use_world_bounds=True) # Draw map viewer.drawWorld(world) viewer.show(block=False) # Generate RoadCorridor roads = [0, 1, 2] driving_direction = XodrDrivingDirection.forward map_interface.GenerateRoadCorridor(roads, driving_direction) road_corridor = map_interface.GetRoadCorridor(roads, driving_direction) # Assert road corridor # Assert: 3 roads self.assertEqual(len(road_corridor.roads), 3) # Assert: road1: 2 lanes, road2: 1 lane, road3: 1 lane self.assertEqual(len(road_corridor.GetRoad(0).lanes), 3) self.assertEqual(len(road_corridor.GetRoad(1).lanes), 2) self.assertEqual(len(road_corridor.GetRoad(2).lanes), 3) # Assert: next road self.assertEqual(road_corridor.GetRoad(0).next_road.road_id, 1) self.assertEqual(road_corridor.GetRoad(1).next_road.road_id, 2) # Assert: lane links self.assertEqual( road_corridor.GetRoad(0).GetLane(3).next_lane.lane_id, 5) self.assertEqual( road_corridor.GetRoad(1).GetLane(5).next_lane.lane_id, 8) # Assert: LaneCorridor self.assertEqual(len(road_corridor.lane_corridors), 3) colors = ["blue", "red", "green"] count = 0 for lane_corridor in road_corridor.lane_corridors: viewer.drawPolygon2d(lane_corridor.polygon, color=colors[count], alpha=0.5) viewer.drawLine2d(lane_corridor.left_boundary, color="red") viewer.drawLine2d(lane_corridor.right_boundary, color="blue") viewer.drawLine2d(lane_corridor.center_line, color="black") viewer.show(block=False) plt.pause(2.) count += 1
def test_dr_deu_merging_centered(self): # threeway_intersection xodr_parser = XodrParser( os.path.join( os.path.dirname(__file__), "../../../runtime/tests/data/DR_DEU_Merging_MT_v01_centered.xodr" )) # World Definition params = ParameterServer() world = World(params) map_interface = MapInterface() map_interface.SetOpenDriveMap(xodr_parser.map) world.SetMap(map_interface) roads = [0, 1] driving_direction = XodrDrivingDirection.forward map_interface.GenerateRoadCorridor(roads, driving_direction) road_corridor = map_interface.GetRoadCorridor(roads, driving_direction) # Draw map viewer = MPViewer(params=params, use_world_bounds=True) viewer.drawWorld(world) viewer.drawPolygon2d(road_corridor.lane_corridors[0].polygon, color="blue", alpha=0.5) viewer.drawPolygon2d(road_corridor.lane_corridors[1].polygon, color="blue", alpha=0.5) viewer.show(block=False) self.assertTrue(road_corridor.lane_corridors[0].polygon.Valid()) self.assertTrue(road_corridor.lane_corridors[1].polygon.Valid()) self.assertTrue(road_corridor.polygon.Valid()) tol = 0.2 center_line_array = road_corridor.lane_corridors[ 0].center_line.ToArray() left_boundary_array = road_corridor.lane_corridors[ 0].left_boundary.ToArray() right_boundary_array = road_corridor.lane_corridors[ 0].right_boundary.ToArray() # beginning of left lane self.assertAlmostEquals(center_line_array[0, 0], 106.4, 1) self.assertAlmostEquals(center_line_array[0, 1], 103.47, 1) self.assertAlmostEquals( Distance( Point2d(left_boundary_array[0, 0], left_boundary_array[0, 1]), Point2d(right_boundary_array[0, 0], right_boundary_array[0, 1])), 2.8, 1) # end of left lane self.assertAlmostEquals(center_line_array[-1, 0], -18.15, 1) self.assertAlmostEquals(center_line_array[-1, 1], 109.0, 1) self.assertAlmostEquals( Distance( Point2d(left_boundary_array[-1, 0], left_boundary_array[-1, 1]), Point2d(right_boundary_array[-1, 0], right_boundary_array[-1, 1])), 2.63, 1)