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