def test_create_social_vehicle(showbase, 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)),
showbase=showbase,
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)),
showbase=showbase,
chassis=chassis,
sumo_vehicle_type="truck",
)
assert truck.vehicle_type == "truck"
def test_vehicle_spawned_in_bubble_is_not_captured(vehicle, bubble):
manager = BubbleManager([bubble], road_network)
# Spawned inside bubble, didn't "drive through" airlocking region, so should _not_
# get captured
vehicle = Vehicle(
id="vehicle-1",
pose=Pose.from_center((0, 0, 0), Heading(0)),
showbase=mock.MagicMock(),
chassis=mock.Mock(),
)
change = manager.step_bubble_state([vehicle], [])
assert len(manager.vehicle_ids_in_bubble(bubble)) == 0
# Spawned vehicle drove through airlock so _should_ get captured
vehicle = Vehicle(
id="vehicle-2",
pose=Pose.from_center((-8, 0, 0), Heading(0)),
showbase=mock.MagicMock(),
chassis=mock.Mock(),
)
change = manager.step_bubble_state([vehicle], [])
vehicle.position = (-6, 0)
change = manager.step_bubble_state([vehicle], [])
assert len(manager.vehicle_ids_in_bubble(bubble)) == 1
manager.teardown()
def social_vehicle(position, heading, speed, showbase, bullet_client):
pose = Pose.from_center(position, heading)
chassis = BoxChassis(
pose=pose,
speed=speed,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
bullet_client=bullet_client,
)
return Vehicle(id="sv-132", pose=pose, showbase=showbase, chassis=chassis)
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 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_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_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(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()
# erp=0.1,
# contactERP=0.1,
# frictionERP=0.1,
)
path = Path(__file__).parent / "../smarts/core/models/plane.urdf"
path = str(path.absolute())
plane_body_id = client.loadURDF(path, useFixedBase=True)
client.changeDynamics(plane_body_id, -1, **frictions(sliders))
pose = pose = Pose.from_center((0, 0, 0), Heading(0))
vehicle = Vehicle(
id="vehicle",
chassis=AckermannChassis(
pose=pose,
bullet_client=client,
),
)
run(
client,
vehicle,
plane_body_id,
sliders,
n_steps=int(1e6),
)
except Exception as e:
print(e)
# Uncomment for calculating the acceleration
# ax=[(x-vel[i-1])/TIMESTEP_SEC for i, x in enumerate(vel)][1:]
# erp=0.1,
# contactERP=0.1,
# frictionERP=0.1,
)
path = Path(__file__).parent / "../smarts/core/models/plane.urdf"
path = str(path.absolute())
plane_body_id = client.loadURDF(path, useFixedBase=True)
client.changeDynamics(plane_body_id, -1, **frictions(sliders))
pose = pose = Pose.from_center((0, 0, 0), Heading(0))
vehicle = Vehicle(
"hello",
chassis=AckermannChassis(
pose=pose,
bullet_client=client,
tire_parameters_filepath="../../smarts/core/models/tire_parameters.yaml",
),
)
run(
client,
vehicle,
plane_body_id,
sliders,
n_steps=int(1e6),
)
except Exception as e:
print(e)
# plt.plot(xx,yy)
# plt.plot(time, speed)
# frictionERP=0.1,
)
path = Path(__file__).parent / "../smarts/core/models/plane.urdf"
path = str(path.absolute())
plane_body_id = client.loadURDF(path, useFixedBase=True)
client.changeDynamics(plane_body_id, -1, **frictions(sliders))
pose = pose = Pose.from_center((0, 0, 0), Heading(0))
vehicle = Vehicle(
"hello",
pose=pose,
showbase=showbase,
chassis=AckermannChassis(
pose=pose,
bullet_client=client,
tire_parameters_filepath=
"/home/kyber/MainProjectSMARTS/SMARTS/tools/tire_parameters.yaml",
),
)
run(
showbase,
client,
vehicle,
plane_body_id,
sliders,
n_steps=int(1e6),
)
except Exception as e:
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()
def step(self, sim):
"""Run hijacking and update agent and actor states."""
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 = [
v_id for v_id in sim.vehicle_index.social_vehicle_ids()
if not sim.vehicle_index.vehicle_is_shadowed(v_id)
]
vehicles = {
v_id: sim.vehicle_index.vehicle_by_id(v_id)
for v_id in social_vehicle_ids
}
def largest_vehicle_plane_dimension(vehicle):
return max(*vehicle.chassis.dimensions.as_lwh[:2])
vehicle_comp = [(v.position[:2], largest_vehicle_plane_dimension(v), v)
for v in vehicles.values()]
for agent_id in sim.agent_manager.pending_agent_ids:
trap = self._traps[agent_id]
if trap is None:
continue
trap.step_trigger(sim.last_dt)
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[:2]),
)
for v_id in sorted_vehicle_ids:
# Skip the capturing process if history traffic is used
if sim.scenario.traffic_history is not None:
break
if not trap.includes(v_id):
continue
vehicle = vehicles[v_id]
point = vehicle.pose.point.as_shapely
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 = sim.switch_control_to_agent(vehicle_id,
agent_id,
mission,
recreate=True,
is_hijacked=False)
elif trap.patience_expired:
# Make sure there is not a vehicle in the same location
mission = trap.mission
nv_dims = Vehicle.agent_vehicle_dims(mission)
new_veh_maxd = max(nv_dims.as_lwh[:2])
overlapping = False
for pos, largest_dimension, _ in vehicle_comp:
if (squared_dist(pos, mission.start.position[:2]) <=
(0.5 * (largest_dimension + new_veh_maxd))**2):
overlapping = True
break
if overlapping:
continue
vehicle = TrapManager._make_vehicle(sim, agent_id, mission,
trap.default_entry_speed)
else:
continue
if vehicle == None:
continue
sim.create_vehicle_in_providers(vehicle, agent_id)
agents_given_vehicle.add(agent_id)
used_traps.append((agent_id, trap))
if len(agents_given_vehicle) > 0:
self.remove_traps(used_traps)
sim.agent_manager.remove_pending_agent_ids(agents_given_vehicle)