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
