def steering_control(self, target_lane_index): """ Steer the vehicle to follow the center of an given lane. 1. Lateral position is controlled by a proportional controller yielding a lateral velocity command 2. Lateral velocity 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.velocity * self.PURSUIT_TAU lane_future_heading = target_lane.heading_at(lane_next_coords) # Lateral position control lateral_velocity_command = -self.KP_LATERAL * lane_coords[1] # Lateral velocity to heading heading_command = np.arcsin( np.clip(lateral_velocity_command / utils.not_zero(self.velocity), -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.arctan( self.LENGTH / utils.not_zero(self.velocity) * heading_rate_command) steering_angle = np.clip(steering_angle, -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE) return steering_angle
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 not isinstance(ego_vehicle, Vehicle): 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 acceleration(cls, ego_vehicle, front_vehicle=None, rear_vehicle=None): """ Compute an acceleration command with the Intelligent Driver Model. The acceleration is chosen so as to: - reach a target velocity; - 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: return 0 acceleration = cls.COMFORT_ACC_MAX * (1 - np.power( ego_vehicle.velocity / utils.not_zero(ego_vehicle.target_velocity), cls.DELTA)) if front_vehicle: d = ego_vehicle.lane_distance_to(front_vehicle) acceleration -= cls.COMFORT_ACC_MAX * \ np.power(cls.desired_gap(ego_vehicle, front_vehicle) / utils.not_zero(d), 2) return acceleration
def acceleration(self, ego_vehicle, front_vehicle=None, rear_vehicle=None): """ Compute an acceleration command with the Intelligent Driver Model. The acceleration is chosen so as to: - reach a target velocity; - 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: return 0 ego_target_velocity = utils.not_zero( getattr(ego_vehicle, "target_velocity", 0)) acceleration = self.COMFORT_ACC_MAX * (1 - np.power( max(ego_vehicle.velocity, 0) / ego_target_velocity, 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) # print("distance to front vehicle", d, ego_vehicle.LENGTH) # if ((front_vehicle.lane == self.lane and d < 1.5*ego_vehicle.LENGTH)): # # print(ego_vehicle.lane, front_vehicle.lane) # acceleration = -self.ACC_MAX # self.target_velocity = 15 # print("target_velocity", self.target_velocity) return acceleration
def steering_features(self, target_lane_index): """ 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.velocity * 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.velocity), -lane_coords[1] * self.LENGTH / (utils.not_zero(self.velocity) ** 2)]) return features
def compute_ttc_grid(env, time_quantization, horizon, considered_lanes="all"): """ For each ego-velocity and lane, compute the predicted time-to-collision to each vehicle within the lane and store the results in an occupancy grid. """ road_lanes = env.road.network.all_side_lanes(env.vehicle.lane_index) grid = np.zeros((env.vehicle.SPEED_COUNT, len(road_lanes), int(horizon / time_quantization))) for velocity_index in range(grid.shape[0]): ego_velocity = env.vehicle.index_to_speed(velocity_index) for other in env.road.vehicles: if (other is env.vehicle) or (ego_velocity == other.velocity): continue margin = other.LENGTH / 2 + env.vehicle.LENGTH / 2 collision_points = [(0, 1), (-margin, 0.5), (margin, 0.5)] for m, cost in collision_points: distance = env.vehicle.lane_distance_to(other) + m other_projected_velocity = other.velocity time_to_collision = distance / utils.not_zero(ego_velocity - other_projected_velocity) if time_to_collision < 0: continue if env.road.network.is_connected_road(env.vehicle.lane_index, other.lane_index, route=env.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(env.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[velocity_index, lane, time] = np.maximum(grid[velocity_index, lane, time], cost) return grid
def compute_ttc_grid(env, time_quantization, horizon): """ For each ego-velocity and lane, compute the predicted time-to-collision to each vehicle within the lane and store the results in an occupancy grid. """ grid = np.zeros((env.vehicle.SPEED_COUNT, len(env.road.lanes), int(horizon / time_quantization))) for velocity_index in range(grid.shape[0]): ego_velocity = env.vehicle.index_to_speed(velocity_index) for other in env.road.vehicles: if (other is env.vehicle) or (ego_velocity == other.velocity): continue margin = other.LENGTH / 2 + env.vehicle.LENGTH / 2 collision_points = [(0, 1), (-margin, 0.5), (margin, 0.5)] for m, cost in collision_points: distance = env.vehicle.lane_distance_to(other) + m time_to_collision = distance / utils.not_zero(ego_velocity - other.velocity) if time_to_collision < 0: continue # Quantize time-to-collision to both upper and lower values lane = other.lane_index for time in [ int(time_to_collision / time_quantization), int(np.ceil(time_to_collision / time_quantization)) ]: if 0 <= lane < grid.shape[1] and 0 <= time < grid.shape[2]: grid[velocity_index, lane, time] = max(grid[velocity_index, lane, time], cost) return grid
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