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 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 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 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 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)
assert len(collisions) > 0
assert b_chassis.bullet_id in [c.bullet_id for c in collisions]
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=1)
assert b_chassis.bullet_id in [c.bullet_id for c in collisions]
assert len(collisions) > 0
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_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_ackerman_chassis_size_unchanged(bullet_client: bc.BulletClient):
"""Test that the ackerman chassis size has not changed accidentally by packing it around itself
with no forces and then check for collisions after a few steps."""
bullet_client.setGravity(0, 0, 0)
separation_for_collision_error = 0.05
original_vehicle_dimensions = VEHICLE_CONFIGS["passenger"].dimensions
shared_heading = Heading(0)
chassis = AckermannChassis(Pose.from_center([0, 0, 0], shared_heading),
bullet_client)
chassis_n = BoxChassis(
Pose.from_center(
[
0,
(original_vehicle_dimensions.length +
separation_for_collision_error),
0,
],
shared_heading,
),
speed=0,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
bullet_client=bullet_client,
)
chassis_e = BoxChassis(
Pose.from_center(
[
(original_vehicle_dimensions.width +
separation_for_collision_error),
0,
0,
],
shared_heading,
),
speed=0,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
bullet_client=bullet_client,
)
chassis_s = BoxChassis(
Pose.from_center(
[
0,
-(original_vehicle_dimensions.length +
separation_for_collision_error),
0,
],
shared_heading,
),
speed=0,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
bullet_client=bullet_client,
)
chassis_w = BoxChassis(
Pose.from_center(
[
-(original_vehicle_dimensions.width +
separation_for_collision_error),
0,
0,
],
shared_heading,
),
speed=0,
dimensions=VEHICLE_CONFIGS["passenger"].dimensions,
bullet_client=bullet_client,
)
collisions = step_with_vehicle_commands(chassis, steps=10)
assert len(collisions) < 1
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()
