def run_step(self, target_speed): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. """ if not self._waypoints_queue: control = CarlaEgoVehicleControl() 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.get_waypoint(self._current_pose.position) # target waypoint target_route_point = self._waypoint_buffer[0] # for us redlight-detection self.target_waypoint = self.get_waypoint(target_route_point.position) target_point = PointStamped() target_point.header.frame_id = "map" target_point.point.x = target_route_point.position.x target_point.point.y = target_route_point.position.y target_point.point.z = target_route_point.position.z self._target_point_publisher.publish(target_point) # move using PID controllers control = self._vehicle_controller.run_step(target_speed, self._current_speed, self._current_pose, target_route_point) # purge the queue of obsolete waypoints max_index = -1 sampling_radius = target_speed * 1 / 3.6 # 1 seconds horizon min_distance = sampling_radius * self.MIN_DISTANCE_PERCENTAGE for i, route_point in enumerate(self._waypoint_buffer): if distance_vehicle(route_point, self._current_pose.position) < min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() 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: """ # 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 get_target_waypoint(self, debug=False): # 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: return None # 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 print('_waypoints_queue, _waypoint_buffer', len(self._waypoints_queue), len(self._waypoint_buffer)) self.target_waypoint, self._target_road_option = self._waypoint_buffer[ 0] # 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 self.target_waypoint
def done(self): vehicle_transform = self._vehicle.get_transform() return len(self._waypoints_queue) == 0 and all([ distance_vehicle(wp, vehicle_transform) < self._min_distance for wp in self._waypoints_queue ])
def run_step(self): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. """ if self.path_received is False: return if not self._waypoint_buffer and not self._waypoints_queue: self.emergency_stop() self.loginfo("Route finished. Waiting for a new one.") self.path_received = False return # When target speed is 0, brake if self._target_speed == 0.0: self.emergency_stop() return # 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 # target waypoint self.target_route_point = self._waypoint_buffer[0] target_point = Marker() target_point.type = 0 target_point.header.frame_id = "map" target_point.pose = self.target_route_point target_point.scale.x = 1.0 target_point.scale.y = 0.2 target_point.scale.z = 0.2 target_point.color.r = 255.0 target_point.color.a = 1.0 self._target_point_publisher.publish(target_point) # move using PID controllers control = self._vehicle_controller.run_step(self._target_speed, self._current_speed, self._current_pose, self.target_route_point) # purge the queue of obsolete waypoints max_index = -1 sampling_radius = self._target_speed * 1 / 3.6 # search radius for next waypoints in seconds min_distance = sampling_radius * self.MIN_DISTANCE_PERCENTAGE for i, route_point in enumerate(self._waypoint_buffer): if distance_vehicle(route_point, self._current_pose.position) < min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() self._control_cmd_publisher.publish(control) return