def step(self, target_speed):
"""Execute one step of longitudinal control to reach a given target speed.
NOTE: this method suggests that with K_P = 1, K_I = K_D = 0,
then use full throttle when error is over 1 Km/h, or 3.6 m/s.
Parameters
==========
target_speed : float
Target speed in m/s.
Returns
=======
float
Values [-1,1] for throttle or break control
"""
current_speed = carlautil.actor_to_speed(self.__vehicle)
error = target_speed - current_speed
# logging.info(f"step() speed current={current_speed} target={target_speed} error={error}")
self.__error_buffer.append(error)
if len(self.__error_buffer) >= 2:
_ie = sum(self.__error_buffer) * self.__dt
else:
_ie = 0.0
if self.__should_hotfix_mpc:
self.__should_hotfix_mpc = False
_de = 0.0
elif len(self.__error_buffer) >= 2:
_de = (self.__error_buffer[-1] - self.__error_buffer[-2]) / self.__dt
else:
_de = 0.0
self.__store_current(error, _ie, _de)
ctrl_input = (self.__coeff.K_P * error) + (self.__coeff.K_D * _de) + (self.__coeff.K_I * _ie)
ctrl_input = max(0, ctrl_input) - self.__break_prop*max(0, -ctrl_input)
return self.__clip(ctrl_input)
def step(self, target_speed):
"""Execute one step of longitudinal control to reach a given target speed.
NOTE: this method suggests that with K_P = 1, K_I = K_D = 0,
then use full throttle when error is over 1 Km/h, or 3.6 m/s.
Parameters
==========
target_speed : float
Target speed in m/s.
Returns
=======
float
Values [-1,1] for throttle or break control
"""
current_speed = carlautil.actor_to_speed(self.__vehicle)
error = target_speed - current_speed
self.__error_buffer.append(error)
if len(self.__error_buffer) >= 2:
_de = (self.__error_buffer[-1] -
self.__error_buffer[-2]) / self.__dt
_ie = sum(self.__error_buffer) * self.__dt
else:
_de = 0.0
_ie = 0.0
ctrl_input = (self.__k_p * error) + (self.__k_d * _de) + (self.__k_i *
_ie)
return self.__clip(ctrl_input)
def add_stats():
# save the speed, heading and control values
speed = carlautil.actor_to_speed(ego_vehicle)
_, heading, _ = carlautil.to_rotation_ndarray(ego_vehicle)
speeds.append(speed)
headings.append(heading)
control = ego_vehicle.get_control()
control_steers.append(control.steer)
control_throttles.append(control.throttle)
control_brakes.append(control.brake)
def set_plan(self, target_speeds, target_angles, step_period):
"""Given a trajectory consisting of heading angle and
velocities, use lateral and longitudinal PID controllers
to actuate the vehicle.
Parameters
==========
target_speeds : iterable of float
Target speeds in m/s for the next few consecutive steps.
target_angles : iterable of float
Target angles in radians for the next few consecutive steps.
Angles should be in radians and in the Unreal Engine coordinate system.
The iterables `target_speed` and `target_angle` should have the same length.
step_period : int
The fixed number of steps between two consecutive points in the trajectory.
Each step takes `carla.WorldSettings.fixed_delta_seconds` time.
"""
speed = carlautil.actor_to_speed(self.__vehicle)
_, heading, _ = carlautil.to_rotation_ndarray(self.__vehicle)
target_speeds = np.concatenate(([speed], target_speeds))
target_angles = np.concatenate(([heading], target_angles))
# angles are not expected to lie within (-pi, pi]. Enforce this constraint.
target_angles = util.npu.warp_radians_neg_pi_to_pi(target_angles)
self.step_to_speed = []
self.step_to_angle = []
n_steps = len(target_speeds) - 1
for step in range(n_steps):
nxt1 = target_angles[step+1]
nxt2 = target_angles[step+1] + 2*np.pi
nxt3 = target_angles[step+1] - 2*np.pi
dif1 = abs(target_angles[step] - nxt1)
dif2 = abs(target_angles[step] - nxt2)
dif3 = abs(target_angles[step] - nxt3)
if dif1 < dif2 and dif1 < dif3:
nxt = nxt1
elif dif2 < dif1 and dif2 < dif3:
nxt = nxt2
else:
nxt = nxt3
for substep in range(step_period):
self.step_to_speed.append(
target_speeds[step] + (substep/step_period)*(target_speeds[step+1] - target_speeds[step])
)
self.step_to_angle.append(
util.npu.warp_radians_neg_pi_to_pi(
target_angles[step] + (substep/step_period)*(nxt - target_angles[step])
)
)
self.step_to_speed.append(target_speeds[-1])
self.step_to_angle.append(target_angles[-1])
self.__step_idx = 1
self.longitudinal_controller.hotfix_mpc()
self.lateral_controller.hotfix_mpc()
def get_current(self):
"""Get reference and measurements after step has been taken step,
and control is applied to vehicle.
Returns
=======
util.AttrDict
Contains the following at current
- measurement
- reference
- control : the last applied control.
"""
control = self.__vehicle.get_control()
control = util.AttrDict(
throttle=control.throttle, brake=control.brake, steer=control.steer
)
error = util.AttrDict(
speed=self.longitudinal_controller.get_current(),
angle=self.lateral_controller.get_current()
)
speed = carlautil.actor_to_speed(self.__vehicle)
_, angle, _ = carlautil.to_rotation_ndarray(self.__vehicle)
if not self.step_to_speed \
or self.__step_idx - 1 >= len(self.step_to_speed):
return util.AttrDict(
measurement=util.AttrDict(speed=speed, angle=angle),
reference=util.AttrDict(speed=speed, angle=angle),
error=error,
control=control
)
reference_speed = self.step_to_speed[self.__step_idx - 1]
reference_angle = self.step_to_angle[self.__step_idx - 1]
return util.AttrDict(
measurement=util.AttrDict(speed=speed, angle=angle),
reference=util.AttrDict(
speed=reference_speed,
angle=reference_angle,
),
error=error,
control=control
)
def scenario(scenario_params, carla_synchronous, request):
client, world, carla_map, traffic_manager = carla_synchronous
ego_vehicle = None
try:
# Set up the vehicle
spawn_points = carla_map.get_spawn_points()
spawn_point = spawn_points[scenario_params.ego_spawn_idx]
if scenario_params.spawn_shift is not None:
spawn_point = carlautil.move_along_road(
carla_map, spawn_point, scenario_params.spawn_shift)
spawn_point.location += carla.Location(0, 0, 0.1)
blueprint = world.get_blueprint_library().find('vehicle.audi.a2')
ego_vehicle = world.spawn_actor(blueprint, spawn_point)
world.tick()
# Set up the camera.
shift = 45
shift = shift * ego_vehicle.get_transform().get_forward_vector()
shift = shift + carla.Location(z=65)
location = ego_vehicle.get_transform().location + shift
world.get_spectator().set_transform(
carla.Transform(location, carla.Rotation(pitch=-90)))
# NOTE: ego_vehicle.get_speed_limit() documentation says m/s. This is incorrect.
logging.info(
f"Vehicle speed limit is {ego_vehicle.get_speed_limit()} km/h")
###############
# Apply control
# NOTE: 1 m/s == 3.6 km/h
speeds = []
accelerations = []
vangulars = []
control_throttles = []
control_brakes = []
control_steerings = []
for idx in range(scenario_params.run_steps):
for ctrl in scenario_params.controls:
if ctrl.interval[0] <= idx and idx <= ctrl.interval[1]:
ego_vehicle.apply_control(ctrl.control)
break
world.tick()
speed = carlautil.actor_to_speed(ego_vehicle)
speeds.append(speed)
accel = carlautil.actor_to_acceleration(ego_vehicle)
accelerations.append(accel)
vangular = ego_vehicle.get_angular_velocity().z
vangulars.append(vangular)
control_throttles.append(ctrl.control.throttle)
control_brakes.append(ctrl.control.brake)
control_steerings.append(ctrl.control.steer)
##########
# Plotting
fig, axes = plt.subplots(1, 3, figsize=(12, 12))
axes = axes.ravel()
timesteps = np.arange(len(speeds)) * 0.05
axes[0].plot(timesteps, speeds, label="measurement")
axes[0].set_title("Speed input/response")
axes[0].set_ylabel("$m/s$")
axes[1].plot(timesteps, accelerations, label="measurement")
axes[1].set_title("Acceleration response")
axes[1].set_ylabel("$m/s^2$")
axes[2].plot(timesteps, vangulars, label="measurement")
axes[2].plot(timesteps, control_steerings, label="steer input")
axes[2].set_title("Angular velocity response")
axes[2].set_ylabel("deg/$s$")
for ax in axes:
ax.grid()
ax.legend()
ax.set_xlabel("time s")
for ax in axes[:2]:
ax.plot(timesteps, control_throttles, "b", label="throttle input")
ax.plot(timesteps, control_brakes, "r", label="brake input")
fig.tight_layout()
fig.savefig(os.path.join("out", f"{request.node.callspec.id}.png"))
fig.clf()
finally:
if ego_vehicle:
ego_vehicle.destroy()