def follower_step(self): if self.time_counter % 10 == 0: self.udp_multicast.send_current_state() # location x, y, z; rotation roll, pitch, yaw; velocity x, y, z; acceleration x, y, z if self.udp_multicast.msg_log.get(self.car_to_follow, None) is not None: control = self.controller.run_in_series( next_waypoint=Transform.from_array(self.udp_multicast.msg_log[ self.car_to_follow])) if self.front_depth_camera.data is not None and self.front_rgb_camera.data is not None: left, center, right = self.find_obstacles_via_depth_to_pcd() obstacle_detected = left[0] or center[0] or right[0] if obstacle_detected and Transform.from_array( self.udp_multicast.msg_log[self.car_to_follow] ).location.distance(self.vehicle.transform.location ) > self.controller.distance_to_keep: # if obstacle is detected and the obstacle is not the following vehicle, execute avoid sequence control = self.act_on_obstacle(left, center, right) return control return control else: # self.logger.info("No other cars found") 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()) 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 convert_transform_from_source_to_agent(self, source) -> Transform: return Transform( location=self.convert_location_from_source_to_agent( source=source.location), rotation=self.convert_rotation_from_source_to_agent( source=source.rotation), )
class Vehicle(BaseModel): """ Encodes the Vehicle's state at the last tick """ velocity: Optional[Vector3D] = Field(default=Vector3D(x=0, y=0, z=0)) transform: Optional[Transform] = Field(default=Transform()) control: VehicleControl = Field(default=VehicleControl()) # ? wheel_base: float = Field( default=2.875, title="Wheel Base length of the vehilce in meters", description="Default to tesla model 3's wheel base", ) @staticmethod def get_speed(vehicle): # TODO consider the jetson case """ Compute speed of a vehicle in Km/h. :param vehicle: the vehicle for which speed is calculated :return: speed as a float in Km/h """ vel = vehicle.velocity return 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)
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_transform_from_source_to_agent( self, source: carla.Transform ) -> Transform: """Convert CARLA raw location and rotation to Transform(location,rotation).""" return Transform( location=self.convert_location_from_source_to_agent(source=source.location), rotation=self.convert_rotation_from_source_to_agent(source=source.rotation), )
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 next_waypoint_smooth_and_speed( self, smooth_lookahead=400, speed_lookahead=600, min_speed_multiplier=0.6, speed_multiplier_slope=1.3) -> (Transform, float): """ Calculate the next target waypoint and speed for the controller. Parameters smooth_lookahead : int Number of waypoints ahead to look at to compute the smoothed waypoint. speed_lookahead : int Number of waypoint to look ahaed to compute speed factor. min_speed_multiplier : float The minimum value for the speed multiplier. speed_multiplier_slope : float The rate of speed multiplier decrease for every 180 degrees of angle error. Returns target_waypoint : Transform The next target waypoint for the controller speed_multiplier : float The speed multiplier for the controller's target speed. """ smooth_lookahead = min(smooth_lookahead, len(self.way_points_queue) - 1) speed_lookahead = min(speed_lookahead, len(self.way_points_queue) - 1) if smooth_lookahead > 10: # Reduce computation by only looking at every 10 steps ahead sample_points = range(0, smooth_lookahead, smooth_lookahead // 10) location_sum = reduce(lambda x, y: x + y, (self.way_points_queue[i].location for i in sample_points)) rotation_sum = reduce(lambda x, y: x + y, (self.way_points_queue[i].rotation for i in sample_points)) num_points = len(sample_points) target_waypoint = Transform(location=location_sum / num_points, rotation=rotation_sum / num_points) else: target_waypoint = self.way_points_queue[-1] if speed_lookahead > 0: angle_difference = self._calculate_angle_error( self.way_points_queue[speed_lookahead]) # Angle difference is between 0 and 180, but unlikely to be more than 90 speed_multiplier = max( min_speed_multiplier, (1.0 - speed_multiplier_slope * angle_difference / np.pi)) else: speed_multiplier = 1.0 return target_waypoint, speed_multiplier
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 _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 convert_transform_from_source_to_agent(self, source) -> Transform: """ Convert Jetson raw location and rotation to Transform(location,rotation). Args: source (): Returns: Transform(Location, Rotation) """ return Transform( location=self.convert_location_from_source_to_agent( source=source.location), rotation=self.convert_rotation_from_source_to_agent( source=source.rotation), )
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 __init__(self, agent: Agent, jetson_config: JetsonConfig): self.jetson_vehicle: JetsonVehicle = JetsonVehicle() self.jetson_config = jetson_config self.agent = agent self.jetson_bridge = JetsonBridge() self.logger = logging.getLogger("Jetson Runner") self.display: Optional[pygame.display] = None self.serial: Optional[serial.Serial] = None self.transform = Transform() self.controller = JetsonKeyboardControl() self.rs_d435i: Optional[RS_D435i] = None self.vive_tracker: Optional[ViveTrackerClient] = None if jetson_config.initiate_pygame: self.setup_pygame() self.setup_serial() self.setup_jetson_vehicle() self.auto_pilot = True self.pygame_initiated = False self.logger.info("Jetson Vehicle Connected and Intialized. All Hardware is online and running")
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 Vehicle(BaseModel): """ Encodes the Vehicle's state at the last tick """ velocity: Optional[Vector3D] = Field(default=Vector3D(x=0, y=0, z=0)) transform: Optional[Transform] = Field(default=Transform()) acceleration: Optional[Vector3D] = Field(default=Vector3D(x=0, y=0, z=0)) control: VehicleControl = Field(default=VehicleControl()) # ? wheel_base: float = Field( default=2.875, title="Wheel Base length of the vehilce in meters", description="Default to tesla model 3's wheel base", ) @staticmethod def get_speed(vehicle): # TODO consider the jetson case """ Compute speed of a vehicle in Km/h. :param vehicle: the vehicle for which speed is calculated :return: speed as a float in Km/h """ vel = vehicle.velocity return 3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2) def to_array(self) -> np.ndarray: return np.concatenate([ self.transform.to_array(), self.velocity.to_array(), self.acceleration.to_array(), self.control.to_array() ]) def __repr__(self): return f"Location: {self.transform.location.__str__()} | Rotation: {self.transform.rotation.__str__()} | " \ f"Velocity: {self.velocity.__str__()} | Acceleration: {self.acceleration.__str__()}"
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 _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]) )
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 next_waypoint_smooth_and_speed( self, smooth_factor=400, speed_lookahead=600) -> (Transform, float): # car_speed = np.linalg.norm(self.agent.vehicle.velocity.to_array()) * 3.6 # m/s to km/hr # lookahead_multiplier = car_speed / 170 # self.agent.agent_settings.target_speed # # waypoint_lookahead = max(waypoint_lookahead // 2, int(waypoint_lookahead * lookahead_multiplier)) # speed_lookahead = max(speed_lookahead // 2, int(speed_lookahead * lookahead_multiplier)) # # waypoint_lookahead = min(waypoint_lookahead, len(self.way_points_queue) - 1) # speed_lookahead = min(speed_lookahead, len(self.way_points_queue) - 1) # # print("WAYPOINT", waypoint_lookahead) # # if waypoint_lookahead > 1: # can get rid of if statement here # target_waypoint = self.way_points_queue[waypoint_lookahead] # else: # target_waypoint = self.way_points_queue[-1] smooth_factor = min(smooth_factor, len(self.way_points_queue) - 1) speed_lookahead = min(speed_lookahead, len(self.way_points_queue) - 1) if smooth_factor > 10: sample_points = range(0, smooth_factor, smooth_factor // 10) location_sum = reduce(lambda x, y: x + y, (self.way_points_queue[i].location for i in sample_points)) rotation_sum = reduce(lambda x, y: x + y, (self.way_points_queue[i].rotation for i in sample_points)) num_points = len(sample_points) target_waypoint = Transform(location=location_sum / num_points, rotation=rotation_sum / num_points) else: target_waypoint = self.way_points_queue[-1] # angle_std = np.std([self.way_points_queue[i].rotation.yaw for i in sample_points]) # # speed_multiplier = 1 - angle_std # speed_multiplier = 1.0 # print() # print([round(self.way_points_queue[i].rotation.yaw, 2) for i in sample_points]) # print([round(self.way_points_queue[i].rotation.pitch, 2) for i in sample_points]) # mean_pitch_0 = (self.way_points_queue[0].rotation.pitch + self.way_points_queue[1].rotation.pitch) / 2 # mean_pitch_ahead = (self.way_points_queue[speed_lookahead - 1].rotation.pitch + # self.way_points_queue[speed_lookahead].rotation.pitch) / 2 # angle_difference = abs(np.deg2rad(mean_pitch_0 - mean_pitch_ahead) % (2 * np.pi)) # angle_difference = min((2 * np.pi) - angle_difference, angle_difference) # # speed_multiplier = (1.0 - angle_difference / np.pi) if speed_lookahead > 0: # print(speed_lookahead, len(self.way_points_queue)) angle_difference = self._calculate_angle_error( self.way_points_queue[speed_lookahead]) # Angle difference is between 0 and 180, but unlikely to be more than 90 speed_multiplier = max(0.6, (1.0 - 1.3 * angle_difference / np.pi)) # speed_multiplier = np.exp(- 2.0 * angle_difference) * 0.5 + 0.5 # speed_multiplier = max(0.5, (1.0 - angle_difference / np.pi) ** 1.5) # speed_multiplier = max(0.5, (2.0 - (1.0 + angle_difference) ** 2)) # print("Angle difference:", np.degrees(angle_difference)) # print("Speed Multiplier", speed_multiplier) else: speed_multiplier = 0.0 # angles = [] # for index in (speed_lookahead, speed_lookahead // 5): # delta_x = self.way_points_queue[index].location.x - self.way_points_queue[0].location.x # delta_z = self.way_points_queue[index].location.z - self.way_points_queue[0].location.z # angles.append(np.arctan2(delta_z, delta_x)) # delta_angle = angles[0] - anglessum return target_waypoint, speed_multiplier