diff --git a/axis-ptz-controller/axis_ptz_controller.py b/axis-ptz-controller/axis_ptz_controller.py index 2fae439..da0f15e 100644 --- a/axis-ptz-controller/axis_ptz_controller.py +++ b/axis-ptz-controller/axis_ptz_controller.py @@ -19,6 +19,7 @@ import traceback from types import FrameType from typing import Any, Dict, Optional, Union +from enum import Enum import numpy as np import quaternion @@ -30,6 +31,10 @@ from camera_configuration import CameraConfiguration from camera_control import CameraControl +class Status(Enum): + SLEEPING = 0 + SLEWING = 1 + TRACKING = 2 class AxisPtzController(BaseMQTTPubSub): """Point the camera at an object using a proportional rate @@ -60,10 +65,8 @@ def __init__( tripod_pitch: float = 0.0, tripod_roll: float = 0.0, pan_gain: float = 0.2, - pan_rate_min: float = 1.8, pan_rate_max: float = 150.0, tilt_gain: float = 0.2, - tilt_rate_min: float = 1.8, tilt_rate_max: float = 150.0, zoom: int = 6000, focus: int = 8749, @@ -132,14 +135,10 @@ def __init__( Roll angle of camera tripod from level North [degrees] pan_gain: float Proportional control gain for pan error [1/s] - pan_rate_min: float - Camera pan rate minimum [deg/s] pan_rate_max: float Camera pan rate maximum [deg/s] tilt_gain: float Proportional control gain for tilt error [1/s] - tilt_rate_min: float - Camera tilt rate minimum [deg/s] tilt_rate_max: float Camera tilt rate maximum [deg/s] zoom: int @@ -201,10 +200,8 @@ def __init__( self.beta = tripod_pitch self.gamma = tripod_roll self.pan_gain = pan_gain - self.pan_rate_min = pan_rate_min self.pan_rate_max = pan_rate_max self.tilt_gain = tilt_gain - self.tilt_rate_min = tilt_rate_min self.tilt_rate_max = tilt_rate_max self.zoom = zoom self.focus = focus @@ -293,6 +290,9 @@ def __init__( # to camera fixed (rst) coordinates self.E_XYZ_to_rst = np.zeros((3, 3)) + self.r_XYZ_o_0_t = np.zeros((3,)) + self.v_XYZ_o_0_t = np.zeros((3,)) + # Object pan and tilt rates self.rho_dot_o = 0.0 # [deg/s] self.tau_dot_o = 0.0 # [deg/s] @@ -309,6 +309,19 @@ def __init__( # Camera pan and tilt rate differences self.delta_rho_dot_c = 0.0 # [deg/s] self.delta_tau_dot_c = 0.0 # [deg/s] + self.pan_rate_index = 0 + self.tilt_rate_index = 0 + + # Set the status for the controller + self.status = Status.SLEEPING + + # Object track, ground speed, and vertical rate + self.track_o = 0.0 # [deg] + self.ground_speed_o = 0.0 # [m/s] + self.vertical_rate_o = 0.0 # [m/s] + + # Lock to make sure object info is not used while being updated + self.object_lock = threading.Lock() # Camera focus parameters. Note that the focus setting is # minimum at and greater than the hyperfocal distance, and the @@ -383,7 +396,9 @@ def __init__( f"Absolute move to pan: {self.rho_c}, and tilt: {self.tau_c}, with zoom: {self.zoom}" ) try: - self.camera_control.absolute_move(self.rho_c, self.tau_c, self.zoom, 99) + self.camera_control.absolute_move( + self.rho_c, self.tau_c, self.zoom, 99 + ) except Exception as e: logging.error(f"Error: {e}") else: @@ -494,11 +509,8 @@ def _config_callback( self.lead_time = config.get("lead_time", self.lead_time) # [s] # Cannot set tripod yaw, pitch, and roll because of side effects self.pan_gain = config.get("pan_gain", self.pan_gain) # [1/s] - self.pan_rate_min = config.get("pan_rate_min", self.pan_rate_min) self.pan_rate_max = config.get("pan_rate_max", self.pan_rate_max) self.tilt_gain = config.get("tilt_gain", self.tilt_gain) # [1/s] - self.tilt_gain = config.get("tilt_gain", self.tilt_gain) - self.tilt_rate_min = config.get("tilt_rate_min", self.tilt_rate_min) self.tilt_rate_max = config.get("tilt_rate_max", self.tilt_rate_max) self.zoom = config.get("zoom", self.zoom) # [0-9999] self.focus = config.get("focus", self.focus) # [7499-9999] @@ -563,10 +575,8 @@ def _log_config(self) -> None: "tripod_pitch": self.beta, "tripod_roll": self.gamma, "pan_gain": self.pan_gain, - "pan_rate_min": self.pan_rate_min, "pan_rate_max": self.pan_rate_max, "tilt_gain": self.tilt_gain, - "tilt_rate_min": self.tilt_rate_min, "tilt_rate_max": self.tilt_rate_max, "zoom": self.zoom, "focus": self.focus, @@ -587,135 +597,16 @@ def _log_config(self) -> None: f"AxisPtzController configuration:\n{json.dumps(config, indent=4)}" ) - def _orientation_callback( - self, - _client: Union[mqtt.Client, None], - _userdata: Union[Dict[Any, Any], None], - msg: Union[mqtt.MQTTMessage, str], - ) -> None: - """ - Process orientation message. - - Parameters - ---------- - _client: Union[mqtt.Client, None] - MQTT client - _userdata: Union[Dict[Any, Any], None] - Any required user data - msg: Union[mqtt.MQTTMessage, Dict[Any, Any]] - An MQTT message, or dictionary - - Returns - ------- - None - """ - # Assign camera housing rotation angles - data = self.decode_payload(msg, "Orientation") - logging.info(f"Processing orientation msg data: {data}") - self.alpha = data["tripod_yaw"] # [deg] - self.beta = data["tripod_pitch"] # [deg] - self.gamma = data["tripod_roll"] # [deg] - - # Compute the rotations from the geocentric (XYZ) coordinate - # system to the camera housing fixed (uvw) coordinate system - logging.info(f"Initial E_XYZ_to_uvw: {self.E_XYZ_to_uvw}") - ( - self.q_alpha, - self.q_beta, - self.q_gamma, - self.E_XYZ_to_uvw, - _, - _, - _, - ) = axis_ptz_utilities.compute_camera_rotations( - self.e_E_XYZ, - self.e_N_XYZ, - self.e_z_XYZ, - self.alpha, - self.beta, - self.gamma, - self.rho_c, - self.tau_c, - ) - logging.info(f"Final E_XYZ_to_uvw: {self.E_XYZ_to_uvw}") - - def _reset_stop_timer(self) -> None: - if hasattr(self, "_timer"): - self._timer.cancel() # type: ignore - self._timer = threading.Timer(3, self._stop_timer_callback) - self._timer.start() - - def _stop_timer_callback(self) -> None: - # Call your function here - print("Timer callback called") - - logging.info(f"Stopping image capture of object, updates timed out") - self.do_capture = False - logging.info("Stopping continuous pan and tilt - updates timed out") - try: - self.camera_control.stop_move() - except Exception as e: - logging.error(f"Error: {e}") - - # ... existing code ... - - def _object_callback( - self, - _client: Union[mqtt.Client, None], - _userdata: Union[Dict[Any, Any], None], - msg: Union[mqtt.MQTTMessage, str], - ) -> None: - """ - Process object message. - - Parameters - ---------- - _client: Union[mqtt.Client, None] - MQTT client - _userdata: Union[Dict[Any, Any], None] - Any required user data - msg: Union[mqtt.MQTTMessage, Dict[Any, Any]] - An MQTT message, or dictionary - Returns - ------- - None - """ - # Assign identifier, time, position, and velocity of the - # object - data = self.decode_payload(msg, "Selected Object") - if not set( - [ - "object_id", - "object_type", - "timestamp", - "latitude", - "longitude", - "altitude", - "track", - "horizontal_velocity", - "vertical_velocity", - ] - ) <= set(data.keys()): - logging.info(f"Required keys missing from object message data: {data}") - return - logging.info(f"Processing object msg data: {data}") - self.timestamp_o = float(data["timestamp"]) # [s] - self.timestamp_c = self.timestamp_o - self.lambda_o = data["longitude"] # [deg] - self.varphi_o = data["latitude"] # [deg] - self.h_o = data["altitude"] # [m] - track_o = data["track"] # [deg] - ground_speed_o = data["horizontal_velocity"] # [m/s] - vertical_rate_o = data["vertical_velocity"] # [m/s] + def _compute_object_pointing(self) -> None: # 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 + self.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 @@ -729,13 +620,13 @@ def _object_callback( # 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.r_ENz_o_0_t = np.matmul(self.E_XYZ_to_ENz, self.r_XYZ_o_0_t) + self.track_o = math.radians(self.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, + self.ground_speed_o * math.sin(self.track_o), + self.ground_speed_o * math.cos(self.track_o), + self.vertical_rate_o, ] ) r_ENz_o_1_t = self.r_ENz_o_0_t + self.v_ENz_o_0_t * lead_time @@ -744,7 +635,7 @@ def _object_callback( # 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) + self.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 @@ -775,17 +666,17 @@ def _object_callback( math.atan2(r_uvw_o_1_t[2], axis_ptz_utilities.norm(r_uvw_o_1_t[0:2])) ) # [deg] logging.debug(f"Object pan and tilt: {self.rho_o}, {self.tau_o} [deg]") - object_id = data["object_id"] - if self.use_camera and self.tau_o < 0: - logging.info(f"Stopping image capture of object: {object_id}") - self.do_capture = False - logging.info( - "Stopping continuous pan and tilt - Object is below the horizon" - ) - try: - self.camera_control.stop_move() - except Exception as e: - logging.error(f"Error: {e}") + + + def _track_object(self) -> None: + + if self.status != Status.TRACKING: + return + + # Make sure Object info is not updated while pointing is being computed + self.object_lock.acquire() + + self._compute_object_pointing() if self.use_camera: # Get camera pan and tilt @@ -793,42 +684,6 @@ def _object_callback( logging.info( f"Camera pan, tilt, zoom, and focus: {self.rho_c} [deg], {self.tau_c} [deg], {_zoom}, {_focus}" ) - self._reset_stop_timer() - - # Point the camera at any new object directly - if self.object_id != object_id: - self.object_id = object_id - if self.auto_focus: - logging.info( - f"Absolute move to pan: {self.rho_o}, and tilt: {self.tau_o}, with zoom: {self.zoom}" - ) - try: - self.camera_control.absolute_move( - self.rho_o, self.tau_o, self.zoom, 99 - ) - except Exception as e: - logging.error(f"Error: {e}") - else: - logging.info( - f"Absolute move to pan: {self.rho_o}, and tilt: {self.tau_o}, with zoom: {self.zoom}, and focus: {self.focus}" - ) - try: - self.camera_control.absolute_move( - self.rho_o, self.tau_o, self.zoom, 99, self.focus - ) - except Exception as e: - logging.error(f"Error: {e}") - - duration = max( - math.fabs(self._compute_angle_delta(self.rho_c, self.rho_o)) - / (self.pan_rate_max), - math.fabs(self._compute_angle_delta(self.tau_c, self.tau_o)) - / (self.tilt_rate_max), - ) - logging.info(f"Sleeping: {duration} [s]") - sleep(duration) - return - else: logging.debug(f"Controller pan and tilt: {self.rho_c}, {self.tau_c} [deg]") @@ -842,7 +697,6 @@ def _object_callback( logging.debug( f"Delta pan and tilt rates: {self.delta_rho_dot_c}, {self.delta_tau_dot_c} [deg/s]" ) - # Compute position and velocity in the camera fixed (rst) # coordinate system of the object relative to the tripod at # time zero after pointing the camera at the object @@ -864,8 +718,8 @@ def _object_callback( self.rho_o, self.tau_o, ) - self.r_rst_o_0_t = np.matmul(self.E_XYZ_to_rst, r_XYZ_o_0_t) - self.v_rst_o_0_t = np.matmul(self.E_XYZ_to_rst, v_XYZ_o_0_t) + self.r_rst_o_0_t = np.matmul(self.E_XYZ_to_rst, self.r_XYZ_o_0_t) + self.v_rst_o_0_t = np.matmul(self.E_XYZ_to_rst, self.v_XYZ_o_0_t) # Compute object slew rate omega = ( @@ -903,16 +757,19 @@ def _object_callback( except Exception as e: logging.error(f"Error: {e}") - pan_rate_index = self._compute_pan_rate_index(self.rho_dot_c) - tilt_rate_index = self._compute_tilt_rate_index(self.tau_dot_c) + self._compute_pan_rate_index(self.rho_dot_c) + self._compute_tilt_rate_index(self.tau_dot_c) logging.info( - f"Commanding pan and tilt rate indexes: {pan_rate_index}, {tilt_rate_index}" + f"Commanding pan and tilt rate indexes: {self.pan_rate_index}, {self.tilt_rate_index}" ) + + # All done with object info + self.object_lock.release() if self.camera_control.is_connected(): try: self.camera_control.continuous_move( - pan_rate_index, - tilt_rate_index, + self.pan_rate_index, + self.tilt_rate_index, 0.0, ) except Exception as e: @@ -955,8 +812,8 @@ def _object_callback( "tau_c": self.tau_c, "rho_dot_c": self.rho_dot_c, "tau_dot_c": self.tau_dot_c, - "pan_rate_index": pan_rate_index, - "tilt_rate_index": tilt_rate_index, + "pan_rate_index": self.pan_rate_index, + "tilt_rate_index": self.tilt_rate_index, "delta_rho_dot_c": self.delta_rho_dot_c, "delta_tau_dot_c": self.delta_tau_dot_c, "delta_rho": self._compute_angle_delta( @@ -973,6 +830,207 @@ def _object_callback( logging.debug(f"Publishing logger msg: {logger_msg}") self.publish_to_topic(self.logger_topic, logger_msg) + + def _slew_camera(self) -> None: + + if self.status == Status.SLEWING: + logging.error("Camera is already slewing") + return + + self.status = Status.SLEWING + + # Get camera pan and tilt + self.rho_c, self.tau_c, _zoom, _focus = self.camera_control.get_ptz() + logging.info( + f"Camera pan, tilt, zoom, and focus: {self.rho_c} [deg], {self.tau_c} [deg], {_zoom}, {_focus}" + ) + + if self.auto_focus: + logging.info( + f"Absolute move to pan: {self.rho_o}, and tilt: {self.tau_o}, with zoom: {self.zoom}" + ) + try: + self.camera_control.absolute_move( + self.rho_o, self.tau_o, self.zoom, 99 + ) + except Exception as e: + logging.error(f"Error: {e}") + else: + logging.info( + f"Absolute move to pan: {self.rho_o}, and tilt: {self.tau_o}, with zoom: {self.zoom}, and focus: {self.focus}" + ) + try: + self.camera_control.absolute_move( + self.rho_o, self.tau_o, self.zoom, 99, self.focus + ) + except Exception as e: + logging.error(f"Error: {e}") + + duration = max( + math.fabs(self._compute_angle_delta(self.rho_c, self.rho_o)) + / (self.pan_rate_max), + math.fabs(self._compute_angle_delta(self.tau_c, self.tau_o)) + / (self.tilt_rate_max), + ) + logging.info(f"Sleeping: {duration} [s]") + sleep(duration) + + # Start Tracking + self.status = Status.TRACKING + + + def _orientation_callback( + self, + _client: Union[mqtt.Client, None], + _userdata: Union[Dict[Any, Any], None], + msg: Union[mqtt.MQTTMessage, str], + ) -> None: + """ + Process orientation message. + + Parameters + ---------- + _client: Union[mqtt.Client, None] + MQTT client + _userdata: Union[Dict[Any, Any], None] + Any required user data + msg: Union[mqtt.MQTTMessage, Dict[Any, Any]] + An MQTT message, or dictionary + + Returns + ------- + None + """ + # Assign camera housing rotation angles + data = self.decode_payload(msg, "Orientation") + logging.info(f"Processing orientation msg data: {data}") + self.alpha = data["tripod_yaw"] # [deg] + self.beta = data["tripod_pitch"] # [deg] + self.gamma = data["tripod_roll"] # [deg] + + # Compute the rotations from the geocentric (XYZ) coordinate + # system to the camera housing fixed (uvw) coordinate system + logging.info(f"Initial E_XYZ_to_uvw: {self.E_XYZ_to_uvw}") + ( + self.q_alpha, + self.q_beta, + self.q_gamma, + self.E_XYZ_to_uvw, + _, + _, + _, + ) = axis_ptz_utilities.compute_camera_rotations( + self.e_E_XYZ, + self.e_N_XYZ, + self.e_z_XYZ, + self.alpha, + self.beta, + self.gamma, + self.rho_c, + self.tau_c, + ) + logging.info(f"Final E_XYZ_to_uvw: {self.E_XYZ_to_uvw}") + + def _reset_stop_timer(self) -> None: + if hasattr(self, "_timer"): + self._timer.cancel() # type: ignore + self._timer = threading.Timer(3, self._stop_timer_callback) + self._timer.start() + + def _stop_timer_callback(self) -> None: + # Call your function here + print("Timer callback called") + + logging.info(f"Stopping image capture of object, updates timed out") + self.do_capture = False + logging.info("Stopping continuous pan and tilt - updates timed out") + try: + self.camera_control.stop_move() + except Exception as e: + logging.error(f"Error: {e}") + + # ... existing code ... + + def _object_callback( + self, + _client: Union[mqtt.Client, None], + _userdata: Union[Dict[Any, Any], None], + msg: Union[mqtt.MQTTMessage, str], + ) -> None: + """ + Process object message. + + Parameters + ---------- + _client: Union[mqtt.Client, None] + MQTT client + _userdata: Union[Dict[Any, Any], None] + Any required user data + msg: Union[mqtt.MQTTMessage, Dict[Any, Any]] + An MQTT message, or dictionary + + Returns + ------- + None + """ + # Assign identifier, time, position, and velocity of the + # object + data = self.decode_payload(msg, "Selected Object") + if not set( + [ + "object_id", + "object_type", + "timestamp", + "latitude", + "longitude", + "altitude", + "track", + "horizontal_velocity", + "vertical_velocity", + ] + ) <= set(data.keys()): + logging.info(f"Required keys missing from object message data: {data}") + return + logging.info(f"Processing object msg data: {data}") + self.timestamp_o = float(data["timestamp"]) # [s] + self.timestamp_c = self.timestamp_o + self.lambda_o = data["longitude"] # [deg] + self.varphi_o = data["latitude"] # [deg] + self.h_o = data["altitude"] # [m] + self.track_o = data["track"] # [deg] + self.ground_speed_o = data["horizontal_velocity"] # [m/s] + self.vertical_rate_o = data["vertical_velocity"] # [m/s] + object_id = data["object_id"] + + if self.use_camera and self.tau_o < 0: + logging.info(f"Stopping image capture of object: {object_id}") + self.do_capture = False + self.status = Status.SLEEPING + logging.info( + "Stopping continuous pan and tilt - Object is below the horizon" + ) + try: + self.camera_control.stop_move() + except Exception as e: + logging.error(f"Error: {e}") + + # Reset the stop timer because we received an object message + self._reset_stop_timer() + + if self.use_camera: + + + # Point the camera at any new object directly + if self.object_id != object_id: + self.object_id = object_id + # Compute object pointing + self._compute_object_pointing() + self._slew_camera() + else: + self._track_object() + else: + logging.debug(f"Controller pan and tilt: {self.rho_c}, {self.tau_c} [deg]") + def _manual_control_callback( self, _client: Union[mqtt.Client, None], @@ -1123,7 +1181,7 @@ def _compute_angle_delta(self, theta_c: float, theta_o: float) -> float: return 0 return math.degrees(math.acos(d)) * c / math.fabs(c) - def _compute_pan_rate_index(self, rho_dot: float) -> int: + def _compute_pan_rate_index(self, rho_dot: float): """Compute pan rate index between -100 and 100 using rates in deg/s, limiting the results to the specified minimum and maximum. Note that the dead zone from -1.8 to 1.8 deg/s is ignored. @@ -1135,20 +1193,18 @@ def _compute_pan_rate_index(self, rho_dot: float) -> int: Returns ------- - pan_rate : int - Pan rate index """ if rho_dot < -self.pan_rate_max: - pan_rate = -100 + self.pan_rate = -100 elif self.pan_rate_max < rho_dot: - pan_rate = +100 + self.pan_rate = +100 else: - pan_rate = round((100 / self.pan_rate_max) * rho_dot) - return pan_rate + self.pan_rate = round((100 / self.pan_rate_max) * rho_dot) + - def _compute_tilt_rate_index(self, tau_dot: float) -> int: + def _compute_tilt_rate_index(self, tau_dot: float): """Compute tilt rate index between -100 and 100 using rates in deg/s, limiting the results to the specified minimum and maximum. Note that the dead zone from -1.8 to 1.8 deg/s is ignored. @@ -1160,18 +1216,15 @@ def _compute_tilt_rate_index(self, tau_dot: float) -> int: Returns ------- - tilt_rate : int - Tilt rate index """ if tau_dot < -self.tilt_rate_max: - tilt_rate = -100 + self.tilt_rate = -100 elif self.tilt_rate_max < tau_dot: - tilt_rate = +100 + self.tilt_rate = 100 else: - tilt_rate = round((100 / self.tilt_rate_max) * tau_dot) - return tilt_rate + self.tilt_rate = round((100 / self.tilt_rate_max) * tau_dot) def _capture_image(self) -> None: """When enabled, capture an image in JPEG format, and publish @@ -1428,10 +1481,8 @@ def make_controller() -> AxisPtzController: tripod_pitch=float(os.environ.get("TRIPOD_PITCH", 0.0)), tripod_roll=float(os.environ.get("TRIPOD_ROLL", 0.0)), pan_gain=float(os.environ.get("PAN_GAIN", 0.2)), - pan_rate_min=float(os.environ.get("PAN_RATE_MIN", 1.8)), pan_rate_max=float(os.environ.get("PAN_RATE_MAX", 150.0)), tilt_gain=float(os.environ.get("TILT_GAIN", 0.2)), - tilt_rate_min=float(os.environ.get("TILT_RATE_MIN", 1.8)), tilt_rate_max=float(os.environ.get("TILT_RATE_MAX", 150.0)), zoom=int(os.environ.get("ZOOM", 6000)), focus=int(os.environ.get("FOCUS", 8749)),