class _MotorController(Timer):
def __init__(self):
super().__init__(0.25)
self.pid = PID(60, 30, 20, -50, 50)
self.pid.difference = compass.angleDifference
self.drive_forward = False
self.drive_backward = False
self.t = 0
self.last = 0
self.r_speed = 100
self.l_speed = 100
def forward(self):
self.drive_backward = False
self.drive_forward = True
def backward(self):
self.drive_backward = True
self.drive_forward = False
def run(self):
dt = time.time() - self.t
self.t = time.time()
direction = compass.getHeading()
ret = self.pid.update(direction, dt)
if ret < 0:
self.r_speed = 100 + ret
self.l_speed = 100
if ret > 0:
self.r_speed = 100
self.l_speed = 100 - ret
if self.drive_forward:
LEFT.forward(self.l_speed)
RIGHT.forward(self.r_speed)
elif self.drive_backward:
LEFT.backward(self.l_speed)
RIGHT.backward(self.r_speed)
def start(self):
self.last = compass.getHeading()
self.t = time.time()
super().start()
class _MotorController(Timer):
def __init__(self):
super().__init__(0.25)
self.pid = PID(60, 30, 20, -50, 50)
self.pid.difference = compass.angleDifference
self.drive_forward = False
self.drive_backward = False
self.t = 0
self.last = 0
self.r_speed = 100
self.l_speed = 100
def forward(self):
self.drive_backward = False
self.drive_forward = True
def backward(self):
self.drive_backward = True
self.drive_forward = False
def run(self):
dt = time.time() - self.t
self.t = time.time()
direction = compass.getHeading()
ret = self.pid.update(direction, dt)
if ret < 0:
self.r_speed = 100 + ret
self.l_speed = 100
if ret > 0:
self.r_speed = 100
self.l_speed = 100 - ret
if self.drive_forward:
LEFT.forward(self.l_speed)
RIGHT.forward(self.r_speed)
elif self.drive_backward:
LEFT.backward(self.l_speed)
RIGHT.backward(self.r_speed)
def start(self):
self.last = compass.getHeading()
self.t = time.time()
super().start()
def turn_to(heading, error=math.radians(4)):
pid = PID(3, 0.1, 5, -50, 50)
pid.set_target(heading)
pid.difference = compass.angleDifference
h = compass.getHeading()
while True:
ret = pid.update(h)
if ret < 0:
motors.right(50-ret)
elif ret > 0:
motors.left(50+ret)
if abs(compass.angleDifference(h, heading)) <= error:
motors.stop()
time.sleep(0.5)
if abs(compass.angleDifference(compass.getHeading(), heading)) <= error:
break
h = compass.getHeading()
time.sleep(0.05)
def turn_to(heading, error=math.radians(4)):
pid = PID(3, 0.1, 5, -50, 50)
pid.set_target(heading)
pid.difference = compass.angleDifference
h = compass.getHeading()
while True:
ret = pid.update(h)
if ret < 0:
motors.right(50 - ret)
elif ret > 0:
motors.left(50 + ret)
if abs(compass.angleDifference(h, heading)) <= error:
motors.stop()
time.sleep(0.5)
if abs(compass.angleDifference(compass.getHeading(),
heading)) <= error:
break
h = compass.getHeading()
time.sleep(0.05)
class CAL(Agent):
def __init__(self, city_name):
self.timer = Timer()
# Map setup
self._map = CarlaMap(city_name)
self._centerlines = Centerlines(city_name)
# Agent Setup
Agent.__init__(self)
self._neural_net = CAL_network()
self._seq_len = 5 #self._neural_net.model.max_input_shape
self._state = VehicleState()
self._agents_present = False
# Controller setup
param_path = os.path.dirname(__file__) + '/controller/params/'
cruise_params = get_params_from_txt(param_path + 'cruise_params.txt')
self._PID_cruise = PID(*cruise_params)
follow_params = get_params_from_txt(param_path + 'follow_params.txt')
self._PID_follow = PID(*follow_params)
# General Parameter setup
general_params = get_params_from_txt(param_path + 'general_params.txt')
self.c, self.d = general_params[0], general_params[1]
self.Kl_STANLEY = general_params[2]
self.Kr_STANLEY = general_params[3]
self.K0_STANLEY = general_params[4]
self.curve_slowdown = general_params[5]
self.DELTAl = general_params[6]
self.DELTAr = general_params[7]
self.DELTA0 = general_params[8]
self.EXP_DECAY = general_params[9]
def reset_state(self):
""" for resetting at the start of a new episode"""
self._state = VehicleState()
def modified_run_step(self, measurements, sensor_data, carla_direction,
target):
curr_time = time.time()
# update the vehicle state
#self._state.speed = measurements.player_measurements.forward_speed*3.6
# get the current location and orientation of the agent in the world COS
location, psi = self._get_location_and_orientation(measurements)
# check if there are other cars or pedestrians present
# speed up the benchmark, because we dont need to stop for red lights
#self._agents_present = any([agent.HasField('vehicle') for agent in measurements.non_player_agents])
# get the possible directions (at position of the front axle)
front_axle_pos = self._get_front_axle(location, psi)
try:
directions_list_new = self._centerlines.get_directions(
front_axle_pos)
except:
directions_list_new = {}
# determine the current direction
if directions_list_new:
self._set_current_direction(directions_list_new, carla_direction)
#### SIGNALS FOR EVALUATION ####
# set the correct center line and get the ground truth
self._state.center_distance_GT = self._centerlines.get_center_distance(
front_axle_pos)
self._state.long_accel, self._state.lat_accel = self._get_accel(
measurements, psi)
################################
with open(
"/home/self-driving/Desktop/CARLA_0.8.2/Indranil/CAL-master/PythonClient/_benchmarks_results/latestData.csv",
"a+") as myf:
wrs=str(self._state.center_distance_GT)+","+str(self._state.long_accel)+","+str(self._state.lat_accel)+","+\
str(measurements.game_timestamp)+","+str(self._state.direction)
myf.write(wrs + "\n")
def run_step(self, measurements, sensor_data, carla_direction, target):
curr_time = time.time()
# update the vehicle state
self._state.speed = measurements.player_measurements.forward_speed * 3.6
# get the current location and orientation of the agent in the world COS
location, psi = self._get_location_and_orientation(measurements)
# check if there are other cars or pedestrians present
# speed up the benchmark, because we dont need to stop for red lights
self._agents_present = any([
agent.HasField('vehicle')
for agent in measurements.non_player_agents
])
# get the possible directions (at position of the front axle)
front_axle_pos = self._get_front_axle(location, psi)
try:
directions_list_new = self._centerlines.get_directions(
front_axle_pos)
except:
directions_list_new = {}
# determine the current direction
if directions_list_new:
self._set_current_direction(directions_list_new, carla_direction)
#### SIGNALS FOR EVALUATION ####
# set the correct center line and get the ground truth
self._state.center_distance_GT = self._centerlines.get_center_distance(
front_axle_pos)
self._state.long_accel, self._state.lat_accel = self._get_accel(
measurements, psi)
################################
with open(
"/home/self-driving/Desktop/CARLA_0.8.2/Indranil/latestData.csv",
"a+") as myf:
wrs=str(self._state.center_distance_GT)+","+str(self._state.long_accel)+","+str(self._state.lat_accel)+","+\
str(measurements.game_timestamp)+","+str(self._state.direction)
myf.write(wrs + "\n")
# cycle the image sequence
new_im = sensor_data['CameraRGB'].data
if self._state.image_hist is None:
im0 = self._neural_net.preprocess(new_im, sequence=True)
print("1 works")
self._state.image_hist = im0.repeat_interleave(self._seq_len,
dim=1)
print("2 works")
else:
# get the newest entry
im0 = self._neural_net.preprocess(new_im, sequence=True)
print("2.5 works")
# drop the oldelst entry
self._state.image_hist = self._state.image_hist[:, 1:, :, :]
# add new entry
print("3 works")
self._state.image_hist = np.concatenate(
(self._state.image_hist, im0), axis=1)
print("4 works")
# calculate the control
control, prediction = self._compute_action(carla_direction,
self._state.direction)
return control, prediction
#edited ash
def getMetData(self):
tempDict = {}
tempDict['direction'] = self._state.direction
tempDict['centerDist'] = self._state.center_distance_GT
tempDict['long_accel'], tempDict[
'lat_accel'] = self._state.long_accel, self._state.lat_accel
return tempDict
def _set_current_direction(self, directions_list_new, carla_direction):
if carla_direction == 3.0:
self._state.direction = -1
if carla_direction == 4.0:
self._state.direction = 1
if carla_direction == 2.0 or len(directions_list_new) == 1:
self._state.direction = list(directions_list_new)[0]
if carla_direction == 5.0 or carla_direction == 0.0:
self._state.direction = 0
### set the correct GT centerline maps
# set the centerlines according to the decided direction
# and to the current street type
direction = self._state.direction
is_straight = direction == 0 or directions_list_new == {0}
is_c1 = (direction == -1 and directions_list_new == {0,-1}) or \
(direction == 1 and directions_list_new == {1,-1}) or \
directions_list_new == {-1} or directions_list_new == {1}
is_c2 = (direction == 1 and directions_list_new == {0,1}) or \
(direction ==-1 and directions_list_new == {1,-1})
# set the centerlines accordingly
if is_straight: self._centerlines.set_centerlines('straight')
if is_c1: self._centerlines.set_centerlines('c1')
if is_c2: self._centerlines.set_centerlines('c2')
def _compute_action(self, carla_direction, direction):
start = time.time()
# Predict the intermediate representations
prediction = self._neural_net.predict(self._state.image_hist,
[direction])
logging.info("Time for prediction: {}".format(time.time() - start))
logging.info("CARLA Direction {}, Real Direction {}".format(
carla_direction, direction))
# update the speed limit if a speed limit sign is detected
if prediction['speed_sign'][0] != -1:
self._state.speed_limit = prediction['speed_sign']
# Calculate the control
control = Control()
control.throttle, control.brake = self._longitudinal_control(
prediction, direction)
control.steer = self._lateral_control(prediction)
return control, prediction
def _longitudinal_control(self, prediction, direction):
"""
calculate the _longitudinal_control
the constants (c, d, curve_slowdown) are defined on top of the file
"""
### Variable setup
throttle = 0
brake = 0
# unpack the state variables
speed = self._state.speed
limit = self._state.speed_limit
# uncomment for similar conditions to carla paper
limit = 30
# determine whether to use other states than cruising
cruising_only = not self._agents_present
# brake when driving into a curve
if direction: limit -= self.curve_slowdown
# get the distance to the leading car
veh_distance = prediction['veh_distance']
# half of the speed indicator
is_following = veh_distance < np.clip(limit, 30, 50)
# optimal car following model
following_speed = limit * (
1 - np.exp(-self.c / limit * veh_distance - self.d))
### State machine
if prediction['hazard_stop'][0] \
and prediction['hazard_stop'][1] > 0.9 \
and self._agents_present:
state_name = 'hazard_stop'
prediction_proba = prediction['hazard_stop'][1]
brake = 1
elif prediction['red_light'][0] \
and prediction['red_light'][1] > 0.98 \
and self._agents_present:
state_name = 'red_light'
prediction_proba = prediction['red_light'][1]
throttle = 0
if speed > 5:
# brake if driving to fast
brake = 0.8 * (speed / 30.0)
else:
# fully brake if close to standing still
brake = 1.0
elif is_following and self._agents_present:
state_name = 'following'
prediction_proba = 1.0
desired_speed = following_speed
self._PID_follow.update(desired_speed - speed)
throttle = -self._PID_follow.output
else: # is cruising
state_name = 'cruising'
prediction_proba = 1.0
desired_speed = limit
self._PID_cruise.update(desired_speed - speed)
throttle = -self._PID_cruise.output
logging.info('STATE: {}, PROBA: {:.4f} %'.format(
state_name, prediction_proba * 100))
### Additional Corrections
# slow down speed limit is exceeded
if speed > limit + 10: brake = 0.3 * (speed / 30)
# clipping
throttle = np.clip(throttle, 0, 0.8)
brake = np.clip(brake, 0, 1)
if brake: throttle = 0
return throttle, brake
def _lateral_control(self, prediction):
"""
function implements the lateral control algorithm
input:
- vehicle speed
- front axle position
- vehicle yaw
- distance to closest pixel on center line [with correct sign]
- yaw in closest pixel on center line
output:
- delta signal in [-1,1]
"""
# vehicle state
v = self._state.speed
# when standing don't steer
if abs(v) <= 0.1: return 0
# choose value for k and d depending on the street
if self._state.direction == 0:
k = self.K0_STANLEY
d = self.DELTA0
elif self._state.direction == -1:
k = self.Kl_STANLEY
d = self.DELTAl
elif self._state.direction == 1:
k = self.Kr_STANLEY
d = self.DELTAr
# stanley control
theta_e = prediction['relative_angle']
theta_d = math.atan2(k * prediction['center_distance'], v)
delta = theta_e + theta_d
# normalize delta
delta /= MAX_STEER
# get delta sign, damping is calculed using the absolute delta
delta_sign = np.sign(delta)
delta = abs(delta)
# add exponential damping
decay = d * math.exp(-self.EXP_DECAY * delta)
logging.info("DECAY: {}".format(decay))
delta -= decay
delta = np.clip(delta, 0, 1)
# return the signed delta
return delta_sign * delta
def _get_location_and_orientation(self, measurements):
# get the location
location_world = [
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
]
location_map = self._map.convert_to_pixel(location_world)
# get the orientation
veh_ori_x = measurements.player_measurements.transform.orientation.x,
veh_ori_y = measurements.player_measurements.transform.orientation.y,
psi = math.atan2(veh_ori_y[0], veh_ori_x[0]) # angle in the world COS
return location_map[:2], psi
def _get_front_axle(self, location, psi):
# calculate the position of the front axle
point = self._vehicle_to_world_COS(location, (0, 8.7644), psi)
return (int(point[0]), int(point[1]))
def _vehicle_to_world_COS(self, origin, point, psi):
"""
transform a 2d point from the vehicle COS to the world COS
"""
x_new = origin[0] - point[0] * math.sin(psi) + point[1] * math.cos(psi)
y_new = origin[1] + point[0] * math.cos(psi) + point[1] * math.sin(psi)
return (x_new, y_new)
def _get_accel(self, measurements, psi):
# get the absolute aceleration in the cars COS
acceleration = measurements.player_measurements.acceleration
a_x, a_y = acceleration.x, acceleration.y
# calculate in the relative COS
a_x_rel = a_x * math.cos(psi) + a_y * math.sin(psi)
a_y_rel = -a_x * math.sin(psi) + a_y * math.cos(psi)
return a_x_rel, a_y_rel
def get_GT(self):
""""
This functions returns the current distance to the center line
and the current directions_list_new
"""
d = {}
d['center_distance'] = self._state.center_distance_GT
d['direction'] = self._state.direction
d['long_accel'] = self._state.long_accel
d['lat_accel'] = self._state.lat_accel
return d
s2.Kp = 5 s2.Ki = 16 s2.Kd = 3 s2.Ts = 0.002 # they all must be the same!!! s2.Tt = 1 / s1.Ki s2.YMin = 0 s2.YMax = 10 # PID parameters using each individual setting params = PIDParameters() params.setSettings([s1, s2]) params.wrap = False params.saturate = True params.integration_method = IntegrationMethod.TUSTIN # configure PID instance controller = PID() controller.configParameters(params) # ... code is running ... # ... inside a mainloop to update the controller ... ctrl_in = PIDInput() ctrl_in.time = LinearSystem.getTimeFromSeconds(0.2) ctrl_in.ref = [0.1, 0.2] ctrl_in.sig = [0.3, 0.4] ctrl_in.dref = [0, 0] ctrl_in.dsig = [0, 0] ctrl_in.sat = [0, 0] # last PID output after saturation out = controller.update(ctrl_in) print(out.getValue())
