Skip to content

Commit

Permalink
catching errors
Browse files Browse the repository at this point in the history
  • Loading branch information
@robotastic
robotastic committed Apr 29, 2024
1 parent 1ccf65a commit 35c0e06
Show file tree
Hide file tree
Showing 2 changed files with 107 additions and 67 deletions.
38 changes: 37 additions & 1 deletion skyscan-c2/axis_ptz_utilities.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
import quaternion

# WGS84 parameters
R_OPLUS = 6378137 # [m]
R_OPLUS = 6378137 #6371008.7714 #6378137 # [m]
F_INV = 298.257223563


Expand Down Expand Up @@ -171,6 +171,9 @@ def compute_r_XYZ(
((1.0 - f) ** 2 * N + o_h) * np.sin(r_varphi_np),
),
)
else:
logging.error(f"compute_r_XYZ - Invalid data type for d_lambda: {type(d_lambda)}")
raise ValueError("Invalid data type for d_lambda")
return r_XYZ


Expand Down Expand Up @@ -274,6 +277,39 @@ def norm(v: npt.NDArray[np.float64]) -> float:
s += v[i] ** 2
return math.sqrt(s)

def compute_angle_delta( theta_c: float, theta_o: float) -> float:
"""Given the angle of the camera and object in a domain of
width 360 degrees, determine the angle delta, that is the
smallest difference in angle, signed according to the sign of
the angular rate required to bring the angle of the camera
toward the angle of the object.
Parameters
----------
theta_c : float
Pan or tilt of the camera [deg]
theta_o : float
Pan or tilt of the object [deg]
Returns
-------
float
Angle delta [deg]
"""
theta_c = math.radians(theta_c)
theta_o = math.radians(theta_o)
d = math.cos(theta_c) * math.cos(theta_o) + math.sin(theta_c) * math.sin(
theta_o
)
c = math.cos(theta_c) * math.sin(theta_o) - math.sin(theta_c) * math.cos(
theta_o
)
if math.fabs(c) == 0:
logging.info(
f"theta_c: {theta_c}, theta_o: {theta_o}, d: {d}, c: {c}, math.fabs(c): {math.fabs(c)}"
)
return 0
return math.degrees(math.acos(d)) * c / math.fabs(c)

def compute_camera_rotations(
e_E_XYZ: npt.NDArray[np.float64],
Expand Down
136 changes: 70 additions & 66 deletions skyscan-c2/c2_pub_sub.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,74 +203,78 @@ def _calculate_camera_angles(self: Any, data: Any) -> tuple[float, float, float]
ground_speed_o = data["horizontal_velocity"] # [m/s]
vertical_rate_o = data["vertical_velocity"] # [m/s]

# Compute position in the geocentric (XYZ) coordinate system
# of the object relative to the tripod at time zero, the
# observation time
r_XYZ_o_0 = axis_ptz_utilities.compute_r_XYZ(
self.lambda_o, self.varphi_o, self.h_o
)
r_XYZ_o_0_t = r_XYZ_o_0 - self.r_XYZ_t
try:
# Compute position in the geocentric (XYZ) coordinate system
# of the object relative to the tripod at time zero, the
# observation time
r_XYZ_o_0 = axis_ptz_utilities.compute_r_XYZ(
self.lambda_o, self.varphi_o, self.h_o
)
r_XYZ_o_0_t = r_XYZ_o_0 - self.r_XYZ_t

# Assign lead time, computing and adding age of object
# message, if enabled
lead_time = self.lead_time # [s]
# Assign lead time, computing and adding age of object
# message, if enabled
lead_time = self.lead_time # [s]

object_msg_age = time() - self.timestamp_o # [s]
logging.debug(
f"Object msg age: {object_msg_age} [s] Lead time: {lead_time} [s]"
)
lead_time += object_msg_age

# Compute position and velocity in the topocentric (ENz)
# coordinate system of the object relative to the tripod at
# time zero, and position at slightly later time one
self.r_ENz_o_0_t = np.matmul(self.E_XYZ_to_ENz, r_XYZ_o_0_t)
track_o = math.radians(track_o)
self.v_ENz_o_0_t = np.array(
[
ground_speed_o * math.sin(track_o),
ground_speed_o * math.cos(track_o),
vertical_rate_o,
]
)
r_ENz_o_1_t = self.r_ENz_o_0_t + self.v_ENz_o_0_t * lead_time

# Compute position, at time one, and velocity, at time zero,
# in the geocentric (XYZ) coordinate system of the object
# relative to the tripod
r_XYZ_o_1_t = np.matmul(self.E_XYZ_to_ENz.transpose(), r_ENz_o_1_t)
v_XYZ_o_0_t = np.matmul(self.E_XYZ_to_ENz.transpose(), self.v_ENz_o_0_t)

# Compute the distance between the object and the tripod at
# time one
self.distance3d = axis_ptz_utilities.norm(r_ENz_o_1_t)

# TODO: Restore?
# Compute the distance between the object and the tripod
# along the surface of a spherical Earth
# distance2d = axis_ptz_utilities.compute_great_circle_distance(
# self.self.lambda_t,
# self.varphi_t,
# self.lambda_o,
# self.varphi_o,
# ) # [m]

# Compute the object azimuth and elevation relative to the
# tripod
self.azm_o = math.degrees(math.atan2(r_ENz_o_1_t[0], r_ENz_o_1_t[1])) # [deg]
self.elv_o = math.degrees(
math.atan2(r_ENz_o_1_t[2], axis_ptz_utilities.norm(r_ENz_o_1_t[0:2]))
) # [deg]
# logging.info(f"Object azimuth and elevation: {self.azm_o}, {self.elv_o} [deg]")

# Compute pan and tilt to point the camera at the object
r_uvw_o_1_t = np.matmul(self.E_XYZ_to_uvw, r_XYZ_o_1_t)
self.rho_o = math.degrees(math.atan2(r_uvw_o_1_t[0], r_uvw_o_1_t[1])) # [deg]
self.tau_o = math.degrees(
math.atan2(r_uvw_o_1_t[2], axis_ptz_utilities.norm(r_uvw_o_1_t[0:2]))
) # [deg]

return self.rho_o, self.tau_o, self.distance3d
object_msg_age = time() - self.timestamp_o # [s]
logging.debug(
f"Object msg age: {object_msg_age} [s] Lead time: {lead_time} [s]"
)
lead_time += object_msg_age

# Compute position and velocity in the topocentric (ENz)
# coordinate system of the object relative to the tripod at
# time zero, and position at slightly later time one
self.r_ENz_o_0_t = np.matmul(self.E_XYZ_to_ENz, r_XYZ_o_0_t)
track_o = math.radians(track_o)
self.v_ENz_o_0_t = np.array(
[
ground_speed_o * math.sin(track_o),
ground_speed_o * math.cos(track_o),
vertical_rate_o,
]
)
r_ENz_o_1_t = self.r_ENz_o_0_t + self.v_ENz_o_0_t * lead_time

# Compute position, at time one, and velocity, at time zero,
# in the geocentric (XYZ) coordinate system of the object
# relative to the tripod
r_XYZ_o_1_t = np.matmul(self.E_XYZ_to_ENz.transpose(), r_ENz_o_1_t)
v_XYZ_o_0_t = np.matmul(self.E_XYZ_to_ENz.transpose(), self.v_ENz_o_0_t)

# Compute the distance between the object and the tripod at
# time one
self.distance3d = axis_ptz_utilities.norm(r_ENz_o_1_t)

# TODO: Restore?
# Compute the distance between the object and the tripod
# along the surface of a spherical Earth
# distance2d = axis_ptz_utilities.compute_great_circle_distance(
# self.self.lambda_t,
# self.varphi_t,
# self.lambda_o,
# self.varphi_o,
# ) # [m]

# Compute the object azimuth and elevation relative to the
# tripod
self.azm_o = math.degrees(math.atan2(r_ENz_o_1_t[0], r_ENz_o_1_t[1])) # [deg]
self.elv_o = math.degrees(
math.atan2(r_ENz_o_1_t[2], axis_ptz_utilities.norm(r_ENz_o_1_t[0:2]))
) # [deg]
# logging.info(f"Object azimuth and elevation: {self.azm_o}, {self.elv_o} [deg]")

# Compute pan and tilt to point the camera at the object
r_uvw_o_1_t = np.matmul(self.E_XYZ_to_uvw, r_XYZ_o_1_t)
self.rho_o = math.degrees(math.atan2(r_uvw_o_1_t[0], r_uvw_o_1_t[1])) # [deg]
self.tau_o = math.degrees(
math.atan2(r_uvw_o_1_t[2], axis_ptz_utilities.norm(r_uvw_o_1_t[0:2]))
) # [deg]

return self.rho_o, self.tau_o, self.distance3d
except Exception as e:
logging.error(f"Error: {e} latitude: {self.varphi_o}, longitude: {self.lambda_o}, altitude: {self.h_o}")
return 0.0, 0.0, 0.0

def _relative_distance_meters(
self: Any, lat_one: float, lon_one: float, lat_two: float, lon_two: float
Expand Down

0 comments on commit 35c0e06

Please sign in to comment.