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 _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
def social_spin_on_bumper_cw(step, front_bumper_position, length): return Pose.from_front_bumper(np.array(front_bumper_position[:2]), Heading.from_sumo(step), length)