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