def step(self, provider_actions: Dict, dt, elapsed_sim_time) -> ProviderState: """[summary] Args: Returns: ProviderState: [description] """ provider_state = ProviderState() for vehicle_id, traj in provider_actions.items(): pose, speed = self.perform_trajectory_interpolation(dt, traj) provider_state.vehicles.append( VehicleState( vehicle_id=vehicle_id, vehicle_config_type="passenger", pose=pose, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, speed=speed, source="TrajectoryInterpolation", )) return provider_state
def _extrapolate_to_now(vs: VehicleState, staleness: float, lin_acc_slope: float, ang_acc_slope: float): """Here we just linearly extrapolate the acceleration to "now" from the previous state for each vehicle and then use standard kinematics to project the velocity and position from that.""" # The following ~10 lines are a hack b/c I'm too stupid to figure out # how to do calculus on quaternions... heading = vs.pose.heading heading_delta_vec = staleness * ( vs.angular_velocity + 0.5 * vs.angular_acceleration * staleness + ang_acc_slope * staleness**2 / 6.0) heading += vec_to_radians(heading_delta_vec[:2]) + (0.5 * math.pi) heading %= 2 * math.pi vs.pose.orientation = fast_quaternion_from_angle(heading) # XXX: also need to remove the cached heading_ since we've changed orientation vs.pose.heading_ = None # I assume the following should be updated based on changing # heading from above, but I'll leave that for now... vs.pose.position += staleness * ( vs.linear_velocity + 0.5 * vs.linear_acceleration * staleness + lin_acc_slope * staleness**2 / 6.0) vs.linear_velocity += staleness * (vs.linear_acceleration + 0.5 * lin_acc_slope * staleness) vs.speed = np.linalg.norm(vs.linear_velocity) vs.angular_velocity += staleness * (vs.angular_acceleration + 0.5 * ang_acc_slope * staleness) vs.linear_acceleration += staleness * lin_acc_slope vs.angular_acceleration += staleness * ang_acc_slope
def _hijack_social_vehicle_with_social_agent( self, sim, vehicle_id: str, social_agent_actor: SocialAgentActor, ) -> str: """Upon hijacking the social agent is now in control of the vehicle. It will initialize the vehicle chassis (and by extension the controller) with a "greatest common denominator" state; that is: what's available via the vehicle front-end common to both source and destination policies during airlock. """ self._log.debug(f"Hijack vehicle={vehicle_id} with actor={social_agent_actor}") agent_id = BubbleManager._make_social_agent_id(vehicle_id, social_agent_actor) is_boid = isinstance(social_agent_actor, BoidAgentActor) vehicle = sim.vehicle_index.switch_control_to_agent( sim, vehicle_id, agent_id, boid=is_boid, recreate=False ) self._update_cursor(vehicle_id, vehicle=vehicle) for provider in sim.providers: if ( sim.agent_manager.agent_interface_for_agent_id(agent_id).action_space in provider.action_spaces ): provider.create_vehicle( VehicleState( vehicle_id=vehicle_id, vehicle_type="passenger", pose=vehicle.pose, dimensions=vehicle.chassis.dimensions, speed=vehicle.speed, source="HIJACK", ) )
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 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 override_next_provider_state(self, vehicles: Sequence): self._next_provider_state = ProviderState(vehicles=[ VehicleState( vehicle_id=vehicle_id, vehicle_type="passenger", pose=pose, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, speed=speed, source="MOCK", ) for vehicle_id, pose, speed in vehicles ], )
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 _add_agent(self, agent_id, agent_interface, agent_model, sim, boid=False, trainable=True): # TODO: Disentangle what is entangled below into: # 1. AgentState initialization, # 2. Agent vehicle initialization, which should live elsewhere, and # 3. Provider related state initialization, which does not belong here. # # A couple of things forced the current unhappy state -- # # 1. SMARTS internal coordinate system should be 100% unified. Coordinate # transformation should happen only at the interface between SMARTS and # its providers. For example, mission start pose should just be vehicle # initial pose. # 2. AgentState should be allowed to fully initialized (setup) without vehicle # information. We currently rely on the creation of the Vehicle instance to # do the coordinate transformation. :-( # 3. Providers should not be creating vehicles. They do need to get notified # about new vehicles entering their territory through the VehicleState # message. But that does not need to happen at Agent instantiation. assert isinstance(agent_id, str) # SUMO expects strings identifiers scenario = sim.scenario mission = scenario.mission(agent_id) planner = MissionPlanner( scenario.waypoints, scenario.road_network, agent_behavior=agent_interface.agent_behavior, ) planner.plan(mission) vehicle = sim.vehicle_index.build_agent_vehicle( sim, agent_id, agent_interface, planner, scenario.vehicle_filepath, scenario.tire_parameters_filepath, trainable, scenario.surface_patches, scenario.controller_parameters_filepath, agent_model.initial_speed, boid=boid, ) matching_providers = [ provider for provider in sim.providers if agent_interface.action_space in provider.action_spaces ] if matching_providers: assert ( len(matching_providers) == 1 ), f"Found {matching_providers} for action space {agent_interface.action_space}" provider = matching_providers[0] provider.create_vehicle( VehicleState( vehicle_id=vehicle.id, vehicle_type=vehicle.vehicle_type, pose=vehicle.pose, dimensions=vehicle.chassis.dimensions, source="NEW-AGENT", )) self._agent_interfaces[agent_id] = agent_interface self._social_agent_data_models[agent_id] = agent_model
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 step( self, sim, ): captures_by_agent_id = defaultdict(list) # Do an optimization to only check if there are pending agents. if not sim.agent_manager.pending_agent_ids: return social_vehicle_ids = sim.vehicle_index.social_vehicle_ids vehicles = { v_id: sim.vehicle_index.vehicle_by_id(v_id) for v_id in social_vehicle_ids } existing_agent_vehicles = ( sim.vehicle_index.vehicle_by_id(v_id) for v_id in sim.vehicle_index.agent_vehicle_ids) def largest_vehicle_plane_dimension(vehicle): return max(*vehicle.chassis.dimensions.as_lwh[:2]) agent_vehicle_comp = [(v.position[:2], largest_vehicle_plane_dimension(v), v) for v in existing_agent_vehicles] for agent_id in sim.agent_manager.pending_agent_ids: trap = self._traps[agent_id] if trap is None: continue trap.step_trigger(sim.timestep_sec) if not trap.ready: continue # Order vehicle ids by distance. sorted_vehicle_ids = sorted( list(social_vehicle_ids), key=lambda v: squared_dist(vehicles[v].position[:2], trap. mission.start.position), ) for v_id in sorted_vehicle_ids: vehicle = vehicles[v_id] point = Point(vehicle.position) if any( v.startswith(prefix) for prefix in trap.exclusion_prefixes): continue if not point.within(trap.geometry): continue captures_by_agent_id[agent_id].append(( v_id, trap, replace( trap.mission, start=Start(vehicle.position[:2], vehicle.pose.heading), ), )) # TODO: Resolve overlap using a tree instead of just removing. social_vehicle_ids.remove(v_id) break # Use fed in trapped vehicles. agents_given_vehicle = set() used_traps = [] for agent_id in sim._agent_manager.pending_agent_ids: if agent_id not in self._traps: continue trap = self._traps[agent_id] captures = captures_by_agent_id[agent_id] if not trap.ready: continue vehicle = None if len(captures) > 0: vehicle_id, trap, mission = rand.choice(captures) vehicle = TrapManager._hijack_vehicle(sim, vehicle_id, agent_id, mission) elif trap.patience_expired: if len(agent_vehicle_comp) > 0: agent_vehicle_comp.sort(key=lambda v: squared_dist( v[0], trap.mission.start.position)) # Make sure there is not an agent vehicle in the same location pos, largest_dimension, _ = agent_vehicle_comp[0] if (squared_dist(pos, trap.mission.start.position) < largest_dimension): continue vehicle = TrapManager._make_vehicle(sim, agent_id, trap.mission) else: continue if vehicle == None: continue agents_given_vehicle.add(agent_id) used_traps.append((agent_id, trap)) for provider in sim.providers: if (sim.agent_manager.agent_interface_for_agent_id( agent_id).action_space in provider.action_spaces): provider.create_vehicle( VehicleState( vehicle_id=vehicle.id, vehicle_type="passenger", pose=vehicle.pose, dimensions=vehicle.chassis.dimensions, speed=vehicle.speed, source="EGO-HIJACK", )) if len(agents_given_vehicle) > 0: self.reset_traps(used_traps) sim.agent_manager.remove_pending_agent_ids(agents_given_vehicle)
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()