This function plans the trajectories of vehicles in a given roadgraph. It takes in the total time T, the roadgraph, and the vehicles_info as parameters. It first listens for keyboard input and then extracts the ego car, current vehicles, and uncontrolled vehicles
(self, T: float, roadgraph: RoadGraph,
vehicles_info: dict)
| 94 | listener.start() # start to listen on a separate thread |
| 95 | |
| 96 | def plan(self, T: float, roadgraph: RoadGraph, |
| 97 | vehicles_info: dict) -> Dict[int, Trajectory]: |
| 98 | """ |
| 99 | This function plans the trajectories of vehicles in a given roadgraph. |
| 100 | It takes in the total time T, the roadgraph, and the vehicles_info as parameters. |
| 101 | It first listens for keyboard input and then extracts the ego car, current vehicles, |
| 102 | and uncontrolled vehicles from the vehicles_info. |
| 103 | It then updates the behavior of the ego car and current vehicles. |
| 104 | It then constructs the observation and predicts the behavior of the uncontrolled vehicles. |
| 105 | It then makes a decision for the ego car if the ego planner is enabled. |
| 106 | It then plans the trajectories of the vehicles and updates the last seen vehicles. |
| 107 | Finally, it returns the output trajectories. |
| 108 | """ |
| 109 | global KEY_INPUT |
| 110 | |
| 111 | start = time.time() |
| 112 | |
| 113 | current_time_step = int(T / self.config["DT"]) |
| 114 | through_timestep = current_time_step - self.time_step |
| 115 | |
| 116 | # Perception module |
| 117 | vehicles = self.extract_vehicles(vehicles_info, roadgraph, T, |
| 118 | through_timestep, self.sumo_model.sim_mode) |
| 119 | history_tracks = self.extract_history_tracks(current_time_step, |
| 120 | vehicles) |
| 121 | static_obs_list = self.extract_static_obstacles() |
| 122 | observation = Observation(vehicles=list(vehicles.values()), |
| 123 | history_track=history_tracks, |
| 124 | static_obstacles=static_obs_list) |
| 125 | |
| 126 | # Prediction Module |
| 127 | prediction = self.predictor.predict(observation, roadgraph, |
| 128 | self.lastseen_vehicles, |
| 129 | through_timestep, self.config) |
| 130 | |
| 131 | # Update Behavior |
| 132 | for vehicle_id, vehicle in vehicles.items(): |
| 133 | # only vehicles in AoI will be controlled |
| 134 | if vehicle.vtype == VehicleType.OUT_OF_AOI: |
| 135 | continue |
| 136 | if not vehicle.update_behaviour(roadgraph, KEY_INPUT): |
| 137 | return {} |
| 138 | KEY_INPUT = "" |
| 139 | |
| 140 | # make sure ego car exists when EGO_PLANNER is used |
| 141 | if self.config["EGO_PLANNER"]: |
| 142 | ego_id = vehicles_info.get("egoCar")["id"] |
| 143 | if ego_id is None: |
| 144 | raise ValueError("Ego car is not found when EGO_PLANER is used.") |
| 145 | |
| 146 | ego_decision: EgoDecision = None |
| 147 | if self.config["USE_DECISION_MAKER"] and T - self.last_decision_time >= self.config["DECISION_INTERVAL"]: |
| 148 | if self.config["EGO_PLANNER"]: |
| 149 | ego_decision = self.ego_decision.make_decision( |
| 150 | observation, roadgraph, prediction) |
| 151 | self.mul_decisions = self.multi_decision.make_decision( |
| 152 | T, observation, roadgraph, prediction, self.config) |
| 153 | self.last_decision_time = T |
no test coverage detected