def test_conversion_sumo(original_pose):
pose_info = [
([2, 0], 90, 4),
([-1, 0], 270, 2),
([5, 5], 45, math.sqrt(50) * 2),
([0, -1.5], 180, 3),
]
# spin around a center point
for offset, angle, length in pose_info:
front_bumper_pos = np.array(
[
original_pose.position[0] + offset[0],
original_pose.position[1] + offset[1],
]
)
p_from_bumper = Pose.from_front_bumper(
front_bumper_pos,
Heading.from_sumo(angle),
length,
)
assert np.isclose(
p_from_bumper.position, original_pose.position, atol=2e-06
).all()
_pose_position, _pose_heading = p_from_bumper.as_sumo(length, Heading(0))
assert math.isclose(
angle,
_pose_heading,
rel_tol=1e-06,
)
assert np.isclose(
np.append(front_bumper_pos, 0),
_pose_position,
rtol=1e-06,
).all()
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 social_spin_on_bumper_cw(step, front_bumper_position, length):
return Pose.from_front_bumper(np.array(front_bumper_position[:2]),
Heading.from_sumo(step), length)
