def observe(self, sim): observations = {} rewards = {} scores = {} dones = { agent_id: agent_id not in self.pending_agent_ids for agent_id in self.agent_ids if agent_id not in sim.vehicle_index.agent_vehicle_ids() } for agent_id in self.active_agents: # An agent may be pointing to its own vehicle or observing a social vehicle vehicle_ids = sim.vehicle_index.vehicle_ids_by_actor_id( agent_id, include_shadowers=True) if self.is_boid_agent(agent_id): vehicles = [ sim.vehicle_index.vehicle_by_id(vehicle_id) for vehicle_id in vehicle_ids ] # returns format of {<agent_id>: {<vehicle_id>: {...}}} sensor_states = { vehicle.id: sim.vehicle_index.sensor_state_for_vehicle_id(vehicle.id) for vehicle in vehicles } observations[ agent_id], dones[agent_id] = Sensors.observe_batch( sim, agent_id, sensor_states, {v.id: v for v in vehicles}) rewards[agent_id] = { vehicle_id: self._vehicle_reward(vehicle_id, sim) for vehicle_id in sensor_states.keys() } scores[agent_id] = { format_actor_id(agent_id, vehicle_id, is_multi=True): self._vehicle_score(vehicle_id, sim) for vehicle_id in sensor_states.keys() } else: assert len(vehicle_ids) == 1, ( "Unless this vehicle is part of a boid then we should only have a " f"single vehicle under agent_id={agent_id}\n " f"(vehicle_ids={vehicle_ids})") vehicle = sim.vehicle_index.vehicle_by_id(vehicle_ids[0]) sensor_state = sim.vehicle_index.sensor_state_for_vehicle_id( vehicle.id) observations[agent_id], dones[agent_id] = Sensors.observe( sim, agent_id, sensor_state, vehicle) if sim.vehicle_index.vehicle_is_shadowed(vehicle.id): # It is not a shadowing agent's fault if it is done dones[agent_id] = False else: logging.log( logging.DEBUG, f"Agent `{agent_id}` has raised done with {observations[agent_id].events}", ) rewards[agent_id] = vehicle.trip_meter_sensor(increment=True) scores[agent_id] = vehicle.trip_meter_sensor() return observations, rewards, scores, dones
def observe(self, sim): observations = {} rewards = {} scores = {} dones = { agent_id: agent_id not in self.pending_agent_ids for agent_id in self.agent_ids if agent_id not in sim.vehicle_index.agent_vehicle_ids } for agent_id in self.active_agents: agent_interface = self._agent_interfaces[agent_id] # An agent may be pointing to its own vehicle or observing a social vehicle indices = sim.vehicle_index.vehicle_indices_by_actor_id( agent_id, columns=["vehicle_id", "shadow_actor_id"], ) if self.is_boid_agent(sim, agent_id): vehicles = [ sim.vehicle_index.vehicle_by_id(vehicle_id) for vehicle_id in indices["vehicle_id"] ] # returns format of {<agent_id>: {<vehicle_id>: {...}}} sensor_states = { vehicle.id: sim.vehicle_index.sensor_state_for_vehicle_id(vehicle.id) for vehicle in vehicles } observations[ agent_id], dones[agent_id] = Sensors.observe_batch( sim, agent_id, sensor_states, {v.id: v for v in vehicles}) # XXX: For now we collapse all vehicle dones into a single done dones[agent_id] = any(dones[agent_id].values()) rewards[agent_id] = { vehicle_id: self._vehicle_reward(vehicle_id, sim) for vehicle_id, sensor_state in sensor_states.items() } scores[agent_id] = { format_actor_id(agent_id, vehicle_id, is_multi=True): self._vehicle_score(vehicle_id, sim) for (vehicle_id, sensor_state) in sensor_states.items() } else: assert len(indices["vehicle_id"]) == 1, ( "Unless this vehicle is part of a boid then we should only have a" f"single vehicle under agent_id={agent_id}\n " f"(vehicle_ids={indices['vehicle_id']})") vehicle = sim.vehicle_index.vehicle_by_id( indices["vehicle_id"][0]) sensor_state = sim.vehicle_index.sensor_state_for_vehicle_id( vehicle.id) observations[agent_id], dones[agent_id] = Sensors.observe( sim, agent_id, sensor_state, vehicle) # It is not a shadow agent's fault if it is done if indices["shadow_actor_id"][0] == agent_id: dones[agent_id] = False else: logging.log( logging.DEBUG, f"Agent `{agent_id}` has raised done with {observations[agent_id].events}", ) rewards[agent_id] = vehicle.trip_meter_sensor(increment=True) scores[agent_id] = vehicle.trip_meter_sensor() return observations, rewards, scores, dones
def _try_emit_envision_state(self, provider_state, obs, scores): if not self._envision: return traffic = {} for v in provider_state.vehicles: if v.vehicle_id in self._vehicle_index.agent_vehicle_ids(): # this is an agent controlled vehicle agent_id = self._vehicle_index.actor_id_from_vehicle_id(v.vehicle_id) agent_obs = obs[agent_id] is_boid_agent = self._agent_manager.is_boid_agent(agent_id) vehicle_obs = agent_obs[v.vehicle_id] if is_boid_agent else agent_obs if self._agent_manager.is_ego(agent_id): actor_type = envision_types.TrafficActorType.Agent mission_route_geometry = self._vehicle_index.sensor_state_for_vehicle_id( v.vehicle_id ).mission_planner.route.geometry else: actor_type = envision_types.TrafficActorType.SocialAgent mission_route_geometry = None point_cloud = vehicle_obs.lidar_point_cloud or ([], [], []) point_cloud = point_cloud[0] # (points, hits, rays), just want points # TODO: driven path should be read from vehicle_obs driven_path = self._vehicle_index.vehicle_by_id( v.vehicle_id ).driven_path_sensor() road_waypoints = [] if vehicle_obs.road_waypoints: road_waypoints = [ path for paths in vehicle_obs.road_waypoints.lanes.values() for path in paths ] traffic[v.vehicle_id] = envision_types.TrafficActorState( name=self._agent_manager.agent_name(agent_id), actor_type=actor_type, vehicle_type=envision_types.VehicleType.Car, position=v.pose.position, heading=v.pose.heading, speed=v.speed, actor_id=envision_types.format_actor_id( agent_id, v.vehicle_id, is_multi=is_boid_agent, ), events=vehicle_obs.events, waypoint_paths=(vehicle_obs.waypoint_paths or []) + road_waypoints, point_cloud=point_cloud, driven_path=driven_path, mission_route_geometry=mission_route_geometry, ) elif v.vehicle_id in self._vehicle_index.social_vehicle_ids(): # this is a social vehicle traffic[v.vehicle_id] = envision_types.TrafficActorState( actor_type=envision_types.TrafficActorType.SocialVehicle, vehicle_type=v.vehicle_type, position=list(v.pose.position), heading=v.pose.heading, speed=v.speed, ) bubble_geometry = [ list(bubble.geometry.exterior.coords) for bubble in self._bubble_manager.bubbles ] state = envision_types.State( traffic=traffic, scenario_id=self.scenario.scenario_hash, bubbles=bubble_geometry, scene_colors=SceneColors.EnvisionColors.value, scores=scores, ) self._envision.send(state)
def _try_emit_envision_state(self, provider_state, obs, scores): if not self._envision: return traffic = {} for v in provider_state.vehicles: if v.vehicle_id in self._vehicle_index.agent_vehicle_ids: # this is an agent controlled vehicle agent_id = self._vehicle_index.actor_id_from_vehicle_id( v.vehicle_id) agent_obs = obs[agent_id] # TODO: find a more robust way of telling whether an agent id refers to a boid agent # boid agent observations are a dict mapping vehicle ids to that vehicle's observation is_boid_agent = isinstance(agent_obs, dict) vehicle_obs = agent_obs[ v.vehicle_id] if is_boid_agent else agent_obs if self._agent_manager.is_ego(agent_id): actor_type = envision_types.TrafficActorType.Agent mission_route_geometry = self._vehicle_index.sensor_state_for_vehicle_id( v.vehicle_id).mission_planner.route.geometry else: actor_type = envision_types.TrafficActorType.SocialAgent mission_route_geometry = None point_cloud = vehicle_obs.lidar_point_cloud or ([], [], []) point_cloud = point_cloud[ 0] # (points, hits, rays), just want points # TODO: driven path should be read from vehicle_obs driven_path = self._vehicle_index.vehicle_by_id( v.vehicle_id).driven_path_sensor() traffic[v.vehicle_id] = envision_types.TrafficActorState( name=self._agent_manager.name_for_agent(agent_id), actor_type=actor_type, vehicle_type=envision_types.VehicleType.Car, position=v.pose.position, heading=v.pose.heading, speed=v.speed, actor_id=envision_types.format_actor_id( agent_id, v.vehicle_id, is_multi=is_boid_agent, ), events=vehicle_obs.events, waypoint_paths=vehicle_obs.waypoint_paths or [], point_cloud=point_cloud, driven_path=driven_path, mission_route_geometry=mission_route_geometry, ) elif v.vehicle_id in self._vehicle_index.social_vehicle_ids: # this is a social vehicle vehicle = self._vehicle_index.vehicle_by_id(v.vehicle_id) traffic[v.vehicle_id] = envision_types.TrafficActorState( actor_type=envision_types.TrafficActorType.SocialVehicle, vehicle_type=v.vehicle_type, position=list(v.pose.position), heading=v.pose.heading, speed=v.speed, ) else: self._log.info( "Vehicle is not a social vehicle and not controlled by agent: ", v) bubble_geometry = [ list(bubble.geometry.exterior.coords) for bubble in self._bubble_manager.bubbles ] state = envision_types.State( traffic=traffic, scenario_id=self.scenario.scenario_hash, bubbles=bubble_geometry, scene_colors=SceneColors.EnvisionColors.value, scores=scores, ) self._envision.send(state)