def run_step(self): transform = self._vehicle.get_transform() if self.waypoints: # If too far off course, reset waypoint queue. if distance_vehicle(self.waypoints[0], transform) > 5.0 * self.radius: self.waypoints = [] # Get more waypoints. if len(self.waypoints) < self.max_waypoints // 2: self.add_next_waypoints() # If no more waypoints, stop. if not self.waypoints: print('Ran out of waypoints; stopping.') control = carla.VehicleControl() control.throttle = 0.0 return control # Remove waypoints we've reached. while distance_vehicle(self.waypoints[0], transform) < self.min_dist: self.waypoints = self.waypoints[1:] # Draw next waypoint draw_waypoints(self._vehicle.get_world(), self.waypoints[:1], self._vehicle.get_location().z + 1.0) return self.controller.run_step(self.target_speed, self.waypoints[0])
def run_step(self, debug=True): """ 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: """ # not enough waypoints in the horizon? => add more! if not self._global_plan and len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): self._compute_next_waypoints(k=100) 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 return control # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: self._waypoint_buffer.append( self._waypoints_queue.popleft()) else: break # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # target waypoint self.target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # move using PID controllers control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) # 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: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0) return control
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, debug=True): """ 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: """ # print("current queue len: ", len(self._waypoints_queue)) # print("maxlen: ", int(self._waypoints_queue.maxlen * 0.5)) # not enough waypoints in the horizon? => add more! # if len(self._waypoints_queue) < int(self._waypoints_queue.maxlen * 0.5): # print("no points here!") # if not self._global_plan: # self._compute_next_waypoints(k=100) # # 队列为空时切换到手动操作模式,待修改 # if len(self._waypoints_queue) == 0: #control = carla.VehicleControl() #control.steer = 0.0 #control.throttle = 0.0 #control.brake = 0.0 #control.hand_brake = False #control.manual_gear_shift = False # return control # Buffering the waypoints # print("queue length inside run_step: ", len(self._waypoints_queue)) if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: # print("queue length: ", len(self._waypoints_queue)) left_point, left_road = self._waypoints_queue.popleft() self._waypoint_buffer.append((left_point, left_road)) # right_point, _ = self._waypoint_buffer.popleft() # print("right point is ", right_point) else: break # target_waypoint, _ = self._waypoint_buffer[0] # print("target_waypoint:", target_waypoint) # for i, (waypoint, _) in enumerate(self._waypoint_buffer): # print("waypoint is ", waypoint) # time.sleep(2) # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) if not self._waypoint_buffer: # control = self._vehicle_controller.run_step(0, self._current_waypoint) # return control return None self._target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # for i in range(len(self._waypoint_buffer)): # wp, _ = self._waypoint_buffer[i] # print("waypoint in buffer is ", wp) # move using PID controllers # print("target_waypoint: ", self._target_waypoint) control = self._vehicle_controller.run_step(self._target_speed, self._target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 # print("len of buffer: ", len(self._waypoint_buffer)) # for i in range(len(self._waypoint_buffer)): # waypoint = self for i, (waypoint, _) in enumerate(self._waypoint_buffer): distance = distance_vehicle(waypoint, vehicle_transform) # print("min distance: ", self._min_distance, "distance: ", distance) # time.sleep(2) # 当前车辆和路点的距离小于最小距离,认为已经行驶完成 if distance < self._min_distance: # print("waypoint in enumerate is ", waypoint) max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() self.finished_waypoints += 1 print("current finished waypoints is ", self.finished_waypoints) if debug: draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0) # if len(self._waypoint_buffer) == 0: # self.finished_waypoints = self.message_waypoints return control
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: """ ##718 remove waypoints until the closest one vehicle_transform = self._vehicle.get_transform() min_index = -1 min_dist = 99999 for i, (waypoint, _) in enumerate(self._waypoint_buffer): dist = distance_vehicle(waypoint, vehicle_transform) if (dist < min_dist): min_dist = dist min_index = i #max_index = i if min_index >= 0: for i in range(min_index): self._waypoint_buffer.popleft() # not enough waypoints in the horizon? => add more! if not self._global_plan and len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): self._compute_next_waypoints(k=100) if len(self._waypoints_queue) == 0: ## #print('_waypoints_queue empty') 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 ## #print(self._waypoint_buffer) # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: self._waypoint_buffer.append( self._waypoints_queue.popleft()) else: break # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # target waypoint self.target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # move using PID controllers control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) ##testing waypoints #if self.target_waypoint: #print('target_waypoint',self.target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 # find the waypoint in waypt buffer that have min distance # and pop anything before that 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: #sett = self._vehicle.get_world().get_settings() #print(sett) draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0) #draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0) return control
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
def run_step(self, debug=True, target_speed=None): """ 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: """ # if not enough waypoints in the horizon, add more if not self._global_plan and len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): self._compute_next_waypoints(k=100) # Empty queue 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 return control # Buffering the waypoints if len(self.waypoint_buffer) < self._buffer_size: for i in range(self._buffer_size - len(self.waypoint_buffer)): if self._waypoints_queue: self.waypoint_buffer.append( self._waypoints_queue.popleft()) else: break # Control Vehicle # current vehicle waypoint vehicle_transform = self._vehicle.get_transform() self._current_waypoint = self._map.get_waypoint( vehicle_transform.location) # target waypoint self.target_waypoint, self._target_road_option = self.waypoint_buffer[ 0] # move using PID controllers _waypoints = [i for i, _ in self.waypoint_buffer] waypoints = [[ points.transform.location.x, points.transform.location.y, points.transform.rotation.yaw ] for points in _waypoints] # print("waypoints: ", waypoints) if target_speed is None: target_speed = self._target_speed control = self._vehicle_controller.run_step(target_speed, waypoints, self.target_waypoint, self._current_waypoint) self.update_buffer() # Draw waypoints if debug: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 0.8) return control
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() 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 return control # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self.waypoints_queue: self._waypoint_buffer.append( self.waypoints_queue.popleft()) else: break # Current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # Target waypoint self.target_waypoint, self.target_road_option = self._waypoint_buffer[ 0] 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 self._pid_controller = VehiclePIDController( self._vehicle, args_lateral=args_lat, args_longitudinal=args_long) control = self._pid_controller.run_step(self._target_speed, self.target_waypoint) # 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: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], 1.0) return control
def run_step(self, dest, lane_change, world, 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: """ # if(lane_change == "right"): # print('switch into the right lane') # # change target waypoint to a point on the right lane # right_lane = self._current_waypoint.get_right_lane() # new_waypoint = right_lane.next(5)[0] # # self.target_waypoint = new_waypoint # self._waypoints_queue.clear() # self._waypoint_buffer.clear() # self._waypoints_queue.append((new_waypoint, RoadOption.LANEFOLLOW)) # elif(lane_change == "left"): # print('switch into the left lane') # # change target waypoint to a point on the right lane # left_lane = self._current_waypoint.get_left_lane() # new_waypoint = left_lane.next(5)[0] # # self.target_waypoint = new_waypoint # self._waypoints_queue.clear() # self._waypoint_buffer.clear() # print(len(self._waypoints_queue)) # # print("new waypoint at " + str(new_waypoint)) # self._waypoints_queue.append((new_waypoint, RoadOption.LANEFOLLOW)) # not enough waypoints in the horizon? => add more! if not self._global_plan and len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): # print("current waypoint: " + str(self._current_waypoint)) # print("add 100 additional waypoints") self._compute_next_waypoints(dest, k=100) current_waypoint = self._map.get_waypoint(self._vehicle.get_location()) if len(self._waypoints_queue) == 0: if not current_waypoint.is_intersection: self._target_road_option = RoadOption.LANEFOLLOW print("way points queue is empty") 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 # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: self._waypoint_buffer.append( self._waypoints_queue.popleft()) else: break # current vehicle waypoint self._current_waypoint = self._map.get_waypoint( self._vehicle.get_location()) # target waypoint self.target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # print("target waypoint at " + str(self.target_waypoint)) if (lane_change == "right"): # print('switch into the right lane') # change target waypoint to a point on the right lane right_lane = self._current_waypoint.get_right_lane() if (not right_lane): print("cannot switch into the right lane") else: self.target_waypoint = right_lane.next(5)[-1] # self.target_waypoint = new_waypoint elif (lane_change == "left"): # print('switch into the left lane') # change target waypoint to a point on the right lane left_lane = self._current_waypoint.get_left_lane() if (not left_lane): print("cannot switch into the right lane") else: self.target_waypoint = left_lane.next(5)[-1] # # set the target speed # speed_limit = self._vehicle.get_speed_limit() # #set highway driving speed to 40 kmh # if(speed_limit > 30): # self.set_speed(30) # # otherwise, set driving speed to 20 kmh # else: # self.set_speed(20) # move using PID controllers #print("target_speed: " + str(self._target_speed)) control = self._vehicle_controller.run_step(self._target_speed, self.target_waypoint) # 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: draw_waypoints(self._vehicle.get_world(), [self.target_waypoint], self._vehicle.get_location().z + 1.0) # if self._target_road_option != RoadOption.LANEFOLLOW not current_waypoint.is_intersection: # self._target_road_option = RoadOption.LANEFOLLOW 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 run_step(self, debug=True): # not enough waypoints in the horizon? => add more! if len(self._waypoints_queue) < int( self._waypoints_queue.maxlen * 0.5): if not self._global_plan: self._compute_next_waypoints(k=100) if len(self._waypoints_queue) == 0: control = carla.VehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 0.0 control.hand_brake = False control.manual_gear_shift = False return control veh_location = self._vehicle.get_location() # type: carla.Location veh_waypoint = self._map.get_waypoint( veh_location) # type: carla.Waypoint veh_yaw = self._vehicle.get_transform( ).rotation.yaw # TODO type: float, range TODO local_plan = self.get_filled_waypoint_buffer( ) # type: List[carla.Waypoint] # Calculate best waypoint to follow considering current yaw L = 2.9 fx = veh_location.x + L * np.cos(veh_yaw) fy = veh_location.y + L * np.sin(veh_yaw) distances = [] for waypoint in local_plan: wp = waypoint.transform.location dx = fx - wp.x dy = fy - wp.y distance = np.sqrt(dx**2 + dy**2) distances.append(distance) target_idx = np.argmin(distances) closest_error = distances[target_idx] self._target_waypoint = local_plan[target_idx] # Calculate path curvature waypoints_to_look_ahead = 6 reference_waypoint = local_plan[target_idx + waypoints_to_look_ahead] ref_location = reference_waypoint.transform.location # delta_x = ref_location.x - veh_location.x # delta_y = ref_location.y - veh_location.y # theta_radians = math.atan2(delta_y, delta_x) # FIXME Sometimes yaw oscilates from 179 to -179 which leads to temporarily bad calculations distance, relative_angle = compute_magnitude_angle( ref_location, veh_location, veh_yaw) #np.rad2deg(theta_radians) - veh_yaw # plt.cla() # plt.plot([self._vehicle.get_transform().location.x, lookahead_waypoint.transform.location.x], [self._vehicle.get_transform().location.y, lookahead_waypoint.transform.location.y], "-r", label="debug") # # plt.plot(, , ".b", label="lookahead") # plt.axis("equal") # plt.legend() # plt.grid(True) # plt.title("Rel angle: {}, yaw {}".format(str(angle), yaw)) # plt.pause(0.0001) if abs(relative_angle) < 15: target_speed = 50 elif abs(relative_angle) < 20: target_speed = 40 elif abs(relative_angle) < 30: target_speed = 30 else: target_speed = 20 print( 'Relative angle to reference waypoint: {:3d} | Vehicle yaw angle: {:3d} | Target speed {} km/h' .format(int(relative_angle), int(veh_yaw), target_speed)) control = self._vehicle_controller.run_step(target_speed, self._target_waypoint) # purge the queue of obsolete waypoints vehicle_transform = self._vehicle.get_transform() max_index = -1 # i, (waypoint, _) 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: #draw_waypoints(self._vehicle.get_world(), [self._target_waypoint], self._vehicle.get_location().z + 1.0, color=carla.Color(0, 255, 0)) draw_waypoints(self._vehicle.get_world(), [reference_waypoint], veh_location.z + 1.0) return control