def run_step(self, vehicle: Vehicle) -> float:
self.sync(vehicle=vehicle)
return float(
VehicleControl.clamp(
self.kp * (self.target_speed - Vehicle.get_speed(vehicle)), 0,
1
)
)
def convert_control_from_source_to_agent(
self, source: carla.VehicleControl) -> VehicleControl:
"""Convert CARLA raw vehicle control to VehicleControl(throttle,steering)."""
return VehicleControl(
throttle=-1 *
source.throttle if source.reverse else source.throttle,
steering=source.steer,
)
def run_step(self, vehicle: Vehicle,
sensors_data: SensorsData) -> VehicleControl:
super(PIDAgent, self).run_step(vehicle=vehicle,
sensors_data=sensors_data)
self.transform_history.append(self.vehicle.transform)
if self.local_planner.is_done():
control = VehicleControl()
self.logger.debug("Path Following Agent is Done. Idling.")
else:
control = self.local_planner.run_step(vehicle=vehicle)
return control
def run_step(self, vehicle: Vehicle,
sensors_data: SensorsData) -> VehicleControl:
super(GPDAgent, self).run_step(vehicle=vehicle, sensors_data=sensors_data)
self.gpd_detector.run_step()
if self.gpd_detector.curr_segmentation is not None:
world_cords = img_to_world2(depth_img=self.front_depth_camera.data,
segmentation=self.gpd_detector.curr_segmentation,
criteria=self.gpd_detector.GROUND,
intrinsics_matrix=self.front_depth_camera.intrinsics_matrix,
extrinsics_matrix=
self.front_depth_camera.transform.get_matrix() @ self.vehicle.transform.get_matrix())[:2]
print(np.shape(world_cords))
return VehicleControl()
def run_step(self, vehicle: Vehicle, next_waypoint: Transform, **kwargs) \
-> VehicleControl:
"""
Abstract function for run step
Args:
vehicle: new vehicle state
next_waypoint: next waypoint
**kwargs:
Returns:
VehicleControl
"""
self.sync_data(vehicle=vehicle)
return VehicleControl()
def run_step(self, sensors_data: SensorsData,
vehicle: Vehicle) -> VehicleControl:
"""
Receive Sensor Data and vehicle state information on every step and
return a control
Args:
sensors_data: sensor data on this frame
vehicle: vehicle state on this frame
Returns:
Vehicle Control
"""
self.time_counter += 1
self.sync_data(sensors_data=sensors_data, vehicle=vehicle)
if self.should_save_sensor_data:
self.save_sensor_data()
return VehicleControl()
def run_step(self, vehicle: Vehicle, next_waypoint: Transform) -> float:
self.sync(vehicle=vehicle)
target_y = next_waypoint.location.y
target_x = next_waypoint.location.x
angle_difference = math.atan2(
target_y - self.vehicle.transform.location.y,
target_x - self.vehicle.transform.location.x,
) - np.radians(self.vehicle.transform.rotation.yaw)
curr_look_forward = (
self.look_ahead_gain * Vehicle.get_speed(vehicle=vehicle)
+ self.look_ahead_distance
)
lateral_difference = math.atan2(
2.0
* self.vehicle.wheel_base
* math.sin(angle_difference)
/ curr_look_forward,
1.0,
)
return VehicleControl.clamp(lateral_difference, -1, 1)
def run_step(
self, vehicle: Vehicle, next_waypoint: Transform, **kwargs
) -> VehicleControl:
"""
run one step of Pure Pursuit Control
Args:
vehicle: current vehicle state
next_waypoint: Next waypoint, Transform
**kwargs:
Returns:
Vehicle Control
"""
control = VehicleControl(
throttle=self.longitunal_controller.run_step(vehicle=vehicle),
steering=self.latitunal_controller.run_step(
vehicle=vehicle, next_waypoint=next_waypoint
),
)
return control
def run_step(self, vehicle: Vehicle) -> VehicleControl:
"""
Run step for the local planner
Procedure:
1. Sync data
2. get the correct look ahead for current speed
3. get the correct next waypoint
4. feed waypoint into controller
5. return result from controller
Args:
vehicle: current vehicle state
Returns:
next control that the local think the agent should execute.
"""
self.sync_data(vehicle=vehicle) # on every run step, sync first
if (len(self.mission_planner.mission_plan) == 0
and len(self.way_points_queue) == 0):
return VehicleControl()
# get vehicle's location
vehicle_transform: Union[Transform, None] = self.vehicle.transform
if vehicle_transform is None:
raise AgentException(
"I do not know where I am, I cannot proceed forward")
# redefine closeness level based on speed
curr_speed = Vehicle.get_speed(self.vehicle)
if curr_speed < 60:
self.closeness_threshold = 5
elif curr_speed < 80:
self.closeness_threshold = 15
elif curr_speed < 120:
self.closeness_threshold = 20
else:
self.closeness_threshold = 50
# print(f"Curr closeness threshold = {self.closeness_threshold}")
# get current waypoint
curr_closest_dist = float("inf")
while True:
if len(self.way_points_queue) == 0:
self.logger.info("Destination reached")
return VehicleControl()
waypoint: Transform = self.way_points_queue[0]
curr_dist = vehicle_transform.location.distance(waypoint.location)
if curr_dist < curr_closest_dist:
# if i find a waypoint that is closer to me than before
# note that i will always enter here to start the calculation for curr_closest_dist
curr_closest_dist = curr_dist
elif curr_dist < self.closeness_threshold:
# i have moved onto a waypoint, remove that waypoint from the queue
self.way_points_queue.popleft()
else:
break
target_waypoint = self.way_points_queue[0]
# target_waypoint = Transform.average(self.way_points_queue[0], self.way_points_queue[1])
# target_waypoint = Transform.average(self.way_points_queue[2], target_waypoint)
control: VehicleControl = self.controller.run_step(
vehicle=vehicle, next_waypoint=target_waypoint)
# self.logger.debug(
# f"Target_Location {target_waypoint.location} "
# f"| Curr_Location {vehicle_transform.location} "
# f"| Distance {int(curr_closest_dist)}")
return control
def run_step(self, vehicle: Vehicle, next_waypoint: Transform,
**kwargs) -> VehicleControl:
"""
Execute one step of control invoking both lateral and longitudinal
PID controllers to reach a target waypoint at a given target_speed.
Args:
vehicle: New vehicle state
next_waypoint: target location encoded as a waypoint
**kwargs:
Returns:
Next Vehicle Control
"""
super(VehiclePIDController, self).run_step(vehicle, next_waypoint)
curr_speed = Vehicle.get_speed(self.vehicle)
if curr_speed < 60:
self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[60].K_D
self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[60].K_I
self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[60].K_P
elif curr_speed < 100:
self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[100].K_D
self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[100].K_I
self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[100].K_P
elif curr_speed < 150:
self._lat_controller.k_d = OPTIMIZED_LATERAL_PID_VALUES[150].K_D
self._lat_controller.k_i = OPTIMIZED_LATERAL_PID_VALUES[150].K_I
self._lat_controller.k_p = OPTIMIZED_LATERAL_PID_VALUES[150].K_P
acceptable_target_speed = self.target_speed
if abs(self.vehicle.control.steering) < 0.05:
acceptable_target_speed += 20 # eco boost
acceleration = self._lon_controller.run_step(acceptable_target_speed)
current_steering = self._lat_controller.run_step(next_waypoint)
control = VehicleControl()
if acceleration >= 0.0:
control.throttle = min(acceleration, self.max_throttle)
# control.brake = 0.0
else:
control.throttle = 0
# control.brake = min(abs(acceleration), self.max_brake)
# Steering regulation: changes cannot happen abruptly, can't steer too much.
if current_steering > self.past_steering + 0.1:
current_steering = self.past_steering + 0.1
elif current_steering < self.past_steering - 0.1:
current_steering = self.past_steering - 0.1
if current_steering >= 0:
steering = min(self.max_steer, current_steering)
else:
steering = max(-self.max_steer, current_steering)
if abs(current_steering) > 0.03 and curr_speed > 110:
# if i am doing a sharp (>0.5) turn, i do not want to step on full gas
control.throttle = -1
control.steering = steering
self.past_steering = steering
return control
def run_step(self, vehicle: Vehicle) -> VehicleControl:
return VehicleControl()
def run_step(self, vehicle: Vehicle, next_waypoint: Transform,
**kwargs) -> VehicleControl:
super(VehicleMPCController, self).run_step(vehicle, next_waypoint)
# get vehicle location (x, y)
# location = self.vehicle.transform.location
location = vehicle.transform.location
x, y = location.x, location.y
# get vehicle rotation
# rotation = self.vehicle.transform.rotation
rotation = vehicle.transform.rotation
ψ = rotation.yaw / 180 * np.pi # transform into radient
cos_ψ = np.cos(ψ)
sin_ψ = np.sin(ψ)
# get vehicle speed
# v = Vehicle.get_speed(self.vehicle)
v = Vehicle.get_speed(vehicle)
# get next waypoint location
wx, wy = next_waypoint.location.x, next_waypoint.location.y
# debug logging
# self.logger.debug(f"car location: ({x}, {y})")
# self.logger.debug(f"car ψ: {ψ}")
# self.logger.debug(f"car speed: {v}")
# self.logger.debug(f"next waypoint: ({wx}, {wy})")
### 3D ###
# get the index of next waypoint
# waypoint_index = self.get_closest_waypoint_index_3D(location,
# next_waypoint.location)
# # find more waypoints index to fit a polynomial
# waypoint_index_shifted = waypoint_index - 2
# indeces = waypoint_index_shifted + self.steps_poly * np.arange(
# self.poly_degree + 1)
# indeces = indeces % self.track_DF.shape[0]
# # get waypoints for polynomial fitting
# pts = np.array([[self.track_DF.iloc[i][0], self.track_DF.iloc[i][
# 1]] for i in indeces])
### 2D ###
index_2D = self.get_closest_waypoint_index_2D(location,
next_waypoint.location)
index_2D_shifted = index_2D - 5
indeces_2D = index_2D_shifted + self.steps_poly * np.arange(
self.poly_degree + 1)
indeces_2D = indeces_2D % self.pts_2D.shape[0]
pts = self.pts_2D[indeces_2D]
# self.logger.debug(f'\nwaypoint index:\n {index_2D}')
# self.logger.debug(f'\nindeces:\n {indeces_2D}')
# transform waypoints from world to car coorinate
pts_car = VehicleMPCController.transform_into_cars_coordinate_system(
pts, x, y, cos_ψ, sin_ψ)
# fit the polynomial
poly = np.polyfit(pts_car[:, 0], pts_car[:, 1], self.poly_degree)
# Debug
# self.logger.debug(f'\nwaypoint index:\n {waypoint_index}')
# self.logger.debug(f'\nindeces:\n {indeces}')
# self.logger.debug(f'\npts for poly_fit:\n {pts}')
# self.logger.debug(f'\npts_car:\n {pts_car}')
###########
cte = poly[-1]
eψ = -np.arctan(poly[-2])
init = (0, 0, 0, v, cte, eψ, *poly)
self.state0 = self.get_state0(v, cte, eψ, self.steer, self.throttle,
poly)
result = self.minimize_cost(self.bounds, self.state0, init)
# self.steer = -0.6 * cte - 5.5 * (cte - self.prev_cte)
# self.prev_cte = cte
# self.throttle = VehicleMPCController.clip_throttle(self.throttle,
# v, self.target_speed)
control = VehicleControl()
if 'success' in result.message:
self.steer = result.x[-self.steps_ahead]
self.throttle = result.x[-2 * self.steps_ahead]
else:
self.logger.debug('Unsuccessful optimization')
control.steering = self.steer
control.throttle = self.throttle
return control