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_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 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_conversion_panda(original_pose):
pose_info = [
(
[0, 1, 0],
0,
),
(
[0, 1, 0],
180,
),
(
[0, 1, 0],
180,
),
(
[0, 1],
90,
),
]
for position, heading in pose_info:
heading = Heading.from_panda3d(heading)
p_from_center = Pose.from_center(base_position=position,
heading=heading)
assert len(p_from_center.position) == 3
assert len(p_from_center.orientation) == 4
a_pos, a_ori = p_from_center.position, p_from_center.heading
if len(position) < 3:
position = [*position, 0]
assert np.isclose(position, a_pos, atol=2e-07).all()
assert math.isclose(heading, a_ori, abs_tol=2e-07)
def test_bubble_manager_limit(smarts, mock_provider, time_resolution):
vehicle_ids = ["vehicle-1", "vehicle-2", "vehicle-3"]
speed = 2.5
distance_per_step = speed * time_resolution
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 * distance_per_step, y * 4 - 4, 0),
Heading(-math.pi / 2),
),
speed, # speed
) 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_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 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 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 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 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 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 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 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_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 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_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_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_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(80)),
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 _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)
client.setGravity(0, 0, -9.8)
client.setPhysicsEngineParameter(
fixedTimeStep=TIMESTEP_SEC,
numSubSteps=int(TIMESTEP_SEC / (1 / 240)),
# enableConeFriction=False,
# 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),
def pose(self) -> Pose:
return Pose.from_center(tuple(self.pos), self.heading)
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()
