def init_traps(self, scenario): self._traps.clear() trap_configs: Sequence[TrapConfig] = [] for agent_id in scenario.missions: mission = scenario.missions[agent_id] mission_planner = MissionPlanner(scenario.waypoints, scenario.road_network) if mission is None: mission = mission_planner.random_endless_mission() if not mission.entry_tactic: mission = replace(mission, entry_tactic=default_entry_tactic()) if (not isinstance(mission.entry_tactic, TrapEntryTactic) and mission.entry_tactic): continue mission_planner.plan(mission) trap_config = self._mission2trap(scenario.road_network, mission) trap_configs.append((agent_id, trap_config)) for agent_id, tc in trap_configs: trap = Trap( geometry=tc.zone.to_geometry(scenario.road_network), mission=tc.mission, exclusion_prefixes=tc.exclusion_prefixes, reactivation_time=tc.reactivation_time, remaining_time_to_reactivation=tc.activation_delay, patience=tc.patience, ) self.add_trap_for_agent_id(agent_id, trap)
def test_waypoints_sensor_with_uturn_task(uturn_scenarios): scenario = next(uturn_scenarios) sim = mock.Mock() vehicle = mock.Mock() sim.elapsed_sim_time = 1 sim.timestep_sec = 0.1 nei_vehicle = mock.Mock() nei_vehicle.pose = Pose( position=np.array([93, -59, 0]), orientation=[0, 0, 0, 0], heading_=Heading(0), ) nei_vehicle.speed = 13.8 sim.neighborhood_vehicles_around_vehicle = mock.MagicMock( return_value=[nei_vehicle]) vehicle.pose = Pose( position=np.array([25, -61, 0]), orientation=[0, 0, 0, 0], heading_=Heading(0), ) mission_planner = MissionPlanner(scenario.waypoints, scenario.road_network, AgentBehavior(aggressiveness=3)) mission = scenario.missions[AGENT_ID] mission_planner.plan(mission) mission_planner._task_is_triggered = True sensor = WaypointsSensor(sim, vehicle, mission_planner) waypoints = sensor() assert len(waypoints) == 1
def test_waypoints_sensor_with_cut_in_task(cut_in_scenarios): scenario = next(cut_in_scenarios) sim = mock.Mock() nei_vehicle = mock.Mock() nei_vehicle.speed = 10 sim.elapsed_sim_time = 4 nei_vehicle.pose = Pose( position=np.array([25, -68, 0]), orientation=[0, 0, 0, 0], heading_=Heading(0), ) sim.neighborhood_vehicles_around_vehicle = mock.MagicMock( return_value=[nei_vehicle]) vehicle = mock.Mock() vehicle.pose = Pose( position=np.array([35, -65, 0]), orientation=[0, 0, 0, 0], heading_=Heading(0), ) mission_planner = MissionPlanner(scenario.waypoints, scenario.road_network, AgentBehavior(aggressiveness=3)) mission = scenario.missions[AGENT_ID] mission_planner.plan(mission) sensor = WaypointsSensor(sim, vehicle, mission_planner) waypoints = sensor() assert len(waypoints) == 1
def _airlock_social_vehicle_with_social_agent( self, vehicle_id: str, social_agent_actor: SocialAgentActor, ) -> str: """When airlocked. The social agent will receive observations and execute its policy, however it won't actually operate the vehicle's controller. """ self._log.debug( f"Airlocked vehicle={vehicle_id} with actor={social_agent_actor}") sim = self._sim agent_id = BubbleManager._make_social_agent_id(vehicle_id, social_agent_actor) if agent_id in sim.agent_manager.social_agent_ids: # E.g. if agent is a boid and was being re-used interface = sim.agent_manager.agent_interface_for_agent_id( agent_id) else: social_agent = make_social_agent( locator=social_agent_actor.agent_locator, **social_agent_actor.policy_kwargs, ) interface = social_agent.interface mission_planner = MissionPlanner(sim.scenario.waypoints, sim.scenario.road_network) is_boid = isinstance(social_agent_actor, BoidAgentActor) vehicle = sim.vehicle_index.prepare_for_agent_control(sim, vehicle_id, agent_id, interface, mission_planner, boid=is_boid) # Setup mission (also used for observations) route = sim.traffic_sim.vehicle_route(vehicle_id=vehicle.id) mission = Mission( start=Start(vehicle.position[:2], vehicle.heading), goal=PositionalGoal.fromedge(route[-1], sim.scenario.road_network), ) mission_planner.plan(mission=mission) if agent_id not in sim.agent_manager.social_agent_ids: social_agent_data_model = SocialAgent( id=SocialAgentId.new(social_agent_actor.name), name=social_agent_actor.name, mission=mission, agent_locator=social_agent_actor.agent_locator, policy_kwargs=social_agent_actor.policy_kwargs, initial_speed=social_agent_actor.initial_speed, ) sim.agent_manager.start_social_agent(agent_id, social_agent, social_agent_data_model) return agent_id
def _hijack_vehicle(sim, vehicle_id, agent_id, mission): agent_interface = sim.agent_manager.agent_interface_for_agent_id(agent_id) planner = MissionPlanner(sim.scenario.waypoints, sim.scenario.road_network) planner.plan(mission=mission) # Apply agent vehicle association. sim.vehicle_index.start_agent_observation( sim, vehicle_id, agent_id, agent_interface, planner ) vehicle = sim.vehicle_index.switch_control_to_agent( sim, vehicle_id, agent_id, recreate=True, hijacking=False ) return vehicle
def attach_sensors_to_vehicles(self, sim, agent_interface, vehicle_ids): for sv_id in vehicle_ids: if sv_id in self._vehicle_with_sensors: continue mission_planner = MissionPlanner(sim.scenario.waypoints, sim.scenario.road_network) mission_planner.plan(mission=None) agent_id = f"Agent-{sv_id}" self._vehicle_with_sensors[sv_id] = agent_id self._agent_interfaces[agent_id] = agent_interface sim.vehicle_index.attach_sensors_to_vehicle( sim, sv_id, agent_interface, mission_planner)
def test_waypoints_sensor(scenarios): scenario = next(scenarios) sim = mock.Mock() vehicle = mock.Mock() vehicle.pose = Pose( position=np.array([33, -65, 0]), orientation=[0, 0, 0, 0], heading_=Heading(0), ) mission_planner = MissionPlanner(scenario.waypoints, scenario.road_network) mission = scenario.missions[AGENT_ID] mission_planner.plan(mission) sensor = WaypointsSensor(sim, vehicle, mission_planner) waypoints = sensor() assert len(waypoints) == 3
def _make_vehicle(sim, agent_id, mission): agent_interface = sim.agent_manager.agent_interface_for_agent_id( agent_id) planner = MissionPlanner(sim.scenario.waypoints, sim.scenario.road_network) planner.plan(mission=mission) # 3. Apply agent vehicle association. vehicle = sim.vehicle_index.build_agent_vehicle( sim, agent_id, agent_interface, planner, sim.scenario.vehicle_filepath, sim.scenario.tire_parameters_filepath, True, sim.scenario.surface_patches, sim.scenario.controller_parameters_filepath, ) return vehicle
def init_traps(self, road_network, waypoints, missions): self._traps.clear() for agent_id, mission in missions.items(): mission_planner = MissionPlanner(waypoints, road_network) if mission is None: mission = mission_planner.random_endless_mission() if not mission.entry_tactic: mission = replace(mission, entry_tactic=default_entry_tactic()) if (not isinstance(mission.entry_tactic, TrapEntryTactic) and mission.entry_tactic): continue mission = mission_planner.plan(mission) trap = self._mission2trap(road_network, mission) self.add_trap_for_agent_id(agent_id, trap)
def _prepare_sensors_for_agent_control(self, sim, vehicle_id, agent_id, agent_interface, bubble): mission_planner = MissionPlanner(sim.scenario.road_network) vehicle = sim.vehicle_index.start_agent_observation( sim, vehicle_id, agent_id, agent_interface, mission_planner, boid=bubble.is_boid, ) # Setup mission (also used for observations) route = sim.traffic_sim.vehicle_route(vehicle_id=vehicle.id) mission = Mission( start=Start(vehicle.position[:2], vehicle.heading), goal=PositionalGoal.fromedge(route[-1], sim.scenario.road_network), ) mission_planner.plan(mission=mission)
def _make_vehicle(sim, agent_id, mission, initial_speed): agent_interface = sim.agent_manager.agent_interface_for_agent_id( agent_id) planner = MissionPlanner( sim.scenario.road_network, agent_interface.agent_behavior, ) planner.plan(mission=mission) # 3. Apply agent vehicle association. vehicle = sim.vehicle_index.build_agent_vehicle( sim, agent_id, agent_interface, planner, sim.scenario.vehicle_filepath, sim.scenario.tire_parameters_filepath, True, sim.scenario.surface_patches, sim.scenario.controller_parameters_filepath, initial_speed=initial_speed, boid=False, ) return vehicle
def _add_agent(self, agent_id, agent_interface, agent_model, sim, boid=False, trainable=True): # TODO: Disentangle what is entangled below into: # 1. AgentState initialization, # 2. Agent vehicle initialization, which should live elsewhere, and # 3. Provider related state initialization, which does not belong here. # # A couple of things forced the current unhappy state -- # # 1. SMARTS internal coordinate system should be 100% unified. Coordinate # transformation should happen only at the interface between SMARTS and # its providers. For example, mission start pose should just be vehicle # initial pose. # 2. AgentState should be allowed to fully initialized (setup) without vehicle # information. We currently rely on the creation of the Vehicle instance to # do the coordinate transformation. :-( # 3. Providers should not be creating vehicles. They do need to get notified # about new vehicles entering their territory through the VehicleState # message. But that does not need to happen at Agent instantiation. assert isinstance(agent_id, str) # SUMO expects strings identifiers scenario = sim.scenario mission = scenario.mission(agent_id) planner = MissionPlanner( scenario.waypoints, scenario.road_network, agent_behavior=agent_interface.agent_behavior, ) planner.plan(mission) vehicle = sim.vehicle_index.build_agent_vehicle( sim, agent_id, agent_interface, planner, scenario.vehicle_filepath, scenario.tire_parameters_filepath, trainable, scenario.surface_patches, scenario.controller_parameters_filepath, agent_model.initial_speed, boid=boid, ) matching_providers = [ provider for provider in sim.providers if agent_interface.action_space in provider.action_spaces ] if matching_providers: assert ( len(matching_providers) == 1 ), f"Found {matching_providers} for action space {agent_interface.action_space}" provider = matching_providers[0] provider.create_vehicle( VehicleState( vehicle_id=vehicle.id, vehicle_type=vehicle.vehicle_type, pose=vehicle.pose, dimensions=vehicle.chassis.dimensions, source="NEW-AGENT", )) self._agent_interfaces[agent_id] = agent_interface self._social_agent_data_models[agent_id] = agent_model