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)
