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()