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]
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