def run_step(self, vd, vehicle_ahead): v = get_speed(self.vehicle) if vehicle_ahead is None: acc_cmd = self.a_max * (1 - (v / vd)**self.delta) else: loc1 = [ vehicle_ahead.get_location().x, vehicle_ahead.get_location().y, vehicle_ahead.get_location().z ] loc2 = [ self.vehicle.get_location().x, self.vehicle.get_location().y, self.vehicle.get_location().z ] d = euclidean_distance(loc1, loc2) v2 = get_speed(vehicle_ahead) dv = v - v2 d_star = self.d0 + max( 0, v * self.T + v * dv / (2 * math.sqrt(self.b * self.a_max))) acc_cmd = self.a_max * (1 - (v / vd)**self.delta - (d_star / d)**2) cmdSpeed = get_speed(self.vehicle) + acc_cmd * self.dt # if self.vehicle.attributes['role_name'] == 'hero': # print(v, cmdSpeed, acc_cmd) # print([x * 3.6 for x in [cmdSpeed, acc_cmd, v, vd]]) return cmdSpeed
def _tailgating(self, location, waypoint, vehicle_list): """ This method is in charge of tailgating behaviors. :param location: current location of the agent :param waypoint: current waypoint of the agent :param vehicle_list: list of all the nearby vehicles """ left_turn = waypoint.left_lane_marking.lane_change right_turn = waypoint.right_lane_marking.lane_change left_wpt = waypoint.get_left_lane() right_wpt = waypoint.get_right_lane() behind_vehicle_state, behind_vehicle, _ = self._bh_is_vehicle_hazard(waypoint, location, vehicle_list, max( self.behavior.min_proximity_threshold, self.speed_limit / 2), up_angle_th=180, low_angle_th=160) if behind_vehicle_state and self.speed < get_speed(behind_vehicle): if (right_turn == carla.LaneChange.Right or right_turn == carla.LaneChange.Both) and waypoint.lane_id * right_wpt.lane_id > 0 and right_wpt.lane_type == carla.LaneType.Driving: new_vehicle_state, _, _ = self._bh_is_vehicle_hazard(waypoint, location, vehicle_list, max( self.behavior.min_proximity_threshold, self.speed_limit / 2), up_angle_th=180, lane_offset=1) if not new_vehicle_state: print("Tailgating, moving to the right!") self.behavior.tailgate_counter = 200 self.set_destination(right_wpt.transform.location, self.end_waypoint.transform.location, clean=True) elif left_turn == carla.LaneChange.Left and waypoint.lane_id * left_wpt.lane_id > 0 and left_wpt.lane_type == carla.LaneType.Driving: new_vehicle_state, _, _ = self._bh_is_vehicle_hazard(waypoint, location, vehicle_list, max( self.behavior.min_proximity_threshold, self.speed_limit / 2), up_angle_th=180, lane_offset=-1) if not new_vehicle_state: print("Tailgating, moving to the left!") self.behavior.tailgate_counter = 200 self.set_destination(left_wpt.transform.location, self.end_waypoint.transform.location, clean=True)
def car_following_manager(self, vehicle, distance, debug=False): """ Module in charge of car-following behaviors when there's someone in front of us. :param vehicle: car to follow :param distance: distance from vehicle :param debug: boolean for debugging :return control: carla.VehicleControl """ vehicle_speed = get_speed(vehicle) delta_v = max(1, (self.speed - vehicle_speed) / 3.6) ttc = distance / delta_v if delta_v != 0 else distance / np.nextafter(0., 1.) # Under safety time distance, slow down. if self.behavior.safety_time > ttc > 0.0: control = self._local_planner.run_step( target_speed=min(positive(vehicle_speed - self.behavior.speed_decrease), min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist)), debug=debug) # Actual safety distance area, try to follow the speed of the vehicle in front. elif 2 * self.behavior.safety_time > ttc >= self.behavior.safety_time: control = self._local_planner.run_step( target_speed=min(max(self.min_speed, vehicle_speed), min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist)), debug=debug) # Normal behavior. else: control = self._local_planner.run_step( target_speed= min(self.behavior.max_speed, self.speed_limit - self.behavior.speed_lim_dist), debug=debug) return control
def state(self): """ frenet state of the ego vehicle. :return: [X, Y, PSI, Velocity(m/s), Xi, Eta, Theta, last_u1, last_u2, kappa] """ vehicle_location = self._vehicle.get_location() vehicle_transform = self._vehicle.get_transform() angle_wp = wp.get_wp_angle(self._wp_current, self._wp_next) angle_xy = wp.get_angle2wp_line(vehicle_transform, self._wp_current, self._wp_next) eta = -1 * np.sign(angle_xy) * wp.get_distance2wp( vehicle_transform, self._wp_current, self._wp_next) return np.array([ vehicle_location.x, # X vehicle_location.y, # Y wrap2pi( np.deg2rad(round(self._vehicle.get_transform().rotation.yaw, 3))), # PSI [rad] get_speed(self._vehicle) / 3.6, # Velocity [m/s] 0, # xi eta, # eta wrap2pi( np.deg2rad(round(self._vehicle.get_transform().rotation.yaw, 3)) - angle_wp), # theta self._last_control[0], # last u0 self._last_control[1], # last u1 # self._kr, # kappa ])
def run_step(self): """Execute one step of navigation.""" hazard_detected = False # Retrieve all relevant actors actor_list = self._world.get_actors() vehicle_list = actor_list.filter("*vehicle*") lights_list = actor_list.filter("*traffic_light*") vehicle_speed = get_speed(self._vehicle) / 3.6 # Check for possible vehicle obstacles max_vehicle_distance = self._base_vehicle_threshold + vehicle_speed affected_by_vehicle, _, _ = self._vehicle_obstacle_detected( vehicle_list, max_vehicle_distance) if affected_by_vehicle: hazard_detected = True # Check if the vehicle is affected by a red traffic light max_tlight_distance = self._base_tlight_threshold + vehicle_speed affected_by_tlight, _ = self._affected_by_traffic_light( lights_list, max_tlight_distance) if affected_by_tlight: hazard_detected = True control = self._local_planner.run_step() if hazard_detected: control = self.add_emergency_stop(control) return control
def _is_vehicle_close(self, vehicle_list): """ Check if a given vehicle is an obstacle in our way. To this end we take into account the road and lane the target vehicle is on and run a geometry test to check if the target vehicle is under a certain distance in front of our ego vehicle. WARNING: This method is an approximation that could fail for very large vehicles, which center is actually on a different lane but their extension falls within the ego vehicle lane. :param vehicle_list: list of potential obstacle to check :return: a tuple given by (bool_flag, vehicle), where - bool_flag is True if there is a vehicle ahead blocking us and False otherwise - vehicle is the blocker object itself """ vehicle, distance = self.get_closest_vehicle_ahead( vehicle_list, max(10 * 1.17, get_speed(self._vehicle) / 2.3) * 1.2) if (vehicle != None): return (True, vehicle) return (False, None)
def update_information(self): """ This method updates the information regarding the ego vehicle based on the surrounding world. """ self.speed = get_speed(self.vehicle) self.speed_limit = self.vehicle.get_speed_limit() self._local_planner.set_speed(self.speed_limit) self.direction = self._local_planner.target_road_option if self.direction is None: self.direction = RoadOption.LANEFOLLOW self.look_ahead_steps = int((self.speed_limit) / 10) self.incoming_waypoint, self.incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( steps=self.look_ahead_steps) if self.incoming_direction is None: self.incoming_direction = RoadOption.LANEFOLLOW self.is_at_traffic_light = self.vehicle.is_at_traffic_light() if self.ignore_traffic_light: self.light_state = "Green" else: # This method also includes stop signs and intersections. self.light_state = str(self.vehicle.get_traffic_light_state())
def run_step(self, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: control to be applied """ if self._follow_speed_limits: self._target_speed = self._vehicle.get_speed_limit() # Add more waypoints too few in the horizon if not self._stop_waypoint_creation and len( self._waypoints_queue) < self._min_waypoint_queue_length: self._compute_next_waypoints(k=self._min_waypoint_queue_length) # Purge the queue of obsolete waypoints veh_location = self._vehicle.get_location() vehicle_speed = get_speed(self._vehicle) / 3.6 self._min_distance = self._base_min_distance + 0.5 * vehicle_speed num_waypoint_removed = 0 for waypoint, _ in self._waypoints_queue: if len(self._waypoints_queue) - num_waypoint_removed == 1: min_distance = 1 # Don't remove the last waypoint until very close by else: min_distance = self._min_distance if veh_location.distance( waypoint.transform.location) < min_distance: num_waypoint_removed += 1 else: break if num_waypoint_removed > 0: for _ in range(num_waypoint_removed): self._waypoints_queue.popleft() # Get the target waypoint and move using the PID controllers. Stop if no target waypoint if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False else: self.target_waypoint, self.target_road_option = self._waypoints_queue[ 0] control = self._vehicle_controller.run_step( self._target_speed, self.target_waypoint) if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) return control
def run_step(self, target_speed): """ Execute one step of longitudinal control to reach a given target speed. :param target_speed: target speed in m/s :return: throttle control in the range [0, 1] """ current_speed = get_speed(self._vehicle) return self._pid_control(target_speed, current_speed), current_speed
def run_step(self, target_speed, debug=False) -> np.ndarray: """ Execute one step of longitudinal control to reach a given target speed. :param target_speed: target speed in Km/h :return: throttle control in the range [0, 1] """ current_speed = get_speed(self._vehicle) if debug: print('Current speed = {}'.format(current_speed)) return self._pid_control(target_speed, current_speed)
def run_step(self, target_speed, debug=False): """ Execute one step of longitudinal control to reach a given target speed. :param target_speed: target speed in Km/h :param debug: boolean for debugging :return: throttle control """ current_speed = get_speed(self._vehicle) if debug: print('Current speed = {}'.format(current_speed)) return self._pid_control(target_speed, current_speed)
def _take_action(self, action, sp): mps_kmph_conversion = 3.6 target_speed = 0.0 # accelerate if action == 0: current_speed = get_speed(self.ego_vehicle) / mps_kmph_conversion desired_speed = current_speed + 0.2 desired_speed *= 3.6 self.current_speed = desired_speed self.planner.local_planner.set_speed(desired_speed) control = self.planner.run_step() control.brake = 0.0 self.ego_vehicle.apply_control(control) # decelerate elif action == 1: current_speed = get_speed(self.ego_vehicle) / mps_kmph_conversion desired_speed = current_speed - 0.2 desired_speed *= 3.6 self.current_speed = desired_speed self.planner.local_planner.set_speed(desired_speed) control = self.planner.run_step() control.brake = 0.0 self.ego_vehicle.apply_control(control) elif action == 2: # emergency stop self.emergency_stop() # speed tracking elif action == 3: self.planner.local_planner.set_speed(self.current_speed) control = self.planner.run_step() control.brake = 0.0 self.ego_vehicle.apply_control(control)
def speed_setting(self): global target_vehicle target_vehicle = None ego_vehicle = self._vehicle ego_vehicle_location = self._vehicle.get_location() # ego_vehicle_waypoint = self._world.get_map().get_waypoint(ego_vehicle_location) vehicle_list = self._world.get_actors().filter("*vehicle*") car_number = len(vehicle_list) if car_number > 1: # if target_vehicle is None: # for index in vehicle_list: # if index.id != ego_vehicle.id: # target_vehicle = index # break # else: # pass # else: # target_vehicle_location = target_vehicle.get_location() # # target_vehicle_waypoint = self._world.get_map().get_waypoint(target_vehicle_location) # # if target_vehicle_waypoint.road_id == ego_vehicle_waypoint.road_id or\ # # target_vehicle_waypoint.lane_id == ego_vehicle_waypoint.lane_id: # offset = abs(ego_vehicle_location.y - target_vehicle_location.y) # if offset < 1.7: # return get_speed(target_vehicle) # else: # return 60 # target_vehicle = vehicle_list[0] for index in vehicle_list: if index.id != ego_vehicle.id: target_vehicle = index break else: pass # target_vehicle_waypoint = self._world.get_map().get_waypoint(target_vehicle_location) # if target_vehicle_waypoint.road_id == ego_vehicle_waypoint.road_id or\ # target_vehicle_waypoint.lane_id == ego_vehicle_waypoint.lane_id: target_vehicle_location = target_vehicle.get_location() offset = abs(ego_vehicle_location.x - target_vehicle_location.x) if offset < 1.77: return get_speed(target_vehicle) else: return 20 else: return 20
def state(self): """ xy-position state of the ego vehicle. :return: [X, Y, PSI, Velocity(m/s), last_u1, last_u2] """ vehicle_location = self._vehicle.get_location() return np.array([ vehicle_location.x, # X vehicle_location.y, # Y wrap2pi( np.deg2rad(round(self._vehicle.get_transform().rotation.yaw, 3))), # PSI [rad] get_speed(self._vehicle) / 3.6, # Velocity [m/s] self._last_control[0], # last u0 self._last_control[1], # last u1 ])
def collision_and_car_avoid_manager(self, location, waypoint): """ This module is in charge of warning in case of a collision and managing possible overtaking or tailgating chances. :param location: current location of the agent :param waypoint: current waypoint of the agent :return vehicle_state: True if there is a vehicle nearby, False if not :return vehicle: nearby vehicle :return distance: distance to nearby vehicle """ vehicle_list = self._world.get_actors().filter("*vehicle*") def dist(v): return v.get_location().distance(waypoint.transform.location) # og dist threshold is 45 vehicle_list = [v for v in vehicle_list if dist(v) < 25 and v.id != self.vehicle.id] if self.direction == RoadOption.CHANGELANELEFT: print('Change lane to left') vehicle_state, vehicle, distance = self._bh_is_vehicle_hazard( waypoint, location, vehicle_list, max( self.behavior.min_proximity_threshold, self.speed_limit / 2), up_angle_th=180, lane_offset=-1) elif self.direction == RoadOption.CHANGELANERIGHT: print('Change lane to right') vehicle_state, vehicle, distance = self._bh_is_vehicle_hazard( waypoint, location, vehicle_list, max( self.behavior.min_proximity_threshold, self.speed_limit / 2), up_angle_th=180, lane_offset=1) else: vehicle_state, vehicle, distance = self._bh_is_vehicle_hazard( waypoint, location, vehicle_list, max( # increase up_angle_th to account for goal waypoint appear after turn self.behavior.min_proximity_threshold, self.speed_limit / 3),up_angle_th=30) # up_angle_th=100) # og up th: 30) # Check for overtaking if vehicle_state and self.direction == RoadOption.LANEFOLLOW and \ not waypoint.is_junction and self.speed > 10 \ and self.behavior.overtake_counter == 0 and self.speed > get_speed(vehicle): print("!! start over taking !!") self._overtake(location, waypoint, vehicle_list) # Check for tailgating elif not vehicle_state and self.direction == RoadOption.LANEFOLLOW \ and not waypoint.is_junction and self.speed > 10 \ and self.behavior.tailgate_counter == 0: self._tailgating(location, waypoint, vehicle_list) return vehicle_state, vehicle, distance
def Ego_Perception(self): """ Currently, getting from the server directly """ self.ego_vehicle_location = self.ego_vehicle.get_location() self.ego_vehicle_speed = get_speed(self.ego_vehicle)/3.6 ###### m/s self.ego_vehicle_transform = self.ego_vehicle.get_transform() self.dt = 0.05 self.speed_limit = self.ego_vehicle.get_speed_limit() ego_vehicle_waypoint = self.map.get_waypoint(self.ego_vehicle_location) if ego_vehicle_waypoint.is_junction: self.in_intersection = True else: self.in_intersection = False self.dt = self._clock.dt()
def _update_information(self): """ This method updates the information regarding the ego vehicle based on the surrounding world. """ self._speed = get_speed(self._vehicle) self._speed_limit = self._vehicle.get_speed_limit() self._local_planner.set_speed(self._speed_limit) self._direction = self._local_planner.target_road_option if self._direction is None: self._direction = RoadOption.LANEFOLLOW self._look_ahead_steps = int((self._speed_limit) / 10) self._incoming_waypoint, self._incoming_direction = self._local_planner.get_incoming_waypoint_and_direction( steps=self._look_ahead_steps) if self._incoming_direction is None: self._incoming_direction = RoadOption.LANEFOLLOW
def run_step(self, target_speed, waypoint): """ Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint at a given target_speed. :param target_speed: desired vehicle speed :param waypoint: target location encoded as a waypoint :return: distance (in meters) to the waypoint """ throttle = self._lon_controller.run_step(target_speed) steering = self._lat_controller.run_step(waypoint) control = carla.VehicleControl() control.steer = steering control.throttle = throttle control.brake = 0.0 if target_speed + 10.0 > get_speed(self._vehicle) else 1.0 control.hand_brake = False control.manual_gear_shift = False return control
def run_step(self, target_speed, target_distance, debug=False): """ Execute one step of longitudinal control to reach a given target speed. :param target_speed: target speed in Km/h :return: throttle control in the range [0, 1] """ current_speed = get_speed(self._vehicle) if self._front_vehicle is not None: dx = self._vehicle.get_location().x - self._front_vehicle.get_location().x dy = self._vehicle.get_location().y - self._front_vehicle.get_location().y current_distance = math.sqrt(dx * dx + dy * dy) if debug: print('Current speed = {}'.format(current_speed)) return self._pid_control(target_speed, current_speed, target_distance, current_distance) else: return self._pid_control(target_speed, current_speed, 0.0, 0.0)
def _update_target_speed(self, hazard_detected, debug): if hazard_detected: self._set_target_speed(0) return 0 MAX_PERCENTAGE_OF_SPEED_LIMIT = 0.75 speed_limit = self._vehicle.get_speed_limit() # km/h current_speed = get_speed(self._vehicle) new_target_speed = speed_limit * MAX_PERCENTAGE_OF_SPEED_LIMIT use_custom_traffic_light_speed = False if use_custom_traffic_light_speed: TRAFFIC_LIGHT_SECONDS_AWAY = 3 METERS_TO_STOP_BEFORE_TRAFFIC_LIGHT = 8 get_traffic_light = self._vehicle.get_traffic_light( ) # type: carla.TrafficLight nearest_traffic_light, distance = get_nearest_traffic_light( self._vehicle) # type: carla.TrafficLight, float distance_to_light = distance distance -= METERS_TO_STOP_BEFORE_TRAFFIC_LIGHT if nearest_traffic_light is None: nearest_traffic_light = get_traffic_light # Draw debug info if debug and nearest_traffic_light is not None: self._world.debug.draw_point( nearest_traffic_light.get_transform().location, size=1, life_time=0.1, color=carla.Color(255, 15, 15)) """ if get_traffic_light is not None: print("get_traffic_light: ", get_traffic_light.get_location() if get_traffic_light is not None else "None", " ", get_traffic_light.state if get_traffic_light is not None else "None") if nearest_traffic_light is not None: print("nearest_traffic_light: ", nearest_traffic_light.get_location() if nearest_traffic_light is not None else "None", " ", nearest_traffic_light.state if nearest_traffic_light is not None else "None") """ ego_vehicle_location = self._vehicle.get_location() ego_vehicle_waypoint = self._map.get_waypoint(ego_vehicle_location) self.is_affected_by_traffic_light = False self.stopping_for_traffic_light = False if ego_vehicle_waypoint.is_junction: # It is too late. Do not block the intersection! Keep going! pass # Check if we should start braking elif distance_to_light <= TRAFFIC_LIGHT_SECONDS_AWAY * new_target_speed / 3.6 and nearest_traffic_light is not None and nearest_traffic_light.state != carla.TrafficLightState.Green: self.is_affected_by_traffic_light = True brake_distance = current_speed / 3.6 * TRAFFIC_LIGHT_SECONDS_AWAY print("TL distance: ", distance_to_light, ", distance (to stop): ", distance, ", distance travel 4 secs: ", brake_distance) new_target_speed = self._target_speed if distance <= 0: new_target_speed = 0 self.stopping_for_traffic_light = True print("Stopping before traffic light, distance ", distance, "m") elif brake_distance >= distance and brake_distance != 0: percent_before_light = (brake_distance - distance) / brake_distance new_target_speed = speed_limit - max( 0, percent_before_light) * speed_limit print("Slowing down before traffic light ", percent_before_light * 100, "% ", new_target_speed, " km/h") self._set_target_speed(max(0, new_target_speed)) return new_target_speed
def run_step(self, debug=False): """ Execute one step of navigation. :return: carla.VehicleControl """ # is there an obstacle in front of us? hazard_detected = False # retrieve relevant elements for safe navigation, i.e.: traffic lights # and other vehicles actor_list = self._world.get_actors() # type: ActorList vehicle_list = actor_list.filter("*vehicle*") # type: List[Actor] pedestrians_list = actor_list.filter("*walker.pedestrian*") lights_list = actor_list.filter( "*traffic_light*") # type: List[carla.TrafficLight] if not self.drawn_lights and debug: for light in lights_list: self._world.debug.draw_box( carla.BoundingBox( light.trigger_volume.location + light.get_transform().location, light.trigger_volume.extent * 2), carla.Rotation(0, 0, 0), 0.05, carla.Color(255, 128, 0, 0), 0) self.drawn_lights = True # check possible obstacles vehicle_state, vehicle = self._is_vehicle_hazard(vehicle_list) if vehicle_state: if debug: print('!!! VEHICLE BLOCKING AHEAD [{}])'.format(vehicle.id)) self._state = AgentState.BLOCKED_BY_VEHICLE hazard_detected = True # Check for pedestrians pedestrian_state, pedestrian = self._is_pedestrian_hazard( pedestrians_list) if pedestrian_state: if debug: print('!!! PEDESTRIAN BLOCKING AHEAD [{}])'.format( pedestrian.id)) self._state = AgentState.BLOCKED_BY_VEHICLE hazard_detected = True # check for the state of the traffic lights light_state, traffic_light = self._is_light_red(lights_list) if light_state: if debug: print('=== RED LIGHT AHEAD [{}])'.format(traffic_light.id)) self._state = AgentState.BLOCKED_RED_LIGHT hazard_detected = True new_target_speed = self._update_target_speed(hazard_detected, debug) # if hazard_detected: # control = self.emergency_stop() # else: # self._state = AgentState.NAVIGATING # self.braking_intial_speed = None # # standard local planner behavior # control = self._local_planner.run_step(debug=debug) # if self.stopping_for_traffic_light: # control.steer = 0.0 self._state = AgentState.NAVIGATING self.braking_intial_speed = None # standard local planner behavior control = self._local_planner.run_step(debug=debug) if self.stopping_for_traffic_light: control.steer = 0.0 # Prevent from steering randomly when stopped if math.fabs(get_speed(self._vehicle)) < 0.1: control.steer = 0 return control
def step(self, action=None): state = [0, 0] self.n_step += 1 track_finished = False """ ********************************************************************************************************************** *********************************************** Behavior Planner ***************************************************** ********************************************************************************************************************** """ # self.MOBIL.run_step(self.f_state, self.traffic_module.actors_batch) # change_lane = 0 # random.randint(-1, 1) """ ********************************************************************************************************************** *********************************************** Motion Planner ******************************************************* ********************************************************************************************************************** """ temp = [self.ego.get_velocity(), self.ego.get_acceleration()] speed = get_speed(self.ego) acc_vec = self.ego.get_acceleration() acc = math.sqrt(acc_vec.x**2 + acc_vec.y**2 + acc_vec.z**2) psi = math.radians(self.ego.get_transform().rotation.yaw) ego_state = [ self.ego.get_location().x, self.ego.get_location().y, speed, acc, psi, temp, self.max_s ] # fpath = self.motionPlanner.run_step_single_path(ego_state, self.f_idx, df_n=action[0], Tf=5, Vf_n=action[1]) fpath, fplist, best_path_idx = self.motionPlanner.run_step( ego_state, self.f_idx, self.traffic_module.actors_batch, target_speed=self.targetSpeed) wps_to_go = len( fpath.t ) - 3 # -2 bc len gives # of items not the idx of last item + 2wp controller is used self.f_idx = 1 """ ********************************************************************************************************************** ************************************************* Controller ********************************************************* ********************************************************************************************************************** """ # initialize flags collision = track_finished = False elapsed_time = lambda previous_time: time.time() - previous_time path_start_time = time.time() # follows path until end of WPs for max 1.8seconds loop_counter = 0 while self.f_idx < wps_to_go: loop_counter += 1 # for _ in range(wps_to_go): # self.f_idx += 1 ego_location = [ self.ego.get_location().x, self.ego.get_location().y, math.radians(self.ego.get_transform().rotation.yaw) ] self.f_idx = closest_wp_idx(ego_location, fpath, self.f_idx) cmdSpeed = math.sqrt((fpath.s_d[self.f_idx])**2 + (fpath.d_d[self.f_idx])**2) cmdWP = [fpath.x[self.f_idx], fpath.y[self.f_idx]] cmdWP2 = [fpath.x[self.f_idx + 1], fpath.y[self.f_idx + 1]] # IDM for ego (Overwrite the desired speed) vehicle_ahead = self.world_module.los_sensor.get_vehicle_ahead() cmdSpeed = self.IDM.run_step(vd=cmdSpeed, vehicle_ahead=vehicle_ahead) # control = self.vehicleController.run_step(cmdSpeed, cmdWP) # calculate control control = self.vehicleController.run_step_2_wp( cmdSpeed, cmdWP, cmdWP2) # calculate control self.ego.apply_control(control) # apply control # print(fpath.s[self.f_idx], self.ego.get_transform().rotation.yaw) """ ********************************************************************************************************************** *********************************************** Draw Waypoints ******************************************************* ********************************************************************************************************************** """ # three layer waypoints to draw: self.world_module.points_to_draw = [{}, {}, {}] if self.world_module.args.play_mode != 0: for j, path in enumerate(fplist): path_name = 'path {}'.format(j) if j != best_path_idx: layer = 0 color = 'COLOR_SKY_BLUE_0' else: layer = 1 color = 'COLOR_ALUMINIUM_0' waypoints = [] for i in range(len(path.t)): waypoints.append( carla.Location(x=path.x[i], y=path.y[i])) self.world_module.points_to_draw[layer][path_name] = { 'waypoints': waypoints, 'color': color } layer = 2 self.world_module.points_to_draw[layer]['ego'] = { 'waypoints': [self.ego.get_location()], 'color': 'COLOR_SCARLET_RED_0' } self.world_module.points_to_draw[layer]['waypoint ahead'] = { 'waypoints': [carla.Location(x=cmdWP[0], y=cmdWP[1])], 'color': 'COLOR_SCARLET_RED_0' } self.world_module.points_to_draw[layer]['waypoint ahead'] = { 'waypoints': [carla.Location(x=cmdWP2[0], y=cmdWP2[1])], 'color': 'COLOR_SCARLET_RED_0' } """ ********************************************************************************************************************** ************************************************ Update Carla ******************************************************** ********************************************************************************************************************** """ ego_s, ego_d = fpath.s[self.f_idx], fpath.d[self.f_idx] state = [ego_s, ego_d] self.module_manager.tick() # Update carla world if self.auto_render: self.render() collision_hist = self.world_module.get_collision_history() if any(collision_hist): collision = True break distance_traveled = ego_s - self.init_s if distance_traveled < -5: distance_traveled = self.max_s + distance_traveled if distance_traveled >= self.track_length: track_finished = True break if loop_counter >= self.loop_break: break self.f_state = [ fpath.s[self.f_idx], fpath.d[self.f_idx], fpath.s_d[self.f_idx], fpath.d_d[self.f_idx], fpath.s_dd[self.f_idx], fpath.d_dd[self.f_idx] ] """ ********************************************************************************************************************** ********************************************* Episode Termination **************************************************** ********************************************************************************************************************** """ done = False if collision: # print('Collision happened!') reward = -10 done = True # print('eps rew: ', self.n_step, self.eps_rew) return state, reward, done, {'reserved': 0} if track_finished: # print('Finished the race') reward = 10 done = True # print('eps rew: ', self.n_step, self.eps_rew) return state, reward, done, {'reserved': 0} reward = 1 return state, reward, done, {'reserved': 0}
def step(self, action=None): self.n_step += 1 self.actor_enumerated_dict['EGO'] = { 'NORM_S': [], 'NORM_D': [], 'S': [], 'D': [], 'SPEED': [] } if self.verbosity: print( 'ACTION'.ljust(15), '{:+8.6f}, {:+8.6f}'.format(float(action[0]), float(action[1]))) if self.is_first_path: # Episode start is bypassed action = [0, -1] self.is_first_path = False """ ********************************************************************************************************************** *********************************************** Motion Planner ******************************************************* ********************************************************************************************************************** """ temp = [self.ego.get_velocity(), self.ego.get_acceleration()] init_speed = speed = get_speed(self.ego) acc_vec = self.ego.get_acceleration() acc = math.sqrt(acc_vec.x**2 + acc_vec.y**2 + acc_vec.z**2) psi = math.radians(self.ego.get_transform().rotation.yaw) ego_state = [ self.ego.get_location().x, self.ego.get_location().y, speed, acc, psi, temp, self.max_s ] fpath, self.lanechange, off_the_road = self.motionPlanner.run_step_single_path( ego_state, self.f_idx, df_n=action[0], Tf=5, Vf_n=action[1]) wps_to_go = len( fpath.t ) - 3 # -2 bc len gives # of items not the idx of last item + 2wp controller is used self.f_idx = 1 """ ********************************************************************************************************************** ************************************************* Controller ********************************************************* ********************************************************************************************************************** """ # initialize flags collision = track_finished = False elapsed_time = lambda previous_time: time.time() - previous_time path_start_time = time.time() ego_init_d, ego_target_d = fpath.d[0], fpath.d[-1] # follows path until end of WPs for max 1.5 * path_time or loop counter breaks unless there is a langechange loop_counter = 0 while self.f_idx < wps_to_go and ( elapsed_time(path_start_time) < self.motionPlanner.D_T * 1.5 or loop_counter < self.loop_break or self.lanechange): loop_counter += 1 ego_state = [ self.ego.get_location().x, self.ego.get_location().y, math.radians(self.ego.get_transform().rotation.yaw), 0, 0, temp, self.max_s ] self.f_idx = closest_wp_idx(ego_state, fpath, self.f_idx) cmdWP = [fpath.x[self.f_idx], fpath.y[self.f_idx]] cmdWP2 = [fpath.x[self.f_idx + 1], fpath.y[self.f_idx + 1]] # overwrite command speed using IDM ego_s = self.motionPlanner.estimate_frenet_state( ego_state, self.f_idx)[0] # estimated current ego_s ego_d = fpath.d[self.f_idx] vehicle_ahead = self.get_vehicle_ahead(ego_s, ego_d, ego_init_d, ego_target_d) cmdSpeed = self.IDM.run_step(vd=self.targetSpeed, vehicle_ahead=vehicle_ahead) # control = self.vehicleController.run_step(cmdSpeed, cmdWP) # calculate control control = self.vehicleController.run_step_2_wp( cmdSpeed, cmdWP, cmdWP2) # calculate control self.ego.apply_control(control) # apply control """ ********************************************************************************************************************** *********************************************** Draw Waypoints ******************************************************* ********************************************************************************************************************** """ if self.world_module.args.play_mode != 0: for i in range(len(fpath.t)): self.world_module.points_to_draw['path wp {}'.format( i)] = [ carla.Location(x=fpath.x[i], y=fpath.y[i]), 'COLOR_ALUMINIUM_0' ] self.world_module.points_to_draw['ego'] = [ self.ego.get_location(), 'COLOR_SCARLET_RED_0' ] self.world_module.points_to_draw[ 'waypoint ahead'] = carla.Location(x=cmdWP[0], y=cmdWP[1]) self.world_module.points_to_draw[ 'waypoint ahead 2'] = carla.Location(x=cmdWP2[0], y=cmdWP2[1]) """ ********************************************************************************************************************** ************************************************ Update Carla ******************************************************** ********************************************************************************************************************** """ self.module_manager.tick() # Update carla world if self.auto_render: self.render() collision_hist = self.world_module.get_collision_history() self.actor_enumerated_dict['EGO']['S'].append(ego_s) self.actor_enumerated_dict['EGO']['D'].append(ego_d) self.actor_enumerated_dict['EGO']['NORM_S'].append( (ego_s - self.init_s) / self.track_length) self.actor_enumerated_dict['EGO']['NORM_D'].append( round((ego_d + self.LANE_WIDTH) / (3 * self.LANE_WIDTH), 2)) last_speed = get_speed(self.ego) self.actor_enumerated_dict['EGO']['SPEED'].append(last_speed / self.maxSpeed) # if ego off-the road or collided if any(collision_hist): collision = True break distance_traveled = ego_s - self.init_s if distance_traveled < -5: distance_traveled = self.max_s + distance_traveled if distance_traveled >= self.track_length: track_finished = True """ ********************************************************************************************************************* *********************************************** RL Observation ****************************************************** ********************************************************************************************************************* """ if cfg.GYM_ENV.FIXED_REPRESENTATION: self.state = self.fix_representation() if self.verbosity == 2: print(3 * '---EPS UPDATE---') print( TENSOR_ROW_NAMES[0].ljust(15), # '{:+8.6f} {:+8.6f}'.format(self.state[-1][1], self.state[-1][0])) '{:+8.6f}'.format(self.state[-1][0])) for idx in range(1, self.state.shape[1]): print(TENSOR_ROW_NAMES[idx].ljust(15), '{:+8.6f}'.format(self.state[-1][idx])) if self.verbosity == 3: print(self.state) """ ********************************************************************************************************************** ********************************************* RL Reward Function ***************************************************** ********************************************************************************************************************** """ e_speed = abs(self.targetSpeed - last_speed) r_speed = self.w_r_speed * np.exp( -e_speed**2 / self.maxSpeed * self.w_speed) # 0<= r_speed <= self.w_r_speed # first two path speed change increases regardless so we penalize it differently spd_change_percentage = ( last_speed - init_speed) / init_speed if init_speed != 0 else -1 r_laneChange = 0 if self.lanechange and spd_change_percentage < self.min_speed_gain: r_laneChange = -1 * r_speed * self.lane_change_penalty # <= 0 elif self.lanechange: r_speed *= self.lane_change_reward positives = r_speed negatives = r_laneChange reward = positives + negatives # r_speed * (1 - lane_change_penalty) <= reward <= r_speed * lane_change_reward # print(self.n_step, self.eps_rew) """ ********************************************************************************************************************** ********************************************* Episode Termination **************************************************** ********************************************************************************************************************** """ done = False if collision: # print('Collision happened!') reward = self.collision_penalty done = True self.eps_rew += reward # print('eps rew: ', self.n_step, self.eps_rew) if self.verbosity: print('REWARD'.ljust(15), '{:+8.6f}'.format(reward)) return self.state, reward, done, {'reserved': 0} elif track_finished: # print('Finished the race') # reward = 10 done = True if off_the_road: reward = self.off_the_road_penalty self.eps_rew += reward # print('eps rew: ', self.n_step, self.eps_rew) if self.verbosity: print('REWARD'.ljust(15), '{:+8.6f}'.format(reward)) return self.state, reward, done, {'reserved': 0} elif off_the_road: # print('Collision happened!') reward = self.off_the_road_penalty # done = True self.eps_rew += reward # print('eps rew: ', self.n_step, self.eps_rew) if self.verbosity: print('REWARD'.ljust(15), '{:+8.6f}'.format(reward)) return self.state, reward, done, {'reserved': 0} self.eps_rew += reward # print(self.n_step, self.eps_rew) if self.verbosity: print('REWARD'.ljust(15), '{:+8.6f}'.format(reward)) return self.state, reward, done, {'reserved': 0}
def _model_predictive_control(self, target_speed, waypoints, vehicle_transform): # Transform points from world frame to car frame _x = vehicle_transform.location.x _y = vehicle_transform.location.y # _psi = vehicle_transform.rotation.yaw _psi = self.get_psi(vehicle_transform) R = np.array([[cos(-_psi), -sin(-_psi)], [sin(-_psi), cos(-_psi)]]) t = np.array([[_x], [_y]]) waypoints_world = np.array(waypoints) waypoints_car = np.array(waypoints) waypoints_car[:, 0:2] = np.dot( R, np.array([waypoints_world[:, 0], waypoints_world[:, 1]]) - t).T N = 10 dt = 0.1 Lf = 2.67 ref_v = target_speed # Define var start positions x_start = 0 y_start = x_start + N psi_start = y_start + N v_start = psi_start + N cte_start = v_start + N epsi_start = cte_start + N delta_start = epsi_start + N a_start = delta_start + N - 1 # State x = 0 y = 0 psi = 0 v = get_speed(self._vehicle) cte = self.get_cross_track_error([x, y], waypoints_car) epsi = self.get_epsi(vehicle_transform, waypoints_car) coeffs = self.get_coeffs(waypoints_car) # number of model variables # For example: If the [state] is a 4 element vector, the [actuators] is a 2 # element vector and there are 10 timesteps. The number of variables is: n_vars = N * 6 + (N - 1) * 2 # Set the number of constraints n_constraints = N * 6 # NLP variable vector vars = MX.sym('x', n_vars) vars_init = np.zeros(n_vars) # set initial variables values vars_init[x_start] = x vars_init[y_start] = y vars_init[psi_start] = psi vars_init[v_start] = v vars_init[cte_start] = cte vars_init[epsi_start] = epsi # upperbound and lowerbound vectors for vars vars_upperbound = np.zeros(n_vars) vars_lowerbound = np.zeros(n_vars) # Set all non-actuators upper and lowerlimits # to the max negative and positive values. vars_upperbound[:delta_start] = 1.0e9 vars_lowerbound[:delta_start] = -1.0e9 # Set the upper and lower limits of delta as -25 and 25 degrees vars_upperbound[delta_start:a_start] = 0.7 vars_lowerbound[delta_start:a_start] = -0.7 # Set the upper and lower limits of accelerations as -1 and 1 vars_upperbound[a_start:] = 0.7 vars_lowerbound[a_start:] = -0.7 # Lower and upper limits for the constraints # Should be 0 besides initial state. constraints_upperbound = np.zeros(n_constraints) constraints_lowerbound = np.zeros(n_constraints) constraints_lowerbound[x_start] = x constraints_lowerbound[y_start] = y constraints_lowerbound[psi_start] = psi constraints_lowerbound[v_start] = v constraints_lowerbound[cte_start] = cte constraints_lowerbound[epsi_start] = epsi constraints_upperbound[x_start] = x constraints_upperbound[y_start] = y constraints_upperbound[psi_start] = psi constraints_upperbound[v_start] = v constraints_upperbound[cte_start] = cte constraints_upperbound[epsi_start] = epsi # Object for defining objective and constraints f, g = self.operator(vars, coeffs, n_constraints, N, dt, ref_v, Lf) # NLP nlp = {'x': vars, 'f': f, 'g': vertcat(*g)} # print("g shape:", vertcat(*g).shape) ## ---- ## SOLVE THE NLP ## ---- # Set options opts = {} opts["expand"] = True #opts["ipopt.max_iter"] = 4 opts["ipopt.linear_solver"] = 'ma27' opts["ipopt.print_level"] = 0 opts["ipopt.sb"] = "yes" opts["print_time"] = 0 # Allocate an NLP solver solver = nlpsol("solver", "ipopt", nlp, opts) arg = {} # Initial condition arg["x0"] = vars_init # print("x0 shape: ", vars_init.shape) # Bounds on x arg["lbx"] = vars_lowerbound arg["ubx"] = vars_upperbound # print("lbx shape: ", vars_lowerbound.shape) # Bounds on g arg["lbg"] = constraints_lowerbound arg["ubg"] = constraints_upperbound # print("ubg: ", constraints_upperbound.shape) # Solve the problem res = solver(**arg) vars_opt = np.array(res["x"]) x_mpc = vars_opt[x_start:y_start] y_mpc = vars_opt[y_start:psi_start] steering = vars_opt[delta_start:a_start] accelerations = vars_opt[a_start:] # print("steering: ", steering) # print("accelerations: ", accelerations) throttle_output = accelerations * 0 brake_output = accelerations * 0 for i in range(N - 1): if accelerations[i] > 0: throttle_output[i] = accelerations[i] brake_output[i] = 0 else: throttle_output[i] = 0 brake_output[i] = -accelerations[i] steer_output = steering ''' print("================= MPC ======================") print("steer_output: ", steer_output[0]) print("throttle_output: ", throttle_output[0]) print("brake_output: ", brake_output[0]) print("============================================") print(" ") ''' return throttle_output[0], brake_output[0], steer_output[0]
def get_ego_speed(self): return get_speed(self.ego_vehicle)
def run_step(self, debug): """ Execute one step of navigation. :Check for lights, junction block and proximity block :return: carla.VehicleControl """ global time_count global lane_change global next_lane_waypoint # is there an obstacle in front of us? hazard_detected = False # retrieve relevant elements for safe navigation, i.e.: traffic lights # and other vehicles, static props (occupancy grid maps) actor_list = self._world.get_actors() vehicle_list = actor_list.filter("*vehicle*") lights_list = actor_list.filter("*traffic_light*") prop_list = actor_list.filter("*static.prop.streetbarrier*") if (len(prop_list) == 0 and len(actor_list.filter("*static.prop.container*")) != 0): prop_list = actor_list.filter("*static.prop.container*") #print(f'=============== prop list {prop_list}') #Get location and current waypoint location = self.vehicle.get_location() waypoint = self.world_map.get_waypoint(location) # check possible longitudinal and lateral obstacles vehicle_state, vehicle = self._is_vehicle_hazard(vehicle_list) if vehicle_state and not lane_change and not self.static_lane_change: if debug: print('!!! VEHICLE BLOCKING AHEAD [{}])'.format(vehicle.id)) time_count = time_count + 1 if (time_count > self._blocking_threshold): print( f'------- Blocked more than threshold Perform lane change manouver ---- {time_count}' ) print(f'current location {location}') lane_change = True next_lane_waypoint = waypoint.get_left_lane() print( f'Left lane--------{waypoint.get_left_lane().transform.location.x}-------{waypoint.get_left_lane().transform.location.y}' ) self._state = AgentState.BLOCKED_BY_VEHICLE hazard_detected = True # check for the state of the traffic lights light_state, traffic_light = self._is_light_red(lights_list) if light_state: if debug: print('=== RED LIGHT AHEAD [{}])'.format(traffic_light.id)) self._state = AgentState.BLOCKED_RED_LIGHT hazard_detected = True #Check the if there is a junction class between ego and another vehicle #If there is a junction class check for priority if (not self.vehicle_junction_hazard): #Return True only if another vehicle has higher priority self.vehicle_junction_hazard, self.vehicle_crossing = self._is_junction_hazard( vehicle_list) #Check if the priority vehicle is still in junction if self.vehicle_junction_hazard and self.world_map.get_waypoint( self.vehicle_crossing.get_location()).is_junction: self._state = AgentState.BLOCKED_IN_JUNCTION if debug: print( f'=== OTHER VEHICLE CROSSING JUNCTION {self.vehicle_crossing.id}' ) control = self.emergency_stop() return control else: self.vehicle_junction_hazard = False self.vehicle_crossing = None #Check for static obstacles on the route if (len(prop_list) != 0) and not self.static_lane_change: self.static_obstacle, prop = self._is_static_obstacle_ahead( prop_list) if self.static_obstacle: print( f'=== VEHICLE BLOCKED BY STATIC OBSTACLE======={prop.id}') next_lane_waypoint = waypoint.get_left_lane() print(f'next lane id ========== {next_lane_waypoint.lane_id}') print(f'current lane id ========== {waypoint.lane_id}') print( f'Next waypoint left lane=========={next_lane_waypoint.transform}' ) #Check if there is a vehicle within n meters self.static_lane_change = self._is_next_lane_clear( next_lane_waypoint, vehicle_list) print( f'Lane clearance value==========={self.static_lane_change}' ) #In case lane change clearance is not available stay stopped if not self.static_lane_change: print(f'I am STOPPED') control = self.emergency_stop() return control if hazard_detected and not lane_change: control = self.emergency_stop() elif lane_change: print(f'current transform {self.vehicle.get_transform()}') vehicle_transform = self.vehicle.get_transform() print( f'current location {next_lane_waypoint.lane_id}===={waypoint.lane_id}' ) print(f'next lane ============= {next_lane_waypoint.transform}') while (next_lane_waypoint.lane_id != waypoint.lane_id or abs( abs(int(vehicle_transform.rotation.yaw)) - abs(int(next_lane_waypoint.transform.rotation.yaw))) > 2): #Observation variables kmh = get_speed(self.vehicle) vehicle_transform = self.vehicle.get_transform() yaw_vehicle = vehicle_transform.rotation.yaw delta_yaw = abs( abs(int(vehicle_transform.rotation.yaw)) - abs(int(next_lane_waypoint.transform.rotation.yaw))) state = [kmh, yaw_vehicle, delta_yaw] state = np.reshape(state, (1, 3)) choice = self.left_lane_model.predict(state) action = self.choose_action_leftlanechange(np.argmax(choice)) print(f"action lane change ------------------> {action}") control = carla.VehicleControl(throttle=action[0], steer=action[1], brake=action[2], reverse=action[3]) return control lane_change = False control = self.nn_control() time_count = 0 #For subsequent steer around lane change maneuver in case of static obstacle elif self.static_lane_change: while (waypoint.lane_id != next_lane_waypoint.lane_id or self.vehicle.get_transform().rotation.yaw < -1.5 ) and not self.first_lane_reached: print(f'=======Left Lane Change=====') kmh = get_speed(self.vehicle) vehicle_transform = self.vehicle.get_transform() yaw_vehicle = vehicle_transform.rotation.yaw delta_yaw = abs( abs(int(179 + vehicle_transform.rotation.yaw)) - abs(int(next_lane_waypoint.transform.rotation.yaw))) lane_id = waypoint.lane_id state = [kmh, yaw_vehicle, lane_id, delta_yaw] state = np.reshape(state, (1, 4)) choice = self.left_lane_static.predict(state) action = self.choose_action_leftlanechange_static( np.argmax(choice)) control = carla.VehicleControl(throttle=action[0], steer=action[1], brake=action[2], reverse=action[3]) return control self.first_lane_reached = True next_lane_waypoint = waypoint.get_right_lane() while (waypoint.lane_id != -1 or self.vehicle.get_transform().rotation.yaw > 1): print(f'=======Right Lane Change=====') kmh = get_speed(self.vehicle) vehicle_transform = self.vehicle.get_transform() yaw_vehicle = vehicle_transform.rotation.yaw delta_yaw = abs( abs(int(vehicle_transform.rotation.yaw)) - abs(int(next_lane_waypoint.transform.rotation.yaw))) lane_id = waypoint.lane_id print(f'current transform {self.vehicle.get_transform()}') state = [kmh, yaw_vehicle, lane_id, delta_yaw] state = np.reshape(state, (1, 4)) choice = self.right_lane_static.predict(state) action = self.choose_action_rightlanechange_static( np.argmax(choice)) if (waypoint.lane_id == -1): action[1] = -0.14 control = carla.VehicleControl(throttle=action[0], steer=action[1], brake=action[2], reverse=action[3]) return control self.static_obstacle = False self.static_lane_change = False control = self.nn_control() else: #print(f'current transform {self.vehicle.get_transform()}') self._state = AgentState.NAVIGATING # standard local planner behavior control = self.nn_control() return control
def nn_control(self): #Retrieve the current option from if self.route_count < len(self.route_list): option = self.route_list[self.route_count][0] target_loc = self.route_list[self.route_count][2] curr_loc = (self.vehicle.get_location().x, self.vehicle.get_location().y) target_diff = target_magnitude(curr_loc, target_loc) #print(f'target difference ======={option}=========== {round(target_diff)}========{self.prev_target_diff}') if option == 'straight': #get_speed(self.vehicle) kmh = get_speed(self.vehicle) target_location = carla.Location(x=float(target_loc[0]), y=float(target_loc[1])) vehicle_waypoint = self.world_map.get_waypoint( self.vehicle.get_location()) current_location = vehicle_waypoint.transform.location norm_target = self.compute_magnitude(target_location, current_location) obs = [kmh, norm_target, 0, 15] obs = np.reshape(obs, (1, 4)) choice = self.straight_model.predict(obs) action = self.choose_action_straight(np.argmax(choice[0])) control = carla.VehicleControl(throttle=action[0], steer=action[1], brake=action[2], reverse=action[3]) #control = carla.VehicleControl(throttle=0.6, steer=0.0, brake=0.0, reverse=False) if option == 'right_turn': kmh = get_speed(self.vehicle) target_location = carla.Location(x=float(target_loc[0]), y=float(target_loc[1])) target_waypoint = self.world_map.get_waypoint(target_location) target_yaw = target_waypoint.transform.rotation.yaw vehicle_waypoint = self.world_map.get_waypoint( self.vehicle.get_location()) current_location = vehicle_waypoint.transform.location current_yaw = vehicle_waypoint.transform.rotation.yaw norm_target, diff_angle = self.compute_magnitude_angle( target_location, current_location, target_yaw, current_yaw) state = [norm_target, diff_angle, 0, kmh] state = np.reshape(state, (1, 4)) choice = self.right_turn_model.predict(state) action = self.choose_action_rightturn(np.argmax(choice)) #print(f'action right turn ------> {action}--------target yaw==={target_yaw}--current yaw===={current_yaw}') control = carla.VehicleControl(throttle=action[0], steer=action[1], brake=action[2], reverse=action[3]) #control = carla.VehicleControl(throttle=0.5, steer=0.2, brake=0.0, reverse=False) if option == 'left_turn': kmh = get_speed(self.vehicle) target_location = carla.Location(x=float(target_loc[0]), y=float(target_loc[1])) target_waypoint = self.world_map.get_waypoint(target_location) target_yaw = target_waypoint.transform.rotation.yaw vehicle_waypoint = self.world_map.get_waypoint( self.vehicle.get_location()) current_location = vehicle_waypoint.transform.location current_yaw = vehicle_waypoint.transform.rotation.yaw norm_target, diff_angle = self.compute_magnitude_angle( target_location, current_location, target_yaw, current_yaw) state = [norm_target, diff_angle, 0, kmh] state = np.reshape(state, (1, 4)) choice = self.left_turn_model.predict(state) action = self.choose_action_leftturn(np.argmax(choice)) #print(f'action right turn ------> {action}--------target yaw==={target_yaw}--current yaw===={current_yaw}') control = carla.VehicleControl(throttle=action[0], steer=action[1], brake=action[2], reverse=action[3]) if (round(target_diff) <= 1 or round(target_diff) > self.prev_target_diff): self.route_count = self.route_count + 1 self.prev_target_diff = 200 print(f'Count increased =====') else: self.prev_target_diff = round(target_diff) else: control = self.emergency_stop() return control
def run_step(self, timestep: int, debug=True, log=False): """ Execute one step of classic mpc controller which follow the waypoints trajectory. :param debug: boolean flag to activate waypoints debugging :return: """ start_time = time.time() # not enough waypoints in the horizon? => Sample new ones self._sampling_radius = calculate_step_distance( get_speed(self._vehicle), 0.2, factor=2) # factor 11 --> prediction horizon 10 steps if self._sampling_radius < 2: self._sampling_radius = 3 # Getting future waypoints self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self._next_wp_queue = compute_next_waypoints(self._current_waypoint, d=self._sampling_radius, k=20) # Getting waypoint history --> history somehow starts at last wp of future wp self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self._previous_wp_queue = compute_previous_waypoints( self._current_waypoint, d=self._sampling_radius, k=5) # concentrate history, current waypoint, future self._waypoint_buffer = deque(maxlen=self._buffer_size) self._waypoint_buffer.extendleft(self._previous_wp_queue) self._waypoint_buffer.append( (self._map.get_waypoint(self._vehicle.get_location()), RoadOption.LANEFOLLOW)) self._waypoint_buffer.extend(self._next_wp_queue) self._waypoints_queue = self._next_wp_queue # If no more waypoints in queue, returning emergency braking control if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False print("Applied emergency control !!!") return control # target waypoint for Frenet calculation self.wp1 = self._map.get_waypoint(self._vehicle.get_location()) self.wp2, _ = self._next_wp_queue[0] # current vehicle waypoint self.kappa_log = dict() self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) self.curv_args, self.kappa_log = self.calculate_curvature_func_args( log=True) self.curv_x0 = func_kappa(0, self.curv_args) # input for MPC controller --> wp_current, wp_next, kappa target_wps = [self.wp1, self.wp2, self.curv_x0] # move using MPC controller if log: if timestep / 30 > 8 and timestep / 30 < 9: # Setting manual control manual_control = [self.data_log.get('u_acceleration'), 0.01] target_wps.append(manual_control) print(manual_control) # apply manual control control, state, u, x_log, u_log, _ = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=False, manual=True, log=log) self.data_log = { 'X': state[0], 'Y': state[1], 'PSI': state[2], 'Velocity': state[3], 'Xi': state[4], 'Eta': state[5], 'Theta': state[6], 'u_acceleration': u[0], 'u_steering_angle': u[1], 'pred_states': [x_log], 'pred_control': [u_log], 'computation_time': time.time() - start_time, "kappa": self.curv_x0, "curvature_radius": 1 / self.curv_x0 } elif timestep / 30 > 14 and timestep / 30 < 14.6: # Setting manual control manual_control = [self.data_log.get('u_acceleration'), -0.02] target_wps.append(manual_control) # apply manual control control, state, u, x_log, u_log, _ = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=False, manual=True, log=log) self.data_log = { 'X': state[0], 'Y': state[1], 'PSI': state[2], 'Velocity': state[3], 'Xi': state[4], 'Eta': state[5], 'Theta': state[6], 'u_acceleration': u[0], 'u_steering_angle': u[1], 'pred_states': [x_log], 'pred_control': [u_log], 'computation_time': time.time() - start_time, "kappa": self.curv_x0, "curvature_radius": 1 / self.curv_x0 } else: if timestep % 6 == 0: control, state, u, u_log, x_log, _ = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=True, log=log) # Updating logging information of the logger self.data_log = { 'X': state[0], 'Y': state[1], 'PSI': state[2], 'Velocity': state[3], 'Xi': state[4], 'Eta': state[5], 'Theta': state[6], 'u_acceleration': u[0], 'u_steering_angle': u[1], 'pred_states': [x_log], 'pred_control': [u_log], 'computation_time': time.time() - start_time, "kappa": self.curv_x0, "curvature_radius": 1 / self.curv_x0 } else: control, state, prediction, u = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=False, log=log) # Updating logging information of the logger self.data_log = { 'X': state[0], 'Y': state[1], 'PSI': state[2], 'Velocity': state[3], 'Xi': state[4], 'Eta': state[5], 'Theta': state[6], 'u_acceleration': u[0], 'u_steering_angle': u[1], 'pred_states': [prediction], 'pred_control': self.data_log.get("pred_control"), 'computation_time': time.time() - start_time, "kappa": self.curv_x0, "curvature_radius": 1 / self.curv_x0 } else: if timestep % 6 == 0: control = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=True, log=False) else: control, _, _, _ = self._vehicle_controller.mpc_control( target_wps, self._target_speed, solve_nmpc=False, log=log) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() if debug: last_wp, _ = self._waypoint_buffer[len(self._waypoint_buffer) - 1] draw_waypoints(self._vehicle.get_world(), [self.wp1, self.wp2], self._vehicle.get_location().z + 1.0) draw_waypoints_debug(self._vehicle.get_world(), [last_wp], self._vehicle.get_location().z + 1.0, color=(0, 255, 0)) return control, self.data_log, self.kappa_log if log else control
def game_loop(options_dict): world = None try: client = carla.Client('localhost', 2000) client.set_timeout(60.0) print('Changing world to Town 5') client.load_world('Town05') world = World(client.get_world()) world = World(client.get_world()) spawn_points = world.world.get_map().get_spawn_points() vehicle_bp = 'model3' vehicle_transform = spawn_points[options_dict['spawn_point_indices'] [0]] vehicle = Car(vehicle_bp, vehicle_transform, world) agent = agent = BasicAgent(vehicle.vehicle) destination_point = spawn_points[options_dict['spawn_point_indices'] [1]].location print('Going to ', destination_point) agent.set_destination( (destination_point.x, destination_point.y, destination_point.z)) camera_bp = [ 'sensor.camera.rgb', 'sensor.camera.depth', 'sensor.lidar.ray_cast' ] camera_transform = [ carla.Transform(carla.Location(x=1.5, z=2.4), carla.Rotation(pitch=-15, yaw=40)), carla.Transform(carla.Location(x=1.5, z=2.4), carla.Rotation(pitch=-15, yaw=-40)), carla.Transform(carla.Location(x=1.5, z=2.4)) ] cam1 = Camera(camera_bp[0], camera_transform[0], vehicle) cam2 = Camera(camera_bp[0], camera_transform[1], vehicle) # depth1 = Camera(camera_bp[1], camera_transform[0], vehicle) # depth2 = Camera(camera_bp[1], camera_transform[1], vehicle) lidar = Lidar(camera_bp[2], camera_transform[2], vehicle) prev_location = vehicle.vehicle.get_location() sp = 2 file = open("control_input.txt", "a") while True: world.world.tick() world_snapshot = world.world.wait_for_tick(60.0) if not world_snapshot: continue control = agent.run_step() vehicle.vehicle.apply_control(control) w = str(world_snapshot.frame_count) + ',' + str( control.steer) + ',' + str(get_speed(vehicle.vehicle)) + '\n' file.write(w) current_location = vehicle.vehicle.get_location() if current_location.distance( prev_location) <= 0.0 and current_location.distance( destination_point) <= 10: print('distance from destination: ', current_location.distance(destination_point)) if len(options_dict['spawn_point_indices']) <= sp: break else: destination_point = spawn_points[ options_dict['spawn_point_indices'][sp]].location print('Going to ', destination_point) agent.set_destination( (destination_point.x, destination_point.y, destination_point.z)) sp += 1 prev_location = current_location finally: if world is not None: world.destroy()
def run_step(self, target_speed=None, debug=False): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. :param target_speed: desired speed :param debug: boolean flag to activate waypoints debugging :return: control """ if target_speed is not None: self._target_speed = target_speed else: self._target_speed = self._vehicle.get_speed_limit() # Buffer waypoints self._buffer_waypoints() if len(self._waypoint_buffer) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control # Current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) speed = get_speed(self._vehicle) # kph look_ahead = max(2, speed / 4.5) # Target waypoint self.target_waypoint, self.target_road_option = self._waypoint_buffer[ 0] look_ahead_loc = self._get_look_ahead_location(look_ahead) if target_speed > 50: args_lat = self.args_lat_hw_dict args_long = self.args_long_hw_dict else: args_lat = self.args_lat_city_dict args_long = self.args_long_city_dict if not self._pid_controller: self._pid_controller = VehiclePIDController( self._vehicle, args_lateral=args_lat, args_longitudinal=args_long) else: self._pid_controller.set_lon_controller_params(**args_long) self._pid_controller.set_lat_controller_params(**args_lat) control = self._pid_controller.run_step(self._target_speed, look_ahead_loc) # Purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 for i, (waypoint, _) in enumerate(self._waypoint_buffer): if distance_vehicle(waypoint, vehicle_transform) < self._min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): if i == max_index: self._prev_waypoint = self._waypoint_buffer.popleft()[0] else: self._waypoint_buffer.popleft() if debug: carla_world = self._vehicle.get_world() # Draw current buffered waypoint buffered_waypts = [elem[0] for elem in self._waypoint_buffer] draw_waypoints(carla_world, buffered_waypts) # Draw current look ahead point look_ahead_loc carla_world.debug.draw_line(look_ahead_loc, look_ahead_loc + carla.Location(z=0.2), color=carla.Color(255, 255, 0), thickness=0.2, life_time=1.0) return control