From f4b108cdbfb99425e5b586d58c7672569fe77749 Mon Sep 17 00:00:00 2001 From: Luke Berndt Date: Thu, 22 Feb 2024 17:46:17 -0500 Subject: [PATCH] Add support for Yaw Pitch Roll to env Allow for Yaw, Pitch, Roll corrections to be saved in the ENV file and applied --- axis-ptz-controller.env | 3 +++ axis-ptz-controller/axis_ptz_controller.py | 22 +++++++++++++++++----- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/axis-ptz-controller.env b/axis-ptz-controller.env index c7900a9..293d266 100644 --- a/axis-ptz-controller.env +++ b/axis-ptz-controller.env @@ -22,6 +22,9 @@ LOOP_INTERVAL=0.1 CAPTURE_INTERVAL=2 CAPTURE_DIR=/data/tosort LEAD_TIME=0.5 +YAW=0.0 +PITCH=0.0 +ROLL=0.0 PAN_GAIN=6.0 PAN_RATE_MIN=1.8 PAN_RATE_MAX=150.0 diff --git a/axis-ptz-controller/axis_ptz_controller.py b/axis-ptz-controller/axis_ptz_controller.py index 3764af8..ebd331e 100644 --- a/axis-ptz-controller/axis_ptz_controller.py +++ b/axis-ptz-controller/axis_ptz_controller.py @@ -53,6 +53,9 @@ def __init__( capture_interval: int = 2, capture_dir: str = ".", lead_time: float = 0.5, + yaw: float = 0.0, + pitch: float = 0.0, + roll: float = 0.0, pan_gain: float = 0.2, pan_rate_min: float = 1.8, pan_rate_max: float = 150.0, @@ -115,6 +118,12 @@ def __init__( lead_time: float Lead time used when computing camera pointing to the object [s] + yaw: float + Yaw correction to the camera's positioning [degrees] + pitch: float + Pitch correction to the camera's positioning [degrees] + roll: float + Roll correction to the camera's positioning [degrees] pan_gain: float Proportional control gain for pan error [1/s] pan_rate_min: float @@ -199,6 +208,11 @@ def __init__( self.log_level = log_level self.continue_on_exception = continue_on_exception + # Tripod yaw, pitch, and roll angles + self.alpha = yaw # [deg] + self.beta = pitch # [deg] + self.gamma = roll # [deg] + # Always construct camera configuration and control since # instantiation only assigns arguments logging.info("Constructing camera configuration and control") @@ -228,11 +242,6 @@ def __init__( self.r_rst_o_0_t = np.zeros((3,)) # [m/s] self.v_rst_o_0_t = np.zeros((3,)) # [m/s] - # Tripod yaw, pitch, and roll angles - self.alpha = 0.0 # [deg] - self.beta = 0.0 # [deg] - self.gamma = 0.0 # [deg] - # Tripod yaw, pitch, and roll rotation quaternions self.q_alpha = quaternion.quaternion() self.q_beta = quaternion.quaternion() @@ -1209,6 +1218,9 @@ def make_controller() -> AxisPtzController: capture_interval=int(os.environ.get("CAPTURE_INTERVAL", 2)), capture_dir=os.environ.get("CAPTURE_DIR", "."), lead_time=float(os.environ.get("LEAD_TIME", 0.5)), + yaw=float(os.environ.get("YAW", 0.0)), + pitch=float(os.environ.get("PITCH", 0.0)), + roll=float(os.environ.get("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)),