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 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())
[Point2d(-1, -1), Point2d(-1, 1), Point2d(1, 1), Point2d(1, -1)]) start_polygon = polygon.Translate(start_p) goal_polygon = polygon.Translate(end_p) rc = map_interface.GenerateRoadCorridor(start_p, goal_polygon) if rc: roads = rc.roads road_ids = list(roads.keys()) print(road_ids, rc.road_ids) viewer.drawWorld(world) viewer.drawRoadCorridor(rc, "blue") viewer.saveFig(output_dir + "/" + "roadcorridor_" + str(cnt) + ".png") viewer.show() viewer.clear() for idx, lane_corridor in enumerate(rc.lane_corridors): viewer.drawWorld(world) viewer.drawLaneCorridor(lane_corridor, "green") viewer.drawLine2d(lane_corridor.left_boundary, color="red") viewer.drawLine2d(lane_corridor.right_boundary, color="green") viewer.drawLine2d(lane_corridor.center_line, color="green") viewer.drawPolygon2d(start_polygon, color="green", facecolor="green", alpha=1.) viewer.drawPolygon2d(goal_polygon, color="red", facecolor="red",
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)