def step(self, provider_actions: Dict, dt,
elapsed_sim_time) -> ProviderState:
"""[summary]
Args:
Returns:
ProviderState: [description]
"""
provider_state = ProviderState()
for vehicle_id, traj in provider_actions.items():
pose, speed = self.perform_trajectory_interpolation(dt, traj)
provider_state.vehicles.append(
VehicleState(
vehicle_id=vehicle_id,
vehicle_config_type="passenger",
pose=pose,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
speed=speed,
source="TrajectoryInterpolation",
))
return provider_state
def _extrapolate_to_now(vs: VehicleState, staleness: float,
lin_acc_slope: float, ang_acc_slope: float):
"""Here we just linearly extrapolate the acceleration to "now" from the previous state for
each vehicle and then use standard kinematics to project the velocity and position from that."""
# The following ~10 lines are a hack b/c I'm too stupid to figure out
# how to do calculus on quaternions...
heading = vs.pose.heading
heading_delta_vec = staleness * (
vs.angular_velocity + 0.5 * vs.angular_acceleration * staleness +
ang_acc_slope * staleness**2 / 6.0)
heading += vec_to_radians(heading_delta_vec[:2]) + (0.5 * math.pi)
heading %= 2 * math.pi
vs.pose.orientation = fast_quaternion_from_angle(heading)
# XXX: also need to remove the cached heading_ since we've changed orientation
vs.pose.heading_ = None
# I assume the following should be updated based on changing
# heading from above, but I'll leave that for now...
vs.pose.position += staleness * (
vs.linear_velocity + 0.5 * vs.linear_acceleration * staleness +
lin_acc_slope * staleness**2 / 6.0)
vs.linear_velocity += staleness * (vs.linear_acceleration +
0.5 * lin_acc_slope * staleness)
vs.speed = np.linalg.norm(vs.linear_velocity)
vs.angular_velocity += staleness * (vs.angular_acceleration +
0.5 * ang_acc_slope * staleness)
vs.linear_acceleration += staleness * lin_acc_slope
vs.angular_acceleration += staleness * ang_acc_slope
def _hijack_social_vehicle_with_social_agent(
self, sim, vehicle_id: str, social_agent_actor: SocialAgentActor,
) -> str:
"""Upon hijacking the social agent is now in control of the vehicle. It will
initialize the vehicle chassis (and by extension the controller) with a
"greatest common denominator" state; that is: what's available via the vehicle
front-end common to both source and destination policies during airlock.
"""
self._log.debug(f"Hijack vehicle={vehicle_id} with actor={social_agent_actor}")
agent_id = BubbleManager._make_social_agent_id(vehicle_id, social_agent_actor)
is_boid = isinstance(social_agent_actor, BoidAgentActor)
vehicle = sim.vehicle_index.switch_control_to_agent(
sim, vehicle_id, agent_id, boid=is_boid, recreate=False
)
self._update_cursor(vehicle_id, vehicle=vehicle)
for provider in sim.providers:
if (
sim.agent_manager.agent_interface_for_agent_id(agent_id).action_space
in provider.action_spaces
):
provider.create_vehicle(
VehicleState(
vehicle_id=vehicle_id,
vehicle_type="passenger",
pose=vehicle.pose,
dimensions=vehicle.chassis.dimensions,
speed=vehicle.speed,
source="HIJACK",
)
)
def test_we_reach_target_pose_at_given_time(motion_planner_provider, loop_scenario):
motion_planner_provider.setup(loop_scenario)
# we sync with the empty provider state since we don't have any other active providers
motion_planner_provider.sync(ProviderState())
motion_planner_provider.create_vehicle(
VehicleState(
vehicle_id="EGO",
vehicle_type="passenger",
pose=Pose.from_center([0, 0, 0.5], heading=Heading(0)),
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
speed=0,
source="TESTS",
)
)
target_position = [32, -12]
target_heading = math.pi * 0.5
dt = 1.0
elapsed_sim_time = 0
for i in range(10):
t = 10 - i
provider_state = motion_planner_provider.step(
dt=dt,
target_poses_at_t={"EGO": np.array([*target_position, target_heading, t])},
elapsed_sim_time=elapsed_sim_time,
)
elapsed_sim_time += dt
assert len(provider_state.vehicles) == 1
ego_vehicle = provider_state.vehicles[0]
position, heading = ego_vehicle.pose.position, ego_vehicle.pose.heading
assert np.linalg.norm(position[:2] - np.array(target_position)) < 1e-16
assert np.isclose(heading, target_heading)
def provider_vehicle(position, heading, speed):
return VehicleState(
vehicle_id="sv-132",
vehicle_type="truck",
pose=Pose.from_center(position, heading),
dimensions=BoundingBox(length=3, width=1, height=2),
speed=speed,
source="TESTS",
)
def override_next_provider_state(self, vehicles: Sequence):
self._next_provider_state = ProviderState(vehicles=[
VehicleState(
vehicle_id=vehicle_id,
vehicle_type="passenger",
pose=pose,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
speed=speed,
source="MOCK",
) for vehicle_id, pose, speed in vehicles
], )
def _entity_to_vs(entity: EntityState) -> VehicleState:
veh_id = entity.entity_id
veh_type = ROSDriver._decode_entity_type(entity.entity_type)
veh_dims = Dimensions(entity.length, entity.width, entity.height)
vs = VehicleState(
source="EXTERNAL",
vehicle_id=veh_id,
vehicle_config_type=veh_type,
pose=Pose(
ROSDriver._xyz_to_vect(entity.pose.position),
ROSDriver._xyzw_to_vect(entity.pose.orientation),
),
dimensions=veh_dims,
linear_velocity=ROSDriver._xyz_to_vect(entity.velocity.linear),
angular_velocity=ROSDriver._xyz_to_vect(entity.velocity.angular),
linear_acceleration=ROSDriver._xyz_to_vect(
entity.acceleration.linear),
angular_acceleration=ROSDriver._xyz_to_vect(
entity.acceleration.angular),
)
vs.set_privileged()
vs.speed = np.linalg.norm(vs.linear_velocity)
return vs
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
def _compute_traffic_vehicles(self) -> List[VehicleState]:
sub_results = self._traci_conn.simulation.getSubscriptionResults()
if sub_results is None or sub_results == {}:
return {}
# New social vehicles that have entered the map
newly_departed_sumo_traffic = [
vehicle_id
for vehicle_id in sub_results[tc.VAR_DEPARTED_VEHICLES_IDS]
if vehicle_id not in self._non_sumo_vehicle_ids
]
reserved_areas = [
position for position in self._reserved_areas.values()
]
# Subscribe to all vehicles to reduce repeated traci calls
for vehicle_id in newly_departed_sumo_traffic:
self._traci_conn.vehicle.subscribe(
vehicle_id,
[
tc.VAR_POSITION, # Decimal=66, Hex=0x42
tc.VAR_ANGLE, # Decimal=67, Hex=0x43
tc.VAR_SPEED, # Decimal=64, Hex=0x40
tc.VAR_VEHICLECLASS, # Decimal=73, Hex=0x49
tc.VAR_ROUTE_INDEX, # Decimal=105, Hex=0x69
tc.VAR_EDGES, # Decimal=84, Hex=0x54
tc.VAR_TYPE, # Decimal=79, Hex=0x4F
tc.VAR_LENGTH, # Decimal=68, Hex=0x44
tc.VAR_WIDTH, # Decimal=77, Hex=0x4d
],
)
sumo_vehicle_state = self._traci_conn.vehicle.getAllSubscriptionResults(
)
for vehicle_id in newly_departed_sumo_traffic:
other_vehicle_shape = self._shape_of_vehicle(
sumo_vehicle_state, vehicle_id)
violates_reserved_area = False
for reserved_area in reserved_areas:
if reserved_area.intersects(other_vehicle_shape):
violates_reserved_area = True
break
if violates_reserved_area:
self._traci_conn.vehicle.remove(vehicle_id)
sumo_vehicle_state.pop(vehicle_id)
continue
self._log.debug("SUMO vehicle %s entered simulation", vehicle_id)
# Non-sumo vehicles will show up the step after the sync where the non-sumo vehicle is
# added.
newly_departed_non_sumo_vehicles = [
vehicle_id
for vehicle_id in sub_results[tc.VAR_DEPARTED_VEHICLES_IDS]
if vehicle_id not in newly_departed_sumo_traffic
]
for vehicle_id in newly_departed_non_sumo_vehicles:
if vehicle_id in self._reserved_areas:
del self._reserved_areas[vehicle_id]
self._sumo_vehicle_ids = (set(sumo_vehicle_state.keys()) -
self._non_sumo_vehicle_ids)
provider_vehicles = []
# batched conversion of positions to numpy arrays
front_bumper_positions = np.array([
sumo_vehicle[tc.VAR_POSITION]
for sumo_vehicle in sumo_vehicle_state.values()
]).reshape(-1, 2)
for i, (sumo_id,
sumo_vehicle) in enumerate(sumo_vehicle_state.items()):
# XXX: We can safely rely on iteration order over dictionaries being
# stable on py3.7.
# See: https://www.python.org/downloads/release/python-370/
# "The insertion-order preservation nature of dict objects is now an
# official part of the Python language spec."
front_bumper_pos = front_bumper_positions[i]
heading = Heading.from_sumo(sumo_vehicle[tc.VAR_ANGLE])
speed = sumo_vehicle[tc.VAR_SPEED]
vehicle_config_type = sumo_vehicle[tc.VAR_VEHICLECLASS]
dimensions = VEHICLE_CONFIGS[vehicle_config_type].dimensions
provider_vehicles.append(
VehicleState(
# XXX: In the case of the SUMO traffic provider, the vehicle ID is
# the sumo ID is the actor ID.
vehicle_id=sumo_id,
vehicle_config_type=vehicle_config_type,
pose=Pose.from_front_bumper(front_bumper_pos, heading,
dimensions.length),
dimensions=dimensions,
speed=speed,
source="SUMO",
))
return provider_vehicles
def step(
self,
sim,
):
captures_by_agent_id = defaultdict(list)
# Do an optimization to only check if there are pending agents.
if not sim.agent_manager.pending_agent_ids:
return
social_vehicle_ids = sim.vehicle_index.social_vehicle_ids
vehicles = {
v_id: sim.vehicle_index.vehicle_by_id(v_id)
for v_id in social_vehicle_ids
}
existing_agent_vehicles = (
sim.vehicle_index.vehicle_by_id(v_id)
for v_id in sim.vehicle_index.agent_vehicle_ids)
def largest_vehicle_plane_dimension(vehicle):
return max(*vehicle.chassis.dimensions.as_lwh[:2])
agent_vehicle_comp = [(v.position[:2],
largest_vehicle_plane_dimension(v), v)
for v in existing_agent_vehicles]
for agent_id in sim.agent_manager.pending_agent_ids:
trap = self._traps[agent_id]
if trap is None:
continue
trap.step_trigger(sim.timestep_sec)
if not trap.ready:
continue
# Order vehicle ids by distance.
sorted_vehicle_ids = sorted(
list(social_vehicle_ids),
key=lambda v: squared_dist(vehicles[v].position[:2], trap.
mission.start.position),
)
for v_id in sorted_vehicle_ids:
vehicle = vehicles[v_id]
point = Point(vehicle.position)
if any(
v.startswith(prefix)
for prefix in trap.exclusion_prefixes):
continue
if not point.within(trap.geometry):
continue
captures_by_agent_id[agent_id].append((
v_id,
trap,
replace(
trap.mission,
start=Start(vehicle.position[:2],
vehicle.pose.heading),
),
))
# TODO: Resolve overlap using a tree instead of just removing.
social_vehicle_ids.remove(v_id)
break
# Use fed in trapped vehicles.
agents_given_vehicle = set()
used_traps = []
for agent_id in sim._agent_manager.pending_agent_ids:
if agent_id not in self._traps:
continue
trap = self._traps[agent_id]
captures = captures_by_agent_id[agent_id]
if not trap.ready:
continue
vehicle = None
if len(captures) > 0:
vehicle_id, trap, mission = rand.choice(captures)
vehicle = TrapManager._hijack_vehicle(sim, vehicle_id,
agent_id, mission)
elif trap.patience_expired:
if len(agent_vehicle_comp) > 0:
agent_vehicle_comp.sort(key=lambda v: squared_dist(
v[0], trap.mission.start.position))
# Make sure there is not an agent vehicle in the same location
pos, largest_dimension, _ = agent_vehicle_comp[0]
if (squared_dist(pos, trap.mission.start.position) <
largest_dimension):
continue
vehicle = TrapManager._make_vehicle(sim, agent_id,
trap.mission)
else:
continue
if vehicle == None:
continue
agents_given_vehicle.add(agent_id)
used_traps.append((agent_id, trap))
for provider in sim.providers:
if (sim.agent_manager.agent_interface_for_agent_id(
agent_id).action_space in provider.action_spaces):
provider.create_vehicle(
VehicleState(
vehicle_id=vehicle.id,
vehicle_type="passenger",
pose=vehicle.pose,
dimensions=vehicle.chassis.dimensions,
speed=vehicle.speed,
source="EGO-HIJACK",
))
if len(agents_given_vehicle) > 0:
self.reset_traps(used_traps)
sim.agent_manager.remove_pending_agent_ids(agents_given_vehicle)
def run(
client,
traffic_sim: SumoTrafficSimulation,
plane_body_id,
n_steps=1e6,
):
prev_friction_sum = None
scenario = next(
Scenario.variations_for_all_scenario_roots(
["scenarios/loop"], agents_to_be_briefed=["007"]))
previous_provider_state = traffic_sim.setup(scenario)
traffic_sim.sync(previous_provider_state)
previous_vehicle_ids = set()
vehicles = dict()
passenger_dimen = VEHICLE_CONFIGS["passenger"].dimensions
for step in range(n_steps):
if not client.isConnected():
print("Client got disconnected")
return
injected_poses = [
social_spin_on_bumper_cw(step * 0.1, [8, 6, 0],
passenger_dimen.length),
# social_spin_on_centre_ccw(step * 0.1, [8, 0, passenger_dimen[2] / 2]),
# social_spin_on_axle_cw(
# step * 0.1, [0, 0, 0], [2 * passenger_dimen[0], 0, 0]
# ),
# Pose(
# [0, -6, passenger_dimen[2] * 0.5],
# fast_quaternion_from_angle(Heading(0)),
# ),
]
current_provider_state = traffic_sim.step(0.01)
for pose, i in zip(injected_poses, range(len(injected_poses))):
converted_to_provider = VehicleState(
vehicle_id=f"EGO{i}",
vehicle_type="passenger",
pose=pose,
dimensions=passenger_dimen,
speed=0,
source="TESTS",
)
current_provider_state.vehicles.append(converted_to_provider)
traffic_sim.sync(current_provider_state)
current_vehicle_ids = {
v.vehicle_id
for v in current_provider_state.vehicles
}
vehicle_ids_removed = previous_vehicle_ids - current_vehicle_ids
vehicle_ids_added = current_vehicle_ids - previous_vehicle_ids
for v_id in vehicle_ids_added:
pose = Pose.from_center([0, 0, 0], Heading(0))
vehicles[v] = Vehicle(
id=v_id,
pose=pose,
chassis=BoxChassis(
pose=pose,
speed=0,
dimensions=vehicle_config.dimensions,
bullet_client=client,
),
)
# Hide any additional vehicles
for v in vehicle_ids_removed:
veh = vehicles.pop(v, None)
veh.teardown()
for pv in current_provider_state.vehicles:
vehicles[pv.vehicle_id].control(pv.pose, pv.speed)
client.stepSimulation()
look_at(client, tuple([0, 0, 0]), top_down=False)
previous_vehicle_ids = current_vehicle_ids
traffic_sim.teardown()
