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