def run_in_series(self, next_waypoint: Transform, speed_multiplier=1.0, **kwargs) -> VehicleControl: # Calculate the current angle to the next waypoint angBoi = -self._calculate_angle_error(next_waypoint=next_waypoint) # Grab our current speed curSpeed = Vehicle.get_speed(self.agent.vehicle) # Toss both values into a current xt xt = np.array([angBoi, curSpeed]) # Generate our target speed with reactive speed reduction when off track target_speed = min(self.max_speed, kwargs.get("target_speed", self.max_speed)) * speed_multiplier # if we are very off track, update error to reflect that absErr = np.abs(angBoi) if absErr > self.errBoi: self.errBoi = absErr else: # if we are getting back on track, gradually reduce our error self.errBoi = self.errBoi * ( 1 - self.errAlpha) + absErr * self.errAlpha # reduce our target speed based on how far off target we are # target_speed *= (math.exp(-self.errBoi) - 1) * self.slowdown + 1 target_speed *= max( (math.cos(self.errBoi) - 1) * self.slowdown, -self.maxSlow) + 1 ## Note for future: It may be helpful to have another module for adaptive speed control and some way to slowly ## increase the target speed when we can. # Assume we want to go in the direction of the waypoint at the target speed foreversies xd = np.array([0, target_speed]) cur_speed = Vehicle.get_speed(self.agent.vehicle) cd = np.array([0, cur_speed]) # Calculate the feasible ud trajectory ud, _, _, _ = np.linalg.lstsq(self.B, xd - np.dot(self.A, cd), rcond=None) # convert to offset variables zt and ht zt = xt - xd ht = -np.dot(self.K, zt) # convert back to ut and clip our inputs ut = ht + ud steering = np.clip(ut[0], self.steering_boundary[0], self.steering_boundary[1]) throttle = np.clip(ut[1], self.throttle_boundary[0], self.throttle_boundary[1]) return VehicleControl(steering=steering, throttle=throttle)
def get_reward(self) -> float: reward: float = 1.0 if self.dist_diff <= 0: reward += 100 self.debug_info["isGettingCloserToNextwaypoint"] = True else: reward -= 10 self.debug_info["isGettingCloserToNextwaypoint"] = False if Vehicle.get_speed(self.agent.vehicle) < 10: reward -= 100 if self.carla_runner.get_num_collision() > 0: reward -= 10000 if abs(self.agent.vehicle.control.steering) > 0.3: reward -= 1000 elif abs(self.agent.vehicle.control.steering) > 0.2: reward -= 500 elif abs(self.agent.vehicle.control.steering) > 0.1: reward -= 100 if abs(self.agent.vehicle.control.steering) < 0.1: reward += 1000 self.debug_info["reward"] = reward return reward
def step(self, action: List[float]) -> Tuple[np.ndarray, float, bool, dict]: """ Args: action: Returns: """ self._prev_speed = Vehicle.get_speed(self.agent.vehicle) control = VehicleControl(throttle=action[0], steering=action[1]) self.agent.kwargs["control"] = control before_dist = self.agent.vehicle.transform.location.distance( self.agent.local_planner.way_points_queue[ self.agent.local_planner.get_curr_waypoint_index()].location) agent, reward, is_done, other_info = super(OccuDebug, self).step(action=action) after_dist = self.agent.vehicle.transform.location.distance( self.agent.local_planner.way_points_queue[ self.agent.local_planner.get_curr_waypoint_index()].location) self.dist_diff = before_dist - after_dist obs = self._get_obs() return obs, reward, is_done, other_info
def _terminal(self) -> bool: if self.agent.time_counter > 100 and Vehicle.get_speed(self.agent.vehicle) < 10: return True elif self.carla_runner.get_num_collision() > 2: return True else: return False
def convert_data(self): try: rear_rgb = None if self.ios_config.ar_mode and self.world_cam_streamer.curr_image is not None: front_rgb = cv2.rotate(self.world_cam_streamer.curr_image, cv2.ROTATE_90_CLOCKWISE) else: front_rgb = cv2.rotate(self.world_cam_streamer.curr_image, cv2.ROTATE_90_CLOCKWISE) sensor_data: SensorsData = \ self.ios_bridge.convert_sensor_data_from_source_to_agent( { "front_rgb": front_rgb, "front_depth": self.depth_cam_streamer.curr_image, "rear_rgb": rear_rgb } ) vehicle = self.ios_bridge.convert_vehicle_from_source_to_agent({ "transform": self.veh_state_streamer.transform, "velocity": self.veh_state_streamer.velocity, "acceleration": self.veh_state_streamer.acceleration }) vehicle.control = self.control_streamer.control_tx if self.depth_cam_streamer.intrinsics is not None: self.agent.front_depth_camera.intrinsics_matrix = self.depth_cam_streamer.intrinsics if self.world_cam_streamer.intrinsics is not None: self.agent.front_rgb_camera.intrinsics_matrix = self.world_cam_streamer.intrinsics return sensor_data, vehicle except Exception as e: self.logger.error(f"Cannot convert data: {e}") return SensorsData(), Vehicle()
def run_in_series(self, next_waypoint: Transform, errBoi: float = 0.0, **kwargs) -> float: target_speed = min(self.max_speed, kwargs.get("target_speed", self.max_speed)) # if we are very off track, update error to reflect that if errBoi > self.errBoi: self.errBoi = errBoi else: # if we are getting back on track, gradually reduce our error self.errBoi = self.errBoi*(1-self.errAlpha) + errBoi*self.errAlpha # reduce our target speed based on how far off target we are target_speed *= (math.exp(-self.errBoi) - 1)/2 + 1 current_speed = Vehicle.get_speed(self.agent.vehicle) k_p, k_d, k_i = PIDController.find_k_values(vehicle=self.agent.vehicle, config=self.config) error = target_speed - current_speed self._error_buffer.append(error) if len(self._error_buffer) >= 2: # print(self._error_buffer[-1], self._error_buffer[-2]) _de = (self._error_buffer[-2] - self._error_buffer[-1]) / self._dt _ie = sum(self._error_buffer) * self._dt else: _de = 0.0 _ie = 0.0 output = float(np.clip((k_p * error) + (k_d * _de) + (k_i * _ie), self.throttle_boundary[0], self.throttle_boundary[1])) # self.logger.debug(f"curr_speed: {round(current_speed, 2)} | kp: {round(k_p, 2)} | kd: {k_d} | ki = {k_i} | " # f"err = {round(error, 2)} | de = {round(_de, 2)} | ie = {round(_ie, 2)}") #f"self._error_buffer[-1] {self._error_buffer[-1]} | self._error_buffer[-2] = {self._error_buffer[-2]}") return output
def get_reward(self) -> float: """ Reward policy: Surviving = +1 Going toward a waypoint = +10 Collision = -10000 abs(steering) > 0.5 = -1000 abs(steering) < 0.2 = +2000 Returns: reward according to the aforementioned policy """ reward: float = 1.0 if self.dist_diff <= 0: reward += 100 self.debug_info["isGettingCloserToNextwaypoint"] = True else: reward -= 10 self.debug_info["isGettingCloserToNextwaypoint"] = False if Vehicle.get_speed(self.agent.vehicle) < 10: reward -= 10 if self.carla_runner.get_num_collision() > 0: reward -= 10000 if abs(self.agent.vehicle.control.steering) > 0.1: reward -= 1000 if abs(self.agent.vehicle.control.steering) < 0.1: reward += 200 self.debug_info['reward'] = reward return reward
def _get_obs(self) -> Any: curr_speed = np.array([ Vehicle.get_speed(self.agent.vehicle), self.agent.vehicle.control.throttle, self.agent.vehicle.control.steering ]) return curr_speed
def step(self, action: List[float]) -> Tuple[np.ndarray, float, bool, dict]: """ Args: action: array of [target_speed, long_kp, long_kd, long_ki, lat_kp, lat_kd, lat_ki] Returns: """ assert type(action) == list or type( action) == np.ndarray, f"Action is of type {type(action)}" assert len( action) == 7, f"Action of shape {np.shape(action)} is not correct" self._prev_speed = Vehicle.get_speed(self.agent.vehicle) if len(self.agent.local_planner.way_points_queue) > 0: self._prev_waypoint = self.agent.local_planner.way_points_queue[ 0].to_array() target_speed = action[0] long_k_p, long_k_d, long_k_i = action[1], action[2], action[3] lat_k_p, lat_k_d, lat_k_i = action[4], action[5], action[6] self.agent.kwargs["long_k_p"] = long_k_p self.agent.kwargs["long_k_d"] = long_k_d self.agent.kwargs["long_k_i"] = long_k_i self.agent.kwargs["target_speed"] = target_speed self.agent.kwargs["lat_k_p"] = lat_k_p self.agent.kwargs["lat_k_d"] = lat_k_d self.agent.kwargs["lat_k_i"] = lat_k_i agent, reward, is_done, other_info = super(ROARPIDEnv, self).step(action=action) obs = self._get_obs() return obs, reward, is_done, other_info
def run_in_series(self, next_waypoint: Transform, **kwargs) -> float: target_speed = min( self.max_speed, kwargs.get("kwargs").get("target_speed", self.max_speed)) # self.logger.debug(f"Target_Speed: {target_speed} | max_speed = {self.max_speed}") current_speed = Vehicle.get_speed(self.agent.vehicle) k_p, k_d, k_i = kwargs.get("long_k_p", 1), kwargs.get("long_k_d", 0), kwargs.get( "long_k_i", 0) error = target_speed - current_speed self._error_buffer.append(error) if len(self._error_buffer) >= 2: # print(self._error_buffer[-1], self._error_buffer[-2]) _de = (self._error_buffer[-2] - self._error_buffer[-1]) / self._dt _ie = sum(self._error_buffer) * self._dt else: _de = 0.0 _ie = 0.0 output = float( np.clip((k_p * error) + (k_d * _de) + (k_i * _ie), self.throttle_boundary[0], self.throttle_boundary[1])) return output
def set_closeness_threhold(self, config: dict): curr_speed = Vehicle.get_speed(self.agent.vehicle) for speed_upper_bound, closeness_threshold in config.items(): speed_upper_bound = float(speed_upper_bound) if curr_speed < speed_upper_bound: self.closeness_threshold = closeness_threshold break
def run_in_series(self, 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 """ curr_speed = Vehicle.get_speed(self.agent.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.agent.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 step(self, action: Any) -> Tuple[Any, float, bool, dict]: assert type(action) == list or type( action) == np.ndarray, f"Action is not recognizable" assert len( action ) == 2, f"Action should be of length 2 but is of length [{len(action)}]." self._prev_speed = Vehicle.get_speed(self.agent.vehicle) self._prev_location = self.agent.vehicle.transform.location action = np.array(action).astype(np.int64) self.action = action if len(self.agent.traditional_local_planner.way_points_queue) > 0: self.correct_next_waypoint_world = self.agent.traditional_local_planner.way_points_queue[ 0] self.my_guess_next_waypoint_occu = action self.my_guess_next_waypoint_world = self.agent.occupancy_map.cropped_occu_to_world( cropped_occu_coord=self.my_guess_next_waypoint_occu, vehicle_transform=self.agent.vehicle.transform, occu_vehicle_center=np.array( [self.obs_size // 2, self.obs_size // 2])) self.agent.kwargs["next_waypoint"] = self.my_guess_next_waypoint_world obs, reward, is_done, other_info = super(LocalPlannerEnv1, self).step(action=action) return obs, reward, is_done, other_info
def run_step(self) -> float: return float( VehicleControl.clamp( self.kp * (self.target_speed - Vehicle.get_speed(self.agent.vehicle)), 0, 1 ) )
def run_in_series(self, next_waypoint: Transform, **kwargs) -> float: target_speed = min(self.max_speed, kwargs.get("target_speed", self.max_speed)) current_speed = Vehicle.get_speed(self.agent.vehicle) k_p, k_d, k_i = CStanley_controller.find_k_values( vehicle=self.agent.vehicle, config=self.config) error = target_speed - current_speed self._error_buffer.append(error) if len(self._error_buffer) >= 2: # print(self._error_buffer[-1], self._error_buffer[-2]) _de = (self._error_buffer[-2] - self._error_buffer[-1]) / self._dt _ie = sum(self._error_buffer) * self._dt else: _de = 0.0 _ie = 0.0 output = float( np.clip((k_p * error) + (k_d * _de) + (k_i * _ie), self.throttle_boundary[0], self.throttle_boundary[1])) # self.logger.debug(f"curr_speed: {round(current_speed, 2)} | kp: {round(k_p, 2)} | kd: {k_d} | ki = {k_i} | " # f"err = {round(error, 2)} | de = {round(_de, 2)} | ie = {round(_ie, 2)}") # f"self._error_buffer[-1] {self._error_buffer[-1]} | self._error_buffer[-2] = {self._error_buffer[-2]}") return output
def __init__(self, carla_settings: CarlaConfig, agent_settings: AgentConfig, npc_agent_class, competition_mode=False, start_bbox: np.ndarray = np.array([5, -5, 0, 13, 5, 50]), lap_count=10): """ Args: carla_settings: CarlaConfig instance agent_settings: AgentConfig instance npc_agent_class: an agent class competition_mode: [Optional] True/False start_bbox: [Optional] array of [minx, miny, minz, maxx, maxy, maxz]. [5, -5, 0, 13, 5, 50] is the bbox for easy_map. [-815, 20, -760, -770, 120, -600] is the bbox for berkeley_minor_map lap_count: [Optional] total lap count """ self.carla_settings = carla_settings self.agent_settings = agent_settings self.carla_bridge = CarlaBridge() self.npc_agent_class = npc_agent_class self.world = None self.client = None self.controller = None self.display = None self.agent = None self.npc_agents: Dict[npc_agent_class, Any] = {} self.agent_collision_counter = 0 self.competition_mode = competition_mode self.start_bbox = start_bbox self.lap_count = lap_count self.completed_lap_count = 0 self.sensor_data = SensorsData() self.vehicle_state = Vehicle() self.start_simulation_time: Optional[float] = None self.start_vehicle_position: Optional[np.array] = None self.end_simulation_time: Optional[float] = None self.end_vehicle_position: Optional[np.array] = None self.logger = logging.getLogger(__name__) self.timestep_counter = 0
def run_step(self, target_speed): """ Execute one step of longitudinal control to reach a given target speed. :param target_speed: target speed in Km/h :return: throttle control """ current_speed = Vehicle.get_speed(self.agent.vehicle) return self._pid_control(target_speed, current_speed)
def update_lon_control_k_values(self): curr_speed = Vehicle.get_speed(self.agent.vehicle) keys = list(OPTIMIZED_LONGITUDINAL_PID_VALUES.keys()) for speed_boundary in keys: if curr_speed <= speed_boundary: pid = OPTIMIZED_LONGITUDINAL_PID_VALUES[speed_boundary] self._lon_controller._k_p, self._lon_controller._k_d, self._lon_controller._k_i, \ self._lon_controller._dt = pid.K_P, pid.K_D, pid.K_I, pid.dt break
def find_k_values(vehicle: Vehicle, config: dict) -> np.array: current_speed = Vehicle.get_speed(vehicle=vehicle) k_p, k_d, k_i = 1, 0, 0 for speed_upper_bound, kvalues in config.items(): speed_upper_bound = float(speed_upper_bound) if current_speed < speed_upper_bound: k_p, k_d, k_i = kvalues["Kp"], kvalues["Kd"], kvalues["Ki"] break return np.array([k_p, k_d, k_i])
def convert_vehicle_from_source_to_agent(self, source: JetsonVehicle) -> Vehicle: return Vehicle(transform=Transform( location=Location(x=-source.location[0], y=source.location[1], z=-source.location[2]), rotation=Rotation(roll=-source.rotation[2], pitch=source.rotation[0], yaw=-source.rotation[1]), ), velocity=Vector3D(x=-source.velocity[0], y=source.velocity[1], z=-source.velocity[2]), wheel_base=0.26, control=self.convert_control_from_source_to_agent(source=source) )
def find_k_values(vehicle: Vehicle, config: dict) -> np.array: current_speed = Vehicle.get_speed(vehicle=vehicle) k_p, k_d, k_i = .8, 0, 0 for speed_upper_bound, kvalues in config.items(): speed_upper_bound = float(speed_upper_bound) if current_speed < speed_upper_bound: k_p, k_d, k_i = kvalues["Kp"]*.9, kvalues["Kd"]*.5, kvalues["Ki"]*.4 #******* lowered gain for smoothness break return np.clip([k_p, k_d, k_i], a_min=0, a_max=1)
def _terminal(self) -> bool: is_done = super(ROARPIDEnv, self)._terminal() if is_done: return True else: if self.agent.time_counter > 100 and Vehicle.get_speed( self.agent.vehicle) < 1: return True else: return False
def convert_vehicle_from_source_to_agent(self, source: carla.Vehicle) -> Vehicle: """Converts Velocity, Transform, and Control of carla.Vehicle""" control: VehicleControl = self.convert_control_from_source_to_agent( source.get_control()) # this is cheating here, vehicle does not know its own location transform: Transform = self.convert_transform_from_source_to_agent( source.get_transform()) velocity: Vector3D = self.convert_vector3d_from_source_to_agent( source.get_velocity()) return Vehicle(velocity=velocity, transform=transform, control=control)
def find_k_values(vehicle: Vehicle, config: dict) -> np.array: current_speed = Vehicle.get_speed(vehicle=vehicle) k_p, k_d, k_i = 1, 0, 0 for speed_upper_bound, kvalues in config.items(): speed_upper_bound = float(speed_upper_bound) if current_speed < speed_upper_bound: k_p, k_d, k_i = kvalues["Kp"], kvalues["Kd"], kvalues["Ki"] break # this clips the K values to be at most 1, so setting any K values larger than # that does absolutely nothing lmao return np.clip([k_p, k_d, k_i], a_min=0, a_max=1)
def find_k_values(vehicle: Vehicle, config: dict) -> np.array: current_speed = Vehicle.get_speed(vehicle=vehicle) #k_p, k_d, k_i = .03, 0.9, 0 #original values k_p, k_d, k_i, = .1, 0, 0 for speed_upper_bound, kvalues in config.items(): speed_upper_bound = float(speed_upper_bound) if current_speed < speed_upper_bound: k_p, k_d, k_i = kvalues["Kp"], kvalues["Kd"], kvalues["Ki"] break return np.clip([k_p, k_d, k_i], a_min=0, a_max=1)
def run_in_series(self, next_waypoint: Transform, **kwargs) -> VehicleControl: throttle = self.long_pid_controller.run_in_series(next_waypoint=next_waypoint, target_speed=kwargs.get("target_speed", self.max_speed)) steering = self.lat_pid_controller.run_in_series(next_waypoint=next_waypoint) if abs(steering) > 0.3 and Vehicle.get_speed(self.agent.vehicle) > 50: print("TOO FAST, FORCED THROTTLE DECREASE") throttle = -1 if abs(steering) < 0.1: throttle = 1 return VehicleControl(throttle=throttle, steering=steering)
def get_reward(self) -> float: reward: float = 0.0 current_speed = Vehicle.get_speed(self.agent.vehicle) if self.carla_runner.get_num_collision() > self.max_collision_allowed: reward -= 1000000 # if the agent is able to complete a lap, reward heavy if self.agent.is_done: reward += 100 if self.agent.time_counter > 100 and Vehicle.get_speed( self.agent.vehicle) < 1: # so that the agent is encouraged to move reward -= 100000 try: if len(self.agent.local_planner.way_points_queue) > 0 and \ self._prev_waypoint != self.agent.local_planner.way_points_queue[0].to_array(): # so that the agent is encouraged to go to the next waypoint reward += 100 except Exception as e: pass # if the agent tries to turn too much, penalize if abs(self.agent.vehicle.control.steering) > 0.3: # prevent it from over steering reward -= 10 if current_speed < 10: # again, agent is encouraged to move reward = 0 else: reward += current_speed if current_speed < 80: # i know that you can drive around the track at least with this speed reward -= 100 if current_speed > self._prev_speed: # agent is incentivized to go faster reward += 1 return reward
def convert_vehicle_from_source_to_agent(self, source: JetsonVehicle) -> Vehicle: """ Converts Transform, Velocity, Wheel_Base, and Control of JetsonVehicle. Args: source (): Returns: Vehicle(Transform, Velocity, Wheel_Base, Control) """ return Vehicle( wheel_base=0.26, control=self.convert_control_from_source_to_agent(source=source))
def get_reward(self) -> float: """ Reward policy: Surviving = +1 Going forward (positive velocity) = +1 Going toward a waypoint = +10 Speed < 10 = -10 Speed > 80 = +50 Collision = -10000 abs(steering) > 0.5 = -100 Returns: reward according to the aforementioned policy """ reward: float = 1.0 if Vehicle.get_speed(self.agent.vehicle) > 10: reward += 1 if len(self.agent.traditional_local_planner.way_points_queue ) > 0 and self._prev_location is not None: next_waypoint: Transform = self.agent.traditional_local_planner.way_points_queue[ 0] curr_location: Location = self.agent.vehicle.transform.location if curr_location.distance( next_waypoint.location) < curr_location.distance( self._prev_location): print("Getting closer to next waypoint!!!") reward += 1000 if Vehicle.get_speed(self.agent.vehicle) < 10: reward -= 10 if Vehicle.get_speed(self.agent.vehicle) > 80: reward += 50 if self.carla_runner.get_num_collision() > 0: reward -= 10000 if abs(self.agent.vehicle.control.steering) > 0.5: reward -= 100 self.reward = reward return reward
def main(): try: agent_config = AgentConfig.parse_file(Path("./ROAR_Jetson/configurations/agent_configuration.json")) jetson_config = JetsonConfig.parse_file(Path("./ROAR_Jetson/configurations/configuration.json")) try: prepare(jetson_config=jetson_config) except Exception as e: logging.error(f"Ignoring Error during setup: {e}") agent = PIDAgent(vehicle=Vehicle(), agent_settings=agent_config, should_init_default_cam=False) jetson_runner = JetsonRunner(agent=agent, jetson_config=jetson_config) jetson_runner.start_game_loop(use_manual_control=False) except Exception as e: print(f"Something bad happened {e}")