Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Extended local planner with a lateral offset #3678

Merged
merged 1 commit into from
Dec 7, 2020
Merged

Extended local planner with a lateral offset #3678

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
* Fixed minor typo in the simulation data section of the documentation
* Fixed the `config.py` to read the `.osm ` files in proper `utf-8` encoding
* Added `horizontal_fov` parameter to lidar sensor to allow for restriction of its field of view
* Extended the local planner with a lateral `offset`.

## CARLA 0.9.10

Expand Down
42 changes: 29 additions & 13 deletions PythonAPI/carla/agents/navigation/controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,8 @@ class VehiclePIDController():
"""


def __init__(self, vehicle, args_lateral, args_longitudinal, max_throttle=0.75, max_brake=0.3, max_steering=0.8):
def __init__(self, vehicle, args_lateral, args_longitudinal, offset=0, max_throttle=0.75, max_brake=0.3,
max_steering=0.8):
"""
Constructor method.

Expand All @@ -35,6 +36,9 @@ def __init__(self, vehicle, args_lateral, args_longitudinal, max_throttle=0.75,
K_P -- Proportional term
K_D -- Differential term
K_I -- Integral term
:param offset: If different than zero, the vehicle will drive displaced from the center line.
Positive values imply a right offset while negative ones mean a left one. Numbers high enough
to cause the vehicle to drive through other lanes might break the controller.
"""

self.max_brake = max_brake
Expand All @@ -45,7 +49,7 @@ def __init__(self, vehicle, args_lateral, args_longitudinal, max_throttle=0.75,
self._world = self._vehicle.get_world()
self.past_steering = self._vehicle.get_control().steer
self._lon_controller = PIDLongitudinalController(self._vehicle, **args_longitudinal)
self._lat_controller = PIDLateralController(self._vehicle, **args_lateral)
self._lat_controller = PIDLateralController(self._vehicle, offset, **args_lateral)

def run_step(self, target_speed, waypoint):
"""
Expand Down Expand Up @@ -152,11 +156,13 @@ class PIDLateralController():
PIDLateralController implements lateral control using a PID.
"""

def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03):
def __init__(self, vehicle, offset=0, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03):
"""
Constructor method.

:param vehicle: actor to apply to local planner logic onto
:param offset: distance to the center line. If might cause issues if the value
is large enough to make the vehicle invade other lanes.
:param K_P: Proportional term
:param K_D: Differential term
:param K_I: Integral term
Expand All @@ -167,6 +173,7 @@ def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03):
self._k_d = K_D
self._k_i = K_I
self._dt = dt
self._offset = offset
self._e_buffer = deque(maxlen=10)

def run_step(self, waypoint):
Expand All @@ -189,19 +196,28 @@ def _pid_control(self, waypoint, vehicle_transform):
:param vehicle_transform: current transform of the vehicle
:return: steering control in the range [-1, 1]
"""
v_begin = vehicle_transform.location
v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)),
y=math.sin(math.radians(vehicle_transform.rotation.yaw)))
# Get the ego's location and forward vector
ego_loc = vehicle_transform.location
v_vec = vehicle_transform.get_forward_vector()
v_vec = np.array([v_vec.x, v_vec.y, 0.0])

# Get the vector vehicle-target_wp
if self._offset != 0:
# Displace the wp to the side
w_tran = waypoint.transform
r_vec = w_tran.get_right_vector()
w_loc = w_tran.location + carla.Location(x=self._offset*r_vec.x,
y=self._offset*r_vec.y)
else:
w_loc = waypoint.transform.location

w_vec = np.array([w_loc.x - ego_loc.x,
w_loc.y - ego_loc.y,
0.0])

v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0])
w_vec = np.array([waypoint.transform.location.x -
v_begin.x, waypoint.transform.location.y -
v_begin.y, 0.0])
_dot = math.acos(np.clip(np.dot(w_vec, v_vec) /
(np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0))

_cross = np.cross(v_vec, w_vec)

if _cross[2] < 0:
_dot *= -1.0

Expand All @@ -213,4 +229,4 @@ def _pid_control(self, waypoint, vehicle_transform):
_de = 0.0
_ie = 0.0

return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)
return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0)
6 changes: 5 additions & 1 deletion PythonAPI/carla/agents/navigation/local_planner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,7 @@ def _init_controller(self, opt_dict):
'K_D': 0,
'K_I': 0.05,
'dt': self._dt}
self._offset = 0

# parameters overload
if opt_dict:
Expand All @@ -132,14 +133,17 @@ def _init_controller(self, opt_dict):
self._max_brake = opt_dict['max_brake']
if 'max_steering' in opt_dict:
self._max_steer = opt_dict['max_steering']
if 'offset' in opt_dict:
self._offset = opt_dict['offset']

self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())
self._vehicle_controller = VehiclePIDController(self._vehicle,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict,
offset=self._offset,
max_throttle=self._max_throt,
max_brake=self._max_brake,
max_steering=self._max_steer,)
max_steering=self._max_steer)

self._global_plan = False

Expand Down