def acceleration(self, ego_vehicle: ControlledVehicle, front_vehicle: Vehicle = None, rear_vehicle: Vehicle = None) -> float: """ Compute an acceleration command with the Intelligent Driver Model. The acceleration is chosen so as to: - reach a target speed; - maintain a minimum safety distance (and safety time) w.r.t the front vehicle. :param ego_vehicle: the vehicle whose desired acceleration is to be computed. It does not have to be an IDM vehicle, which is why this method is a class method. This allows an IDM vehicle to reason about other vehicles behaviors even though they may not IDMs. :param front_vehicle: the vehicle preceding the ego-vehicle :param rear_vehicle: the vehicle following the ego-vehicle :return: the acceleration command for the ego-vehicle [m/s2] """ if not ego_vehicle or isinstance(ego_vehicle, RoadObject): return 0 ego_target_speed = utils.not_zero( getattr(ego_vehicle, "target_speed", 0)) acceleration = self.COMFORT_ACC_MAX * (1 - np.power( max(ego_vehicle.speed, 0) / ego_target_speed, self.DELTA)) if front_vehicle: d = ego_vehicle.lane_distance_to(front_vehicle) acceleration -= self.COMFORT_ACC_MAX * \ np.power(self.desired_gap(ego_vehicle, front_vehicle) / utils.not_zero(d), 2) return acceleration
def steering_control(self, target_lane_index: LaneIndex) -> float: """ Steer the vehicle to follow the center of an given lane. 1. Lateral position is controlled by a proportional controller yielding a lateral speed command 2. Lateral speed command is converted to a heading reference 3. Heading is controlled by a proportional controller yielding a heading rate command 4. Heading rate command is converted to a steering angle :param target_lane_index: index of the lane to follow :return: a steering wheel angle command [rad] """ target_lane = self.road.network.get_lane(target_lane_index) lane_coords = target_lane.local_coordinates(self.position) lane_next_coords = lane_coords[0] + self.speed * self.PURSUIT_TAU lane_future_heading = target_lane.heading_at(lane_next_coords) # Lateral position control lateral_speed_command = -self.KP_LATERAL * lane_coords[1] # Lateral speed to heading heading_command = np.arcsin( np.clip(lateral_speed_command / utils.not_zero(self.speed), -1, 1)) heading_ref = lane_future_heading + np.clip(heading_command, -np.pi / 4, np.pi / 4) # Heading control heading_rate_command = self.KP_HEADING * utils.wrap_to_pi(heading_ref - self.heading) # Heading rate to steering angle steering_angle = np.arcsin( np.clip( self.LENGTH / 2 / utils.not_zero(self.speed) * heading_rate_command, -1, 1)) steering_angle = np.clip(steering_angle, -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE) return float(steering_angle)
def steering_features(self, target_lane_index: LaneIndex) -> np.ndarray: """ A collection of features used to follow a lane :param target_lane_index: index of the lane to follow :return: a array of features """ lane = self.road.network.get_lane(target_lane_index) lane_coords = lane.local_coordinates(self.position) lane_next_coords = lane_coords[0] + self.speed * self.PURSUIT_TAU lane_future_heading = lane.heading_at(lane_next_coords) features = np.array([ utils.wrap_to_pi(lane_future_heading - self.heading) * self.LENGTH / utils.not_zero(self.speed), -lane_coords[1] * self.LENGTH / (utils.not_zero(self.speed)**2) ]) return features
def compute_ttc_grid(env: 'AbstractEnv', time_quantization: float, horizon: float, vehicle: Optional[Vehicle] = None) -> np.ndarray: """ Compute the grid of predicted time-to-collision to each vehicle within the lane For each ego-speed and lane. :param env: environment :param time_quantization: time step of a grid cell :param horizon: time horizon of the grid :param vehicle: the observer vehicle :return: the time-co-collision grid, with axes SPEED x LANES x TIME """ vehicle = vehicle or env.vehicle road_lanes = env.road.network.all_side_lanes(env.vehicle.lane_index) grid = np.zeros((vehicle.SPEED_COUNT, len(road_lanes), int(horizon / time_quantization))) for speed_index in range(grid.shape[0]): ego_speed = vehicle.index_to_speed(speed_index) for other in env.road.vehicles: if (other is vehicle) or (ego_speed == other.speed): continue margin = other.LENGTH / 2 + vehicle.LENGTH / 2 collision_points = [(0, 1), (-margin, 0.5), (margin, 0.5)] for m, cost in collision_points: distance = vehicle.lane_distance_to(other) + m other_projected_speed = other.speed * np.dot( other.direction, vehicle.direction) time_to_collision = distance / utils.not_zero( ego_speed - other_projected_speed) if time_to_collision < 0: continue if env.road.network.is_connected_road(vehicle.lane_index, other.lane_index, route=vehicle.route, depth=3): # Same road, or connected road with same number of lanes if len(env.road.network.all_side_lanes( other.lane_index)) == len( env.road.network.all_side_lanes( vehicle.lane_index)): lane = [other.lane_index[2]] # Different road of different number of lanes: uncertainty on future lane, use all else: lane = range(grid.shape[1]) # Quantize time-to-collision to both upper and lower values for time in [ int(time_to_collision / time_quantization), int(np.ceil(time_to_collision / time_quantization)) ]: if 0 <= time < grid.shape[2]: # TODO: check lane overflow (e.g. vehicle with higher lane id than current road capacity) grid[speed_index, lane, time] = np.maximum( grid[speed_index, lane, time], cost) return grid