Merge pull request #44 from IQTLabs/lberndt/yaw-env

Add support for Yaw Pitch Roll to env

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

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