def test_collision_joust(bullet_client: bc.BulletClient):
"""Run two agents at each other to test for clipping."""
white_knight_chassis = AckermannChassis(
Pose.from_center([10, 0, 0], Heading(math.pi * 0.5)), bullet_client)
black_knight_chassis = AckermannChassis(
Pose.from_center([-10, 0, 0], Heading(-math.pi * 0.5)), bullet_client)
wkc_collisions, bkc_collisions = _joust(white_knight_chassis,
black_knight_chassis,
steps=10000)
assert len(wkc_collisions) > 0
assert len(bkc_collisions) > 0
assert white_knight_chassis.bullet_id in [
c.bullet_id for c in bkc_collisions
]
assert black_knight_chassis.bullet_id in [
c.bullet_id for c in wkc_collisions
]
def _pose_from_ros(ros_pose) -> Pose:
return Pose(
position=(ros_pose.position.x, ros_pose.position.y,
ros_pose.position.z),
orientation=(
ros_pose.orientation.x,
ros_pose.orientation.y,
ros_pose.orientation.z,
ros_pose.orientation.w,
),
)
def step_with_pose_delta(bv: BoxChassis, steps, pose_delta: np.ndarray,
speed: float):
collisions = []
for _ in range(steps):
cur_pose = bv.pose
new_pose = Pose.from_center(cur_pose.position + pose_delta,
cur_pose.heading)
bv.control(new_pose, speed)
bv._client.stepSimulation()
collisions.extend(bv.contact_points)
return collisions
def update_vehicle_state(self):
"""Update this vehicle state."""
assert len(self.vector) == 20
self.vs.pose = Pose.from_center(
self.vector[1:4], Heading(self.vector[4] % (2 * math.pi)))
self.vs.dimensions = Dimensions(*self.vector[5:8])
self.linear_velocity = self.vector[8:11]
self.angular_velocity = self.vector[11:14]
self.linear_acceleration = self.vector[14:17]
self.angular_acceleration = self.vector[17:]
self.vs.speed = np.linalg.norm(self.linear_velocity)
def test_collision(bullet_client: bc.BulletClient):
"""Spawn overlap to check for the most basic collision"""
chassis = AckermannChassis(
Pose.from_center([0, 0, 0], Heading(-math.pi * 0.5)), bullet_client)
b_chassis = BoxChassis(
Pose.from_center([0, 0, 0], Heading(0)),
speed=0,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
bullet_client=chassis._client,
)
collisions = step_with_vehicle_commands(chassis, steps=2)
assert len(collisions) > 0
collided_bullet_ids = set([c.bullet_id for c in collisions])
GROUND_ID = 0
assert b_chassis.bullet_id in collided_bullet_ids
assert chassis.bullet_id not in collided_bullet_ids
assert GROUND_ID not in collided_bullet_ids
def pose(self) -> Pose:
pos, orn = self._client.getBasePositionAndOrientation(self._bullet_id)
heading = Heading(yaw_from_quaternion(orn))
# NOTE: we're inefficiently creating a new Pose object on every call here,
# but it's too risky to change this because our clients now rely on this behavior.
return Pose.from_explicit_offset(
[0, 0, 0],
np.array(pos),
heading,
local_heading=Heading(0),
)
def pose(self) -> Pose:
pos, orn = self._client.getBasePositionAndOrientation(self._bullet_id)
heading = Heading(yaw_from_quaternion(orn))
pose = Pose.from_explicit_offset(
[0, 0, 0],
np.array(pos),
heading,
local_heading=Heading(0),
)
return pose
def perform_trajectory_interpolation(
timestep_sec,
trajectory: np.ndarray,
):
"""Move vehicle by trajectory interpolation.
Trajectory mentioned here has 5 dimensions, which are TIME, X, Y, THETA and VEL.
TIME indicate
If you want vehicle stop at a specific pose,
trajectory[TrajectoryWithTime.TIME_INDEX][0] should be set as numpy.inf
Args:
sim : reference of smarts instance
vehicle : vehicle to be controlled
trajectory (np.ndarray): trajectory with time
"""
TrajectoryInterpolationProvider.is_legal_trajectory(trajectory)
ms0, ms1 = TrajectoryInterpolationProvider.locate_motion_state(
trajectory, timestep_sec)
speed = 0.0
pose = []
if math.isinf(ms0[TrajectoryWithTime.TIME_INDEX]) or math.isinf(
ms1[TrajectoryWithTime.TIME_INDEX]):
center_position = ms0[TrajectoryWithTime.
X_INDEX:TrajectoryWithTime.Y_INDEX + 1]
center_heading = Heading(ms0[TrajectoryWithTime.THETA_INDEX])
pose = Pose.from_center(center_position, center_heading)
speed = 0.0
else:
ms = TrajectoryInterpolationProvider.interpolate(
ms0, ms1, timestep_sec)
center_position = ms[TrajectoryWithTime.
X_INDEX:TrajectoryWithTime.Y_INDEX + 1]
center_heading = Heading(ms[TrajectoryWithTime.THETA_INDEX])
pose = Pose.from_center(center_position, center_heading)
speed = ms[TrajectoryWithTime.VEL_INDEX]
return pose, speed
def vehicle(bullet_client, TIMESTEP_SEC=time_step):
pose = Pose.from_center((0, 0, 0), Heading(0))
vehicle1 = Vehicle(
id="vehicle",
pose=pose,
showbase=mock.MagicMock(),
chassis=AckermannChassis(
pose=pose,
bullet_client=bullet_client,
),
)
return vehicle1
def test_vehicle_spawned_in_bubble_is_not_captured(smarts, mock_provider):
# Spawned inside bubble, didn't "drive through" airlocking region, so should _not_
# get captured
vehicle_id = "vehicle"
for x in range(20):
mock_provider.override_next_provider_state(vehicles=[(
vehicle_id,
Pose.from_center((100 + x, 0, 0), Heading(-math.pi / 2)),
10, # speed
)])
smarts.step({})
assert not smarts.vehicle_index.vehicle_is_hijacked(vehicle_id)
def _pose_from_ros(ros_pose) -> Pose:
return Pose(
position=np.array([
ros_pose.position.x, ros_pose.position.y, ros_pose.position.z
]),
orientation=np.array([
ros_pose.orientation.x,
ros_pose.orientation.y,
ros_pose.orientation.z,
ros_pose.orientation.w,
]),
)
def vehicle(bullet_client, vehicle_controller_file, timestep_sec=time_step):
pose = Pose.from_center((0, 0, 0), Heading(0))
vehicle1 = Vehicle(
id="vehicle",
pose=pose,
chassis=AckermannChassis(
pose=pose,
bullet_client=bullet_client,
vehicle_filepath=vehicle_controller_file[0],
controller_parameters=vehicle_controller_file[1],
),
)
return vehicle1
def test_collision_collide_with_standing_vehicle(
bullet_client: bc.BulletClient):
"""Run a vehicle at a standing vehicle as fast as possible."""
chassis = AckermannChassis(
Pose.from_center([10, 0, 0], Heading(math.pi * 0.5)), bullet_client)
b_chassis = BoxChassis(
Pose.from_center([0, 0, 0], Heading(0)),
speed=0,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
bullet_client=chassis._client,
)
collisions = step_with_vehicle_commands(chassis,
steps=1000,
throttle=1,
steering=0)
collided_bullet_ids = set([c.bullet_id for c in collisions])
GROUND_ID = 0
assert len(collisions) > 0
assert b_chassis.bullet_id in collided_bullet_ids
assert chassis.bullet_id not in collided_bullet_ids
assert GROUND_ID not in collided_bullet_ids
def test_bubble_manager_limit(vehicle, bubble):
limit = 2
bubble = replace(bubble, limit=limit)
manager = BubbleManager([bubble], road_network)
vehicles_captured = [
Vehicle(
id=f"vehicle-{i}",
pose=Pose.from_center((0, 0, 0), Heading(0)),
showbase=mock.MagicMock(),
chassis=mock.Mock(),
) for i in range(limit)
]
vehicles_not_captured = [
Vehicle(
id=f"vehicle-{i}",
pose=Pose.from_center((0, 0, 0), Heading(0)),
showbase=mock.MagicMock(),
chassis=mock.Mock(),
) for i in range(5)
]
for position in [(-8, 0), (-6, 0), (-3, 0), (0, 0), (6, 0), (8, 0)]:
for vehicle in vehicles_captured:
vehicle.position = position
for vehicle in vehicles_not_captured:
vehicle.position = position
change = manager.step_bubble_state(vehicles_captured,
vehicles_not_captured)
vehicle_ids_in_bubble = manager.vehicle_ids_in_bubble(bubble)
assert len(vehicle_ids_in_bubble) <= limit
assert set(vehicle_ids_in_bubble).issubset(
set([v.id for v in vehicles_captured]))
manager.teardown()
def test_non_collision(bullet_client: bc.BulletClient):
"""Spawn without overlap to check for the most basic collision"""
GROUND_ID = 0
chassis = AckermannChassis(
Pose.from_center([0, 0, 0], Heading(-math.pi * 0.5)), bullet_client
)
b_chassis = BoxChassis(
Pose.from_center([0, 10, 0], Heading(0)),
speed=0,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
bullet_client=chassis._client,
)
collisions = step_with_vehicle_commands(chassis, steps=1)
collided_bullet_ids = [c.bullet_id for c in collisions]
assert b_chassis.bullet_id not in collided_bullet_ids
assert (
len(collisions) == 0
or len(collided_bullet_ids) == 1
and GROUND_ID in set(collided_bullet_ids)
)
def test_create_social_vehicle(bullet_client):
chassis = BoxChassis(
pose=Pose.from_center((0, 0, 0), Heading(0)),
speed=0,
dimensions=BoundingBox(length=3, width=1, height=1),
bullet_client=bullet_client,
)
car = Vehicle(
id="sv-132",
pose=Pose.from_center((0, 0, 0), Heading(0)),
chassis=chassis,
sumo_vehicle_type="passenger",
)
assert car.vehicle_type == "car"
truck = Vehicle(
id="sv-132",
pose=Pose.from_center((0, 0, 0), Heading(0)),
chassis=chassis,
sumo_vehicle_type="truck",
)
assert truck.vehicle_type == "truck"
def test_box_chassis_collision(bullet_client: bc.BulletClient):
"""Spawn overlap to check for the most basic BoxChassis collision"""
# This is required to ensure that the bullet move_to constraint
# actually moves the vehicle the correct amount
bullet_client.setPhysicsEngineParameter(
fixedTimeStep=0.1,
numSubSteps=24,
numSolverIterations=10,
solverResidualThreshold=0.001,
)
chassis = BoxChassis(
Pose.from_center([0, 0, 0], Heading(-0.5 * math.pi)),
speed=10,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
bullet_client=bullet_client,
)
b_chassis = BoxChassis(
Pose.from_center([0, 0, 0], Heading(0.5 * math.pi)),
speed=0,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
bullet_client=chassis._client,
)
collisions = step_with_pose_delta(chassis,
steps=10,
pose_delta=np.array((1.0, 0, 0)),
speed=10)
assert len(collisions) == 3
collided_bullet_ids = set([c.bullet_id for c in collisions])
GROUND_ID = 0
assert b_chassis.bullet_id in collided_bullet_ids
assert chassis.bullet_id not in collided_bullet_ids
assert GROUND_ID not in collided_bullet_ids
def test_vehicle_spawned_outside_bubble_is_captured(smarts, mock_provider):
# Spawned vehicle drove through airlock so _should_ get captured
vehicle_id = "vehicle"
got_hijacked = False
for x in range(20):
mock_provider.override_next_provider_state(vehicles=[(
vehicle_id,
Pose.from_center((90 + x, 0, 0), Heading(-math.pi / 2)),
10, # speed
)])
smarts.step({})
if smarts.vehicle_index.vehicle_is_hijacked(vehicle_id):
got_hijacked = True
break
assert got_hijacked
def test_waypoints_sensor(scenarios):
scenario = next(scenarios)
sim = mock.Mock()
vehicle = mock.Mock()
vehicle.pose = Pose(
position=np.array([33, -65, 0]),
orientation=np.array([0, 0, 0, 0]),
heading_=Heading(0),
)
mission = scenario.missions[AGENT_ID]
plan = Plan(scenario.road_map, mission)
sensor = WaypointsSensor(vehicle, plan)
waypoints = sensor()
assert len(waypoints) == 3
def test_bubble_manager_state_change(smarts, mock_provider):
index = smarts.vehicle_index
vehicle_id = "vehicle"
state_at_position = {
# Outside airlock and bubble
(92, 0, 0): (False, False),
(93, 0, 0): (False, False), # need a step for new route to "take"
# Inside airlock, begin collecting experiences, but don't hijack
(94, 0, 0): (True, False),
# Entered bubble, now hijack
(100, 0, 0): (False, True),
# Leave bubble into exiting airlock
(106, 0, 0): (False, True),
# Exit bubble and airlock, now relinquish
(108, 0, 0): (False, False),
}
for position, (shadowed, hijacked) in state_at_position.items():
mock_provider.override_next_provider_state(
vehicles=[(vehicle_id,
Pose.from_center(position, Heading(-math.pi / 2)), 10)])
# Providers must be disjoint
if index.vehicle_is_hijacked(vehicle_id):
mock_provider.clear_next_provider_state()
while (index.vehicle_is_hijacked(vehicle_id)
and index.vehicle_position(vehicle_id)[0] < position[0]):
smarts.step({})
else:
smarts.step({})
# XXX: this is necessary because the bubble manager doesn't know
# XXX: what route to give the agent when it hijacks vehicle.
smarts.traffic_sim.update_route_for_vehicle(
vehicle_id, ["west", "east"])
got_shadowed = index.vehicle_is_shadowed(vehicle_id)
got_hijacked = index.vehicle_is_hijacked(vehicle_id)
assert_msg = (
f"position={position}\n"
f"\t(expected: shadowed={shadowed}, hijacked={hijacked})\n"
f"\t(received: shadowed={got_shadowed}, hijacked={got_hijacked})")
assert got_shadowed == shadowed, assert_msg
assert got_hijacked == hijacked, assert_msg
def test_vehicle_bounding_box(bullet_client):
pose = Pose.from_center((1, 1, 0), Heading(0))
chassis = BoxChassis(
pose=pose,
speed=0,
dimensions=Dimensions(length=3, width=1, height=1),
bullet_client=bullet_client,
)
vehicle = Vehicle(
id="vehicle-0",
chassis=chassis,
vehicle_config_type="passenger",
)
for coordinates in zip(vehicle.bounding_box, [[0.5, 2.5], (1.5, 2.5),
(1.5, -0.5), (0.5, -0.5)]):
assert np.array_equal(coordinates[0], coordinates[1])
def test_bubble_manager_state_change(smarts, mock_provider):
index = smarts.vehicle_index
vehicle_id = "vehicle"
state_at_position = {
# Outside airlock and bubble
(92, 0, 0): (False, False),
# Inside airlock, begin collecting experiences, but don't hijack
(94, 0, 0): (True, False),
# Entered bubble, now hijack
(100, 0, 0): (False, True),
# Leave bubble into exiting airlock
(106, 0, 0): (False, True),
# Exit bubble and airlock, now relinquish
(108, 0, 0): (False, False),
}
for position, (shadowed, hijacked) in state_at_position.items():
mock_provider.override_next_provider_state(
vehicles=[
(vehicle_id, Pose.from_center(position, Heading(-math.pi / 2)), 10)
]
)
# Providers must be disjoint
if index.vehicle_is_hijacked(vehicle_id):
mock_provider.clear_next_provider_state()
while (
index.vehicle_is_hijacked(vehicle_id)
and index.vehicle_position(vehicle_id)[0] < position[0]
):
smarts.step({})
else:
smarts.step({})
got_shadowed = index.vehicle_is_shadowed(vehicle_id)
got_hijacked = index.vehicle_is_hijacked(vehicle_id)
assert_msg = (
f"position={position}\n"
f"\t(expected: shadowed={shadowed}, hijacked={hijacked})\n"
f"\t(received: shadowed={got_shadowed}, hijacked={got_hijacked})"
)
assert got_shadowed == shadowed, assert_msg
assert got_hijacked == hijacked, assert_msg
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 test_bubble_manager_limit(smarts, mock_provider):
vehicle_ids = ["vehicle-1", "vehicle-2", "vehicle-3"]
for x in range(200):
vehicle_ids = {
v_id
for v_id in vehicle_ids
if not smarts.vehicle_index.vehicle_is_hijacked(v_id)
}
vehicles = [(
v_id,
Pose.from_center((80 + y * 0.5 + x * 0.25, y * 4 - 4, 0),
Heading(math.pi / 2)),
10,
) for y, v_id in enumerate(vehicle_ids)]
mock_provider.override_next_provider_state(vehicles=vehicles)
smarts.step({})
# 3 total vehicles, 1 hijacked and removed according to limit, 2 remaining
assert (
len(vehicle_ids) == 2
), "Only 1 vehicle should have been hijacked according to the limit"
def test_bubble_manager_state_change(vehicle, bubble):
manager = BubbleManager([bubble], road_network)
# Outside airlock and bubble
vehicle = Vehicle(
id="vehicle-1",
pose=Pose.from_center((0, 0, 0), Heading(0)),
showbase=mock.MagicMock(),
chassis=mock.Mock(),
)
vehicle.position = (-8, 0)
change = manager.step_bubble_state([vehicle], [])
assert len(change.entered_airlock_1) == len(change.entered_bubble) == 0
# Inside airlock, begin collecting experiences, but don't hijack
vehicle.position = (-6, 0)
change = manager.step_bubble_state([vehicle], [])
assert len(change.entered_airlock_1) == 1 and len(
change.entered_bubble) == 0
# Entered bubble, now hijack
vehicle.position = (-3, 0)
change = manager.step_bubble_state([vehicle], [])
assert len(change.entered_airlock_1) == 0 and len(
change.entered_bubble) == 1
assert change.entered_bubble[0][0] == vehicle.id
# Leave bubble into exiting airlock
vehicle.position = (6, 0)
change = manager.step_bubble_state([], [vehicle])
assert len(change.entered_bubble) == 0 and len(change.exited_bubble) == 1
# Exit bubble and airlock, now relinquish
vehicle.position = (8, 0)
change = manager.step_bubble_state([vehicle], [])
assert len(change.exited_bubble) == 0 and len(change.exited_airlock_2) == 1
manager.teardown()
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 _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 _publish_agents(self, observations: Dict[str, Observation],
dones: Dict[str, bool]):
agents = AgentsStamped()
agents.header.stamp = rospy.Time.now()
for agent_id, agent_obs in observations.items():
veh_state = agent_obs.ego_vehicle_state
pose = Pose.from_center(veh_state.position, veh_state.heading)
agent = AgentReport()
agent.agent_id = agent_id
ROSDriver._vector_to_xyz(pose.position, agent.pose.position)
ROSDriver._vector_to_xyzw(pose.orientation, agent.pose.orientation)
agent.speed = veh_state.speed
agent.distance_travelled = agent_obs.distance_travelled
agent.is_done = dones[agent_id]
agent.reached_goal = agent_obs.events.reached_goal
agent.did_collide = bool(agent_obs.events.collisions)
agent.is_wrong_way = agent_obs.events.wrong_way
agent.is_off_route = agent_obs.events.off_route
agent.is_off_road = agent_obs.events.off_road
agent.is_on_shoulder = agent_obs.events.on_shoulder
agent.is_not_moving = agent_obs.events.not_moving
agents.agents.append(agent)
self._agents_publisher.publish(agents)
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 _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
