class Configuration(BaseModel): # ROAR sensors settings name: str = Field(default="hero", title="Name of the agent", description="Duplicate from Carla Setting. " "But good to have") front_depth_cam: Camera = Field(default=Camera( fov=70, transform=Transform(location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0)), image_size_x=800, image_size_y=600), title="Front Depth Camera") front_rgb_cam: Camera = Field(default=Camera( fov=70, transform=Transform(location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0)), image_size_x=800, image_size_y=600), title="Front RGB Camera") rear_rgb_cam: Camera = Field(default=Camera( fov=145, transform=Transform(location=Location(x=-1.5, y=0.0, z=1.4), rotation=Rotation(pitch=0.0, yaw=180, roll=0.0)), image_size_x=800, image_size_y=600), title="Rear RGB Camera") # data path waypoint_file_path: str = Field( default=(Path(os.getcwd()) / "data" / "easy_map_waypoints.txt").as_posix()) json_waypoint_file_path: str = Field( default=(Path(os.getcwd()) / "data" / "easy_map_waypoints.json").as_posix()) output_data_folder_path: str = Field(default=(Path(os.getcwd()) / "data" / "output")) # miscellaneous settings enable_autopilot: bool = Field(default=True, title="Enable Antopilot", description="Enable Antopilot") spawn_point_id: int = Field(default=1, title="Spaning Location ID", description="Spanning Location ID") show_sensors_data: bool = Field(default=False) graph_post_modem_data: bool = Field(default=False) save_sensor_data: bool = Field(default=False) absolute_maximum_map_size: int = Field( default=1000, title="Absolute Maximum size of the map", description="This variable is used to intialize the Occupancy grid map." "The bigger it is, the more impact it is going to have on the runtime computation" "However, if it is smaller than the actual map, some weird things can happen" ) target_speed: int = 80
def get_visualize_locs(self, size=10): if self.strip_list is not None: return self.strip_list name = self.eq.__name__ if name == 'vertical_eq': xs = np.repeat(self.x2, size) zs = np.arange(self.z2 - (size // 2), self.z2 + (size // 2)) elif name == 'horizontal_eq': xs = np.arange(self.x2 - (size // 2), self.x2 + (size // 2)) zs = np.repeat(self.z2, size) else: range_ = size * np.cos(np.arctan(self.slope)) xs = np.linspace(self.x2 - range_ / 2, self.x2 + range_ / 2, num=size) zs = self.slope * xs + self.intercept # print(np.vstack((xs, zs)).T) # self.strip_list = np.vstack((xs, zs)).T self.strip_list = [] for i in range(len(xs)): self.strip_list.append(Location(x=xs[i], y=0, z=zs[i])) return self.strip_list
def calc_cross_track_error(self, next_waypoint: Transform) -> float: # calculate a vector that represent where you are going v_begin = self.agent.vehicle.transform.location v_end = v_begin + Location( x=math.cos( math.radians(self.agent.vehicle.transform.rotation.pitch)), y=v_begin.y, z=math.sin( math.radians(self.agent.vehicle.transform.rotation.pitch)), ) v_vec = np.array( [v_end.x - v_begin.x, v_end.y - v_begin.y, v_end.z - v_begin.z]) # calculate error projection w_vec = np.array([ next_waypoint.location.x - v_begin.x, next_waypoint.location.y - v_begin.y, next_waypoint.location.z - v_begin.z, ]) _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[1] > 0: _dot *= -1.0 return _dot
def _calculate_angle_error(self, next_waypoint: Transform): # calculate a vector that represent where you are going v_begin = self.agent.vehicle.control.location v_end = v_begin + Location( x=math.cos(math.radians( self.agent.vehicle.control.rotation.pitch)), y=v_begin.y, z=math.sin(math.radians( self.agent.vehicle.control.rotation.pitch)), ) # we ignore the vertical/altitude component, which is y, and only consider horizontal angle for # steering control v_vec = np.array([v_end.x - v_begin.x, 0, v_end.z - v_begin.z]) # calculate error projection w_vec = np.array([ next_waypoint.location.x - v_begin.x, 0, # again, ignoring vertical component next_waypoint.location.z - v_begin.z, ]) _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[1] > 0: _dot *= -1.0 return _dot
class Camera(BaseModel): fov: int = Field(default=70, title="Field of View") transform: Transform = Field( default=Transform( location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0), ) ) image_size_x: int = Field(default=800, title="Image size width") image_size_y: int = Field(default=600, title="Image size width") distortion_coefficient: Optional[np.ndarray] = Field(default=np.array([0, 0, 0, 0, 0]), title="Distortion Coefficient from Intel Realsense") data: Optional[np.ndarray] = Field(default=None) intrinsics_matrix: Optional[np.ndarray] = Field(default=None) def calculate_default_intrinsics_matrix(self) -> np.ndarray: """ Calculate intrinsics matrix Will set the attribute intrinsic matrix so that re-calculation is not necessary. https://github.com/carla-simulator/carla/issues/56 [ ax, 0, cx, 0, ay, cy, 0 , 0, 1 ] Returns: Intrinsics_matrix """ intrinsics_matrix = np.identity(3) intrinsics_matrix[0, 2] = self.image_size_x / 2.0 intrinsics_matrix[1, 2] = self.image_size_y / 2.0 intrinsics_matrix[0, 0] = self.image_size_x / ( 2.0 * np.tan(self.fov * np.pi / 360.0) ) intrinsics_matrix[1, 1] = self.image_size_y / ( 2.0 * np.tan(self.fov * np.pi / 360.0) ) self.intrinsics_matrix = intrinsics_matrix return intrinsics_matrix class Config: arbitrary_types_allowed = True def visualize(self, title="CameraData", duration=1) -> None: """ Visualize camera data. Args: title: title of cv2 image duration: in milisecond Returns: None """ if self.data is not None: cv2.imshow(title, self.data.data) cv2.waitKey(duration)
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 load_data(file_path: str) -> List[Transform]: waypoints = [] f = Path(file_path).open('r') for line in f.readlines(): x, y, z = line.split(",") x, y, z = float(x), float(y), float(z) l = Location(x=x, y=y, z=z) waypoints.append(Transform(location=l)) return waypoints
def cropped_occu_to_world(self, cropped_occu_coord: np.ndarray, vehicle_transform: Transform, occu_vehicle_center: np.ndarray): diff = cropped_occu_coord - occu_vehicle_center vehicle_occu_coord = self.location_to_occu_cord( location=vehicle_transform.location) coord = np.array(vehicle_occu_coord[0]) + diff coord = coord + [self._min_x, self._min_y] return Transform(location=Location(x=coord[0], y=0, z=coord[1]))
def convert_location_from_source_to_agent(self, source: carla.Location) -> Location: """ Convert Location data from Carla.location to Agent's location data type invert the Z axis to make it into right hand coordinate system Args: source: carla.location Returns: """ return Location(x=source.x, y=-source.z, z=-source.y)
def convert_location_from_source_to_agent(self, source: np.ndarray) -> Location: """ Convert Location data from Jetson Vehicle to Agent's location data type. Args: source (): Returns: Location(x, y, z) """ return Location(x=source[0], y=source[1], z=source[2])
def run_in_series(self, next_waypoint: Transform, **kwargs) -> float: """ Calculates a vector that represent where you are going. Args: next_waypoint (): **kwargs (): Returns: lat_control """ # calculate a vector that represent where you are going v_begin = self.agent.vehicle.transform.location v_end = v_begin + Location( x=math.cos( math.radians(self.agent.vehicle.transform.rotation.pitch)), y=0, z=math.sin( math.radians(self.agent.vehicle.transform.rotation.pitch)), ) v_vec = np.array([v_end.x - v_begin.x, 0, v_end.z - v_begin.z]) v_vec = np.array([v_end.x - v_begin.x, 0, v_end.z - v_begin.z]) # calculate error projection w_vec = np.array([ next_waypoint.location.x - v_begin.x, 0, next_waypoint.location.z - v_begin.z, ]) _dot = math.acos( np.clip( np.dot(v_vec, w_vec) / (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0, )) _cross = np.cross(v_vec, w_vec) if _cross[1] > 0: _dot *= -1 self._error_buffer.append(_dot) 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 k_p, k_d, k_i = PIDController.find_k_values(config=self.config, vehicle=self.agent.vehicle) lat_control = float( np.clip((k_p * _dot) + (k_d * _de) + (k_i * _ie), self.steering_boundary[0], self.steering_boundary[1])) return lat_control
def convert_vive_tracker_data_from_source_to_agent(self, source: Optional[ViveTrackerMessage]) -> \ Optional[ViveTrackerData]: if source is not None: vive_tracker_data = ViveTrackerData( location=Location(x=-source.x, y=source.y, z=-source.z), rotation=Rotation(roll=-source.roll, pitch=source.pitch + 60, yaw=-source.yaw), velocity=Vector3D(x=-source.vel_x, y=source.vel_y, z=-source.vel_z)) return vive_tracker_data return None
def _raw_coord_to_transform(self, raw: List[float]) -> Optional[Transform]: """ transform coordinate to Transform instance Args: raw: coordinate in form of [x, y, z, pitch, yaw, roll] Returns: Transform instance """ if len(raw) == 3: return Transform( location=Location(x=raw[0], y=raw[1], z=raw[2]), rotation=Rotation(pitch=0, yaw=0, roll=0), ) elif len(raw) == 6: return Transform( location=Location(x=raw[0], y=raw[1], z=raw[2]), rotation=Rotation(roll=raw[3], pitch=raw[4], yaw=raw[5]), ) else: self.logger.error(f"Point {raw} is invalid, skipping") return None
def _map_entry_to_transform(self, map_entry: MapEntry): """ Finds Transform of the midpoint between two points on the map. Args: map_entry (): Returns: Transform(Location) """ mid_point = (np.array(map_entry.point_a) + np.array(map_entry.point_b)) / 2 return Transform( location=Location(x=mid_point[0], y=mid_point[1], z=mid_point[2]))
def _pid_control(self, target_waypoint, vehicle_transform) -> float: """ Estimate the steering angle of the vehicle based on the PID equations :param target_waypoint: target waypoint :param vehicle_transform: current transform of the vehicle :return: steering control in the range [-1, 1] """ # calculate a vector that represent where you are going v_begin = vehicle_transform.location v_end = v_begin + Location( x=math.cos(math.radians(vehicle_transform.rotation.yaw)), y=math.sin(math.radians(vehicle_transform.rotation.yaw)), z=0, ) v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) # calculate error projection w_vec = np.array([ target_waypoint.location.x - v_begin.x, target_waypoint.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 self._e_buffer.append(_dot) if len(self._e_buffer) >= 2: _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self.dt _ie = sum(self._e_buffer) * self.dt else: _de = 0.0 _ie = 0.0 return float( np.clip((self.k_p * _dot) + (self.k_d * _de) + (self.k_i * _ie), -1.0, 1.0))
def run_in_series(self) -> VehicleControl: simplewaypointcontrol, targetwaypoint = super().run_in_series_for_dwa() # Gives control/target waypoint that the thinks should do next from simple_waypoint_following_local_planner if self.old_waypoint is None: self.old_waypoint = [targetwaypoint.location.x, targetwaypoint.location.z] vehicle_transform = self.agent.vehicle.transform vehicle_velo = self.agent.vehicle.velocity vehicle_control = self.agent.vehicle.control # this is the PREVIOUS control max_speed = self.agent.agent_settings.max_speed # 20.0 try: next_waypoint = self.calc_control_and_trajectory(vehicle_velo, vehicle_control, vehicle_transform, targetwaypoint, max_speed) formatted_nw = Transform(location=Location(x=next_waypoint[0], y=0, z=next_waypoint[1]), rotation=targetwaypoint.rotation) control: VehicleControl = self.controller.run_in_series(next_waypoint=formatted_nw) return control except: simplewaypointcontrol: VehicleControl = self.controller.run_in_series(next_waypoint=targetwaypoint) return simplewaypointcontrol
def convert_vive_tracker_data_from_source_to_agent(self, source: Optional[ViveTrackerMessage]) -> \ Optional[ViveTrackerData]: """ Converts raw Vive Tracker data to ViveTrackerData(Location, Rotation, Velocity). Args: source (): Returns: ViveTrackerData(Location, Rotation, Velocity) """ if source is not None: vive_tracker_data = ViveTrackerData( location=Location(x=-source.x, y=source.y, z=-source.z), rotation=Rotation( roll=-source.roll, pitch=source.pitch - 90, # 不知道为什么有60度的误差 yaw=-source.yaw), velocity=Vector3D(x=source.vel_x, y=source.vel_y, z=source.vel_z)) return vive_tracker_data return None
def run_step(self, sensors_data: SensorsData, vehicle: Vehicle) -> VehicleControl: super().run_step(sensors_data=sensors_data, vehicle=vehicle) # None if nothing is detected, else, transformation matrix P result: Optional[np.ndarray] = self.aruco_detector.run_in_series() if self.front_rgb_camera.data is not None: cv2.imshow("img", cv2.resize(self.front_rgb_camera.data, (600, 800))) if result is not None: # if i detected the aruco marker r: Rotation = self.aruco_detector.rotationMatrixToEulerAngles( result[0:3, 0:3]) t: Location = Location(x=result[0][3], y=result[1][3], z=result[2][3]) p: Transform = Transform(location=t, rotation=r) control = self.controller.run_in_series(next_waypoint=p) return control self.logger.info("No Aruco found, NEU") # execute previous control return VehicleControl(throttle=0, steering=0)
class Configuration(BaseModel): # ROAR sensors settings name: str = Field(default="hero", title="Name of the agent", description="Duplicate from Carla Setting. " "But good to have") front_depth_cam: Camera = Field(default=Camera( fov=70, transform=Transform(location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0)), image_size_x=800, image_size_y=600), title="Front Depth Camera") front_rgb_cam: Camera = Field(default=Camera( fov=70, transform=Transform(location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0)), image_size_x=800, image_size_y=600), title="Front RGB Camera") rear_rgb_cam: Camera = Field(default=Camera( fov=145, transform=Transform(location=Location(x=-1.5, y=0.0, z=1.4), rotation=Rotation(pitch=0.0, yaw=180, roll=0.0)), image_size_x=800, image_size_y=600), title="Rear RGB Camera") # data path waypoint_file_path: str = Field( default=(Path(os.getcwd()) / "data" / "easy_map_waypoints.txt").as_posix()) json_waypoint_file_path: str = Field( default=(Path(os.getcwd()) / "data" / "easy_map_waypoints.json").as_posix()) json_qr_code_file_path: str = Field(default=(Path(os.getcwd()) / "ROAR_Jetson" / "data" / "track_1.json").as_posix()) output_data_folder_path: str = Field(default=(Path(os.getcwd()) / "data" / "output")) # miscellaneous settings enable_autopilot: bool = Field(default=True, title="Enable Antopilot", description="Enable Antopilot") spawn_point_id: int = Field(default=1, title="Spaning Location ID", description="Spanning Location ID") show_sensors_data: bool = Field(default=False) save_sensor_data: bool = Field(default=False) absolute_maximum_map_size: int = Field( default=1000, title="Absolute Maximum size of the map", description="This variable is used to intialize the Occupancy grid map." "The bigger it is, the more impact it is going to have on the runtime computation" "However, if it is smaller than the actual map, some weird things can happen" ) target_speed: int = 80 pid_config_file_path: str = Field( default="./ROAR_Sim/configurations/pid_config.json") pid_config: dict = Field(default={60: { 1, 23, }}) depth_to_pcd_config_path: str = Field( default="./ROAR/configurations/carla/carla_depth_to_pcd_config.json") lqr_config_file_path: str = Field( default="./ROAR_Sim/configurations/lqr_config.json") occu_map_config_path: str = Field( default="./ROAR_Sim/configurations/occu_map_config.json") obstacle_from_depth_config_path: str = Field( default="./ROAR_Sim/configurations/obstacle_from_depth_config.json") simple_waypoint_local_planner_config_file_path: str = \ Field(default="./ROAR_Sim/configurations/simple_waypoint_local_planner_config.json") max_speed: float = Field( default=200, description="maximum speed in which the vehicle can drive at") num_laps: int = Field(default=1, description="Number of laps to run for")
class Configuration(BaseModel): # ROAR sensors settings name: str = Field(default="hero", title="Name of the agent", description="Duplicate from Carla Setting. " "But good to have") front_depth_cam: Camera = Field(default=Camera(fov=70, transform=Transform( location=Location(x=1.6, y=0, z=1.7 ), rotation=Rotation( pitch=0, yaw=0, roll=0)), image_size_x=800, image_size_y=600), title="Front Depth Camera") front_rgb_cam: Camera = Field(default=Camera(fov=70, transform=Transform( location=Location(x=1.6, y=0, z=1.7), rotation=Rotation(pitch=0, yaw=0, roll=0) ), image_size_x=800, image_size_y=600), title="Front RGB Camera") rear_rgb_cam: Camera = Field(default=Camera(fov=145, transform=Transform( location=Location(x=-1.5, y=0.0, z=1.4), rotation=Rotation( pitch=0.0, yaw=180, roll=0.0)), image_size_x=800, image_size_y=600), title="Rear RGB Camera") # data path waypoint_file_path: str = Field(default=(Path( os.getcwd()) / "data" / "easy_map_waypoints.txt").as_posix()) json_waypoint_file_path: str = Field(default=(Path( os.getcwd()) / "data" / "easy_map_waypoints.json").as_posix()) json_qr_code_file_path: str = Field(default=(Path( os.getcwd()) / "ROAR_Jetson" / "data" / "track_1.json" ).as_posix()) output_data_folder_path: str = Field( default=(Path(os.getcwd()) / "data" / "output")) # miscellaneous settings spawn_point_id: int = Field(default=1, title="Spaning Location ID", description="Spanning Location ID") show_sensors_data: bool = Field(default=False) save_sensor_data: bool = Field(default=False) absolute_maximum_map_size: int = Field( default=1000, title="Absolute Maximum size of the map", description="This variable is used to intialize the Occupancy grid map." "The bigger it is, the more impact it is going to have on the runtime computation" "However, if it is smaller than the actual map, some weird things can happen") enable_autopilot: bool = Field(default=True, title="Enable Antopilot", description="Enable Antopilot") num_laps: int = Field(default=1, description="Number of laps to run for") output_data_folder_path: str = Field(default="./data/output", description="path to save newly generated waypoints txt file.") output_data_file_name: str = Field(default="map_waypoints", description="file name for a newly generated waypoints txt file.") max_speed: float = Field(default=200, description="maximum speed in which the vehicle can drive at") target_speed: int = Field(default=80, description="The tracking speed that the pid controller is trying to achieve") steering_boundary: tuple = Field(default=(-1,1), description="maximum and minimum boundary for steering") # ROAR Academy: throttle_boundary: tuple = Field(default=(0,1), description="maximum and minimum boundary for steering") # ROAR Academy: waypoints_look_ahead_values: dict = Field(default={"60": 5, "80": 10, "120": 20, "180": 50}) # ROAR Academy: simple_waypoint_local_planner_config_file_path: str = \ Field(default="./ROAR_Sim/configurations/simple_waypoint_local_planner_config.json") pid_values: dict = Field(default={ 'longitudinal_controller': { '40': { 'Kp': 0.8, 'Kd': 0.2, 'Ki': 0 }, '60': { 'Kp': 0.5, 'Kd': 0.2, 'Ki': 0 }, '150': { 'Kp': 0.2, 'Kd': 0.1, 'Ki': 0.1 } }, 'latitudinal_controller': { '60': { 'Kp': 0.8, 'Kd': 0.1, 'Ki': 0.1 }, '100': { 'Kp': 0.6, 'Kd': 0.2, 'Ki': 0.1 }, '150': { 'Kp': 0.5, 'Kd': 0.2, 'Ki': 0.1 } } } ) # ROAR Academy pid_config_file_path: str = Field(default="./ROAR_Sim/configurations/pid_config.json") lqr_config_file_path: str = Field(default="./ROAR_Sim/configurations/lqr_config.json")
def convert_location_from_source_to_agent(self, source) -> Location: return Location(x=source.x, y=source.y, z=-source.z)
def run_in_series(self, next_waypoint: Transform, **kwargs) -> float: # calculate a vector that represent where you are going v_begin = self.agent.vehicle.transform.location v_end = v_begin + Location( x=math.cos( math.radians(self.agent.vehicle.transform.rotation.pitch)), y=v_begin.y, z=math.sin( math.radians(self.agent.vehicle.transform.rotation.pitch)), ) v_vec = np.array( [v_end.x - v_begin.x, v_end.y - v_begin.y, v_end.z - v_begin.z]) # calculate error projection w_vec = np.array([ next_waypoint.location.x - v_begin.x, next_waypoint.location.y - v_begin.y, next_waypoint.location.z - v_begin.z, ]) _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[1] > 0: _dot *= -1.0 self._error_buffer.append(_dot) # 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 # # # k_p, k_d, k_i = PIDController.find_k_values(config=self.config, vehicle=self.agent.vehicle) # # lat_control = float( # np.clip((.8 * _dot) + (0.3 * _de) + (0* _ie), self.steering_boundary[0], self.steering_boundary[1]) # ) #return lat_control #********************************************************S veh_pitch = self.agent.vehicle.transform.rotation.pitch # *** referencing next waypoint coordinates *** path_x = next_waypoint.location.x #*** next waypoint: self.way_points_queue[0] path_z = next_waypoint.location.z #** how get #*** averaging path points for smooth path vector *** next_pathpoint1 = (self.agent.local_planner.way_points_queue[1]) next_pathpoint2 = (self.agent.local_planner.way_points_queue[2]) next_pathpoint3 = (self.agent.local_planner.way_points_queue[3]) next_pathpoint4 = (self.agent.local_planner.way_points_queue[17]) next_pathpoint5 = (self.agent.local_planner.way_points_queue[18]) next_pathpoint6 = (self.agent.local_planner.way_points_queue[19]) nx = (next_pathpoint1.location.x + next_pathpoint2.location.x + next_pathpoint3.location.x + next_pathpoint4.location.x + next_pathpoint5.location.x + next_pathpoint6.location.x) / 6 nz = (next_pathpoint1.location.z + next_pathpoint2.location.z + next_pathpoint3.location.z + next_pathpoint4.location.z + next_pathpoint5.location.z + next_pathpoint6.location.z) / 6 nx1 = (next_pathpoint1.location.x + next_pathpoint2.location.x + next_pathpoint3.location.x) / 3 nz1 = (next_pathpoint1.location.z + next_pathpoint2.location.z + next_pathpoint3.location.z) / 3 #***get heading if vehicle was at the correct spot on path** path_pitch_rad = (math.atan2((nz - path_z), (nx - path_x))) path_pitch = path_pitch_rad * 180 / np.pi #***difference between correct heading and actual heading - pos error gives right steering, neg gives left *** head_err = path_pitch - veh_pitch # def run_in_series(self, next_waypoint: Transform, **kwargs) -> float: #*********** aka stanley_control(state, cx, cy, cyaw, last_target_idx) # ''' # TODO: tune # # *** inputs needed: vehicle yaw, x, y; nearest path yaw, x, y # # *** implement target calculations: heading error (vehicle yaw - path yaw at nearest path point); # cross track error (front axle x,y - nearest path x,y) # # self.way_points_queue[0] # # *** output lat_control: steering angle delta = heading error + inv tan (gain * cross track error/veh speed) # ''' # # # vel = self.agent.vehicle.velocity veh_spd = math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) #*** m/s k = 0.5 #control gain # veh_loc = self.agent.vehicle.transform.location #***** is this in radians or angle? guessing angle #pos_err, head_err = self.stan_calcs(next_waypoint) #lat_control = head_err + k * pos_err #if angle > 30 then 1, otherwise angle/180 ************ what does 1 equate to? 30 degrees? lat_control = float( np.clip((head_err + np.arctan(k * _dot / (veh_spd + .3))) / 60, self.steering_boundary[0], self.steering_boundary[1] ) #**** guessing steering of '1' equates to 30 degrees ) print('_dot Stanley = ', _dot) print('veh_spd', veh_spd) print('path pitch = ', path_pitch) print('veh pitch = ', veh_pitch) print('** head_err **', head_err) print('lat_control = ', lat_control) print('-----------------------------------------') return lat_control ## ************************************************ # # def stan_calcs(self, next_waypoint: Transform, **kwargs): # # ''' # calculate target # # front axle position (corrected from veh location + lv cos and sin heading # nearest point # # front axle error (distance from front axle to desired path position) # # front_axle_vec = [-np.cos(state.yaw + np.pi / 2), # -np.sin(state.yaw + np.pi / 2)] # error_front_axle = np.dot([dx[target_idx], dy[target_idx]], front_axle_vec) # ''' # # # *** vehicle data *** # # wb = 2.96 # assumed vehicle wheelbase (tesla) # # veh_x = self.agent.vehicle.transform.location.x # veh_y = self.agent.vehicle.transform.location.y # veh_z = self.agent.vehicle.transform.location.z # # veh_yaw = self.agent.vehicle.transform.rotation.yaw # veh_roll = self.agent.vehicle.transform.rotation.roll # veh_pitch = self.agent.vehicle.transform.rotation.pitch # # # # *** getting front axle coordinates *** # frontx = veh_x + wb*np.cos(veh_pitch*180/np.pi)/2 # frontz = veh_z + wb*np.sin(veh_pitch*180/np.pi)/2 # # # *** referencing next waypoint coordinates *** # path_x = next_waypoint.location.x #*** next waypoint: self.way_points_queue[0] # path_z = next_waypoint.location.z #** how get # # #*** averaging path points for smooth path vector *** # next_pathpoint1 = (self.agent.local_planner.way_points_queue[1]) # next_pathpoint2 = (self.agent.local_planner.way_points_queue[2]) # next_pathpoint3 = (self.agent.local_planner.way_points_queue[3]) # next_pathpoint4 = (self.agent.local_planner.way_points_queue[17]) # next_pathpoint5 = (self.agent.local_planner.way_points_queue[18]) # next_pathpoint6 = (self.agent.local_planner.way_points_queue[19]) # nx = (next_pathpoint1.location.x + next_pathpoint2.location.x + next_pathpoint3.location.x + next_pathpoint4.location.x + next_pathpoint5.location.x + next_pathpoint6.location.x)/6 # nz = (next_pathpoint1.location.z + next_pathpoint2.location.z + next_pathpoint3.location.z + next_pathpoint4.location.z + next_pathpoint5.location.z + next_pathpoint6.location.z) / 6 # nx1 = (next_pathpoint1.location.x + next_pathpoint2.location.x + next_pathpoint3.location.x) /3 # nz1 = (next_pathpoint1.location.z + next_pathpoint2.location.z + next_pathpoint3.location.z) /3 # # # # *** calculate crosstrack error *** # # *** calculate front axle position error from path with positive error = turn to right, negative = turn to left # # dx = [frontx - nx1] # dz = [frontz - nz1] # # dx = [frontx - path_x] # # dz = [frontz - path_z] # # dx = [veh_x - path_x] # # dz = [veh_z - path_z] # dpathhead_rad = (math.atan2((nz1-frontz), (nx1-frontx))) # *** need some lead to get sign correct (with sin)? # #dpathhead_rad = (math.atan2((path_z - frontz), (path_x - frontx))) # #dpathhead_rad = (math.atan2((path_z - veh_z), (path_x - veh_x))) # dpathhead_ang = dpathhead_rad * 180 / np.pi # pitch_to_path = dpathhead_ang - veh_pitch # dpath = np.sin(pitch_to_path*np.pi/180)*np.hypot(dx, dz) # *** pitch goes from + to - as crosses x axis # # # dpath = np.hypot(dx, dz)-8 # really should take this value * sign of pitch_to_path # # # # front_axle_vec = [-np.cos(veh_yaw + np.pi / 2), -np.sin(veh_yaw + np.pi / 2)] # RMS error? # # e_front_axle_pos = np.dot([nx, ny], front_axle_vec) # # #***get heading if vehicle was at the correct spot on path** # path_pitch_rad = (math.atan2((nz - path_z), (nx - path_x))) # path_pitch = path_pitch_rad*180/np.pi # # #***difference between correct heading and actual heading - pos error gives right steering, neg gives left *** # head_err = path_pitch - veh_pitch # # #************************************* borrowed from pid for _dot # # calculate a vector that represent where you are going # v_begin = self.agent.vehicle.transform.location # v_end = v_begin + Location( # x=math.cos(math.radians(self.agent.vehicle.transform.rotation.pitch)), # y=v_begin.y, # z=math.sin(math.radians(self.agent.vehicle.transform.rotation.pitch)), # ) # v_vec = np.array([v_end.x - v_begin.x,v_end.y - v_begin.y, v_end.z - v_begin.z]) # # # calculate error projection # w_vec = np.array( # [ # next_waypoint.location.x - v_begin.x, # next_waypoint.location.y - v_begin.y, # next_waypoint.location.z - v_begin.z, # ] # ) # _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[1] > 0: # _dot *= -1.0 # #*************************************** # # print('--------------------------------------') # # print('veh yaw = ', veh_yaw) # # print('veh roll = ', veh_roll) # print('veh pitch = ', veh_pitch) # print('**pitch to path** = ', pitch_to_path) # # # print('veh x = ', veh_x) # print('veh z = ', veh_z) # # print('front x = ', frontx) # # print('front z = ', frontz) # print('path x = ', path_x) # print('path z = ', path_z) # print('next path x = ', nx) # print('next path z = ', nz) # print('**distance to path = ', dpath) # print('path pitch = ', path_pitch) # print('path_pitch_rad = ', path_pitch_rad) # # print('path queue 0 = ', self.agent.local_planner.way_points_queue[0]) # # print('path queue 4 = ', self.agent.local_planner.way_points_queue[9]) # # print('path queue 20 = ', self.agent.local_planner.way_points_queue[17]) # print('** heading error **', head_err) # print('_dot err', _dot) # # return _dot, head_err '''
def run_in_series(self) -> VehicleControl: """ Run step for the local planner Procedure: 1. Sync data 2. get the correct look ahead for current speed 3. get the correct next waypoint 4. feed waypoint into controller 5. return result from controller Returns: next control that the local think the agent should execute. """ if (len(self.mission_planner.mission_plan) == 0 and len(self.way_points_queue) == 0): return VehicleControl() # get vehicle's location vehicle_transform: Union[Transform, None] = self.agent.vehicle.transform if vehicle_transform is None: raise AgentException( "I do not know where I am, I cannot proceed forward") # redefine closeness level based on speed self.set_closeness_threhold(self.closeness_threshold_config) # get current lane center lane_detector: LaneDetector = self.agent.lane_detector lane_center = lane_detector.lane_center if lane_center is not None: next_location = Location.from_array(lane_center[0] * 0.9 + lane_center[1] * 0.1) next_yaw, next_pitch = two_points_to_yaw_pitch( lane_center[0], lane_center[1]) next_rotation = Rotation(yaw=next_yaw, pitch=next_pitch, roll=0) target_waypoint = self.target_waypoint = Transform( location=next_location, rotation=next_rotation) else: target_waypoint = self.target_waypoint curr_closest_dist = float("inf") while True: if len(self.way_points_queue) == 0: self.logger.info("Destination reached") return VehicleControl() # waypoint: Transform = self.way_points_queue[0] waypoint: Transform = self.next_waypoint_smooth() curr_dist = vehicle_transform.location.distance(waypoint.location) if curr_dist < curr_closest_dist: # if i find a waypoint that is closer to me than before # note that i will always enter here to start the calculation for curr_closest_dist curr_closest_dist = curr_dist elif curr_dist < self.closeness_threshold: # i have moved onto a waypoint, remove that waypoint from the queue self.way_points_queue.popleft() else: break target_waypoint = target_waypoint * 0.4 + self.next_waypoint_smooth( ) * 0.6 control: VehicleControl = self.controller.run_in_series( next_waypoint=target_waypoint) self.logger.debug( f"\n" f"Curr Transform: {self.agent.vehicle.transform}\n" f"Target Location: {target_waypoint.location}\n" f"Control: {control} | Speed: {Vehicle.get_speed(self.agent.vehicle)}\n" ) return control
def _map_entry_to_transform(self, map_entry: MapEntry): mid_point = (np.array(map_entry.point_a) + np.array(map_entry.point_b)) / 2 return Transform( location=Location(x=mid_point[0], y=mid_point[1], z=mid_point[2]) )