def test_lane_change(self): # World Definition params = ParameterServer() world = World(params) # Model Definitions behavior_model = BehaviorMobil(params) execution_model = ExecutionModelInterpolate(params) dynamic_model = SingleTrackModel(params) behavior_model2 = BehaviorIDMLaneTracking(params) execution_model2 = ExecutionModelInterpolate(params) dynamic_model2 = SingleTrackModel(params) # Map Definition map_interface = MapInterface() xodr_map = MakeXodrMapOneRoadTwoLanes() map_interface.SetOpenDriveMap(xodr_map) world.SetMap(map_interface) #agent_2d_shape = CarLimousine() agent_2d_shape = CarRectangle() init_state = np.array([0, 3, -1.75, 0, 5]) agent_params = params.AddChild("agent1") goal_polygon = Polygon2d( [1, 1, 0], [Point2d(0, 0), Point2d(0, 2), Point2d(2, 2), Point2d(2, 0)]) goal_polygon = goal_polygon.Translate(Point2d(50, -2)) agent = Agent(init_state, behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, GoalDefinitionPolygon(goal_polygon), map_interface) world.AddAgent(agent) init_state2 = np.array([0, 15, -1.75, 0, 2]) agent2 = Agent(init_state2, behavior_model2, dynamic_model2, execution_model2, agent_2d_shape, agent_params, GoalDefinitionPolygon(goal_polygon), map_interface) world.AddAgent(agent2) # viewer viewer = MPViewer(params=params, use_world_bounds=True) # World Simulation sim_step_time = params["simulation"]["step_time", "Step-time in simulation", 0.05] sim_real_time_factor = params["simulation"][ "real_time_factor", "execution in real-time or faster", 100] # Draw map for _ in range(0, 10): viewer.clear() world.Step(sim_step_time) viewer.drawWorld(world) viewer.show(block=False) time.sleep(sim_step_time / sim_real_time_factor)
def test_python_behavior_model(self): # World Definition scenario_param_file = "macro_actions_test.json" # must be within examples params folder params = ParameterServer(filename=os.path.join( "modules/world/tests/params/", scenario_param_file)) world = World(params) # Define two behavior models one python one standard c++ model behavior_model = PythonDistanceBehavior(params) execution_model = ExecutionModelInterpolate(params) dynamic_model = SingleTrackModel(params) behavior_model2 = BehaviorConstantVelocity(params) execution_model2 = ExecutionModelInterpolate(params) dynamic_model2 = SingleTrackModel(params) # Define the map interface and load a testing map map_interface = MapInterface() xodr_map = MakeXodrMapOneRoadTwoLanes() map_interface.SetOpenDriveMap(xodr_map) world.SetMap(map_interface) # Define the agent shapes agent_2d_shape = CarRectangle() init_state = np.array([0, 3, -5.25, 0, 20]) # Define the goal definition for agents center_line = Line2d() center_line.AddPoint(Point2d(0.0, -1.75)) center_line.AddPoint(Point2d(100.0, -1.75)) max_lateral_dist = (0.4, 0.5) max_orientation_diff = (0.08, 0.1) velocity_range = (5.0, 20.0) goal_definition = GoalDefinitionStateLimitsFrenet( center_line, max_lateral_dist, max_orientation_diff, velocity_range) # define two agents with the different behavior models agent_params = params.AddChild("agent1") agent = Agent(init_state, behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, map_interface) world.AddAgent(agent) init_state2 = np.array([0, 25, -5.25, 0, 15]) agent2 = Agent(init_state2, behavior_model2, dynamic_model2, execution_model2, agent_2d_shape, agent_params, goal_definition, map_interface) world.AddAgent(agent2) # viewer 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() video_renderer.drawWorld(world) video_renderer.drawGoalDefinition(goal_definition, "red", 0.5, "red") time.sleep(sim_step_time / sim_real_time_factor) video_renderer.export_video(filename="./test_video_intermediate", remove_image_dir=True)
parser = argparse.ArgumentParser(description="Interaction dataset file in bark") parser.add_argument("scenario_file", metavar="scenario_name", type=str, help="name of the scenario parameter file") args = parser.parse_args() scenario_param_file = args.scenario_file # must be within examples params folder param_server = ParameterServer(filename=os.path.join("examples/params/", scenario_param_file)) scenario_generation = InteractionDatasetScenarioGeneration(num_scenarios=1, random_seed=0, params=param_server) #viewer = MPViewer(params=param_server, x_range=[-234 + 1114, -105 + 1114], y_range=[-130 + 1107, -73 + 1107]) #viewer = MPViewer(params=param_server) viewer = MPViewer(params=param_server, use_world_bounds=True) sim_step_time = param_server["simulation"]["step_time", "Step-time used in simulation", 0.2] sim_real_time_factor = param_server["simulation"]["real_time_factor", "execution in real-time or faster", 1] scenario = scenario_generation.create_single_scenario() world_state = scenario.get_world_state() sim_time_steps = param_server["simulation"]["simulation_time_steps", "Number of time steps to simulate", 40] for _ in range(0, sim_time_steps): world_state.DoPlanning(sim_step_time) viewer.drawWorld(world_state, scenario._eval_agent_ids) viewer.show(block=False) viewer.clear() world_state.DoExecution(sim_step_time)
def test_uct_single_agent(self): try: from bark.models.behavior import BehaviorUCTSingleAgentMacroActions except: print("Rerun with --define planner_uct=true") return # World Definition scenario_param_file = "macro_actions_test.json" # must be within examples params folder params = ParameterServer(filename=os.path.join( "modules/world/tests/params/", scenario_param_file)) world = World(params) # Model Definitions behavior_model = BehaviorUCTSingleAgentMacroActions(params) execution_model = ExecutionModelInterpolate(params) dynamic_model = SingleTrackModel(params) behavior_model2 = BehaviorConstantVelocity(params) execution_model2 = ExecutionModelInterpolate(params) dynamic_model2 = SingleTrackModel(params) # Map Definition map_interface = MapInterface() xodr_map = MakeXodrMapOneRoadTwoLanes() map_interface.SetOpenDriveMap(xodr_map) world.SetMap(map_interface) # agent_2d_shape = CarLimousine() agent_2d_shape = CarRectangle() init_state = np.array([0, 3, -5.25, 0, 20]) agent_params = params.AddChild("agent1") # goal_polygon = Polygon2d( # [1, 1, 0], [Point2d(0, 0), Point2d(0, 2), Point2d(2, 2), Point2d(2, 0)]) # goal_definition = GoalDefinitionPolygon(goal_polygon) # goal_polygon = goal_polygon.Translate(Point2d(90, -2)) center_line = Line2d() center_line.AddPoint(Point2d(0.0, -1.75)) center_line.AddPoint(Point2d(100.0, -1.75)) max_lateral_dist = (0.4, 0.5) max_orientation_diff = (0.08, 0.1) velocity_range = (5.0, 20.0) goal_definition = GoalDefinitionStateLimitsFrenet( center_line, max_lateral_dist, max_orientation_diff, velocity_range) agent = Agent(init_state, behavior_model, dynamic_model, execution_model, agent_2d_shape, agent_params, goal_definition, map_interface) world.AddAgent(agent) init_state2 = np.array([0, 25, -5.25, 0, 0]) agent2 = Agent(init_state2, behavior_model2, dynamic_model2, execution_model2, agent_2d_shape, agent_params, goal_definition, map_interface) world.AddAgent(agent2) # viewer 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, 5): world.Step(sim_step_time) viewer.clear() video_renderer.drawWorld(world) video_renderer.drawGoalDefinition(goal_definition) time.sleep(sim_step_time / sim_real_time_factor) video_renderer.export_video(filename="./test_video_intermediate", remove_image_dir=True)