From fb49fa3df8c3ccc70a31e63bbe9f12a61ee1d0b8 Mon Sep 17 00:00:00 2001 From: Luke Berndt Date: Sun, 12 May 2024 13:43:18 -0400 Subject: [PATCH] more data --- axis-ptz-controller/axis_ptz_controller.py | 6 ++++++ axis-ptz-controller/object.py | 6 +++--- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/axis-ptz-controller/axis_ptz_controller.py b/axis-ptz-controller/axis_ptz_controller.py index 8be60f5..688712b 100644 --- a/axis-ptz-controller/axis_ptz_controller.py +++ b/axis-ptz-controller/axis_ptz_controller.py @@ -725,6 +725,12 @@ def _track_object(self, time_since_last_update: float) -> None: "tau_dot_c": self.tau_dot_c, "rho_c_gain": self.rho_c_gain, "tau_c_gain": self.tau_c_gain, + "rst_vel_o_0": self.object.rst_velocity_msg_relative_to_tripod[0], + "rst_vel_o_1": self.object.rst_velocity_msg_relative_to_tripod[1], + "rst_vel_o_2": self.object.rst_velocity_msg_relative_to_tripod[2], + "rst_point_o_0": self.object.rst_point_msg_relative_to_tripod[0], + "rst_point_o_1": self.object.rst_point_msg_relative_to_tripod[1], + "rst_point_o_2": self.object.rst_point_msg_relative_to_tripod[2], "distance": self.object.distance_to_tripod3d, "focus": _focus, "zoom": _zoom, diff --git a/axis-ptz-controller/object.py b/axis-ptz-controller/object.py index d98147a..2881a07 100644 --- a/axis-ptz-controller/object.py +++ b/axis-ptz-controller/object.py @@ -167,8 +167,7 @@ def update_from_msg(self, msg: Dict[str, Any]) -> None: self.camera.get_xyz_to_enz_transformation_matrix().transpose(), self.enz_velocity_msg_relative_to_tripod, ) - logging.info(f"XYZ object velocity relative to tripod: {self.xyz_velocity_msg_relative_to_tripod} [m/s]") - logging.info(f"ENZ object velocity relative to tripod: {self.enz_velocity_msg_relative_to_tripod} [m/s]") + self.no_derivative = True # things get weird when you update the object and then recompute the derivative self.object_lock.release() @@ -322,6 +321,7 @@ def recompute_location(self) -> None: self.E_XYZ_to_rst, self.xyz_velocity_msg_relative_to_tripod ) + omega = ( axis_ptz_utilities.cross( self.rst_point_lead_relative_to_tripod, @@ -329,7 +329,7 @@ def recompute_location(self) -> None: ) / axis_ptz_utilities.norm(self.rst_point_lead_relative_to_tripod) ** 2 ) - + self.rho_rate = math.degrees(-omega[2]) self.tau_rate = math.degrees(omega[0]) self.no_derivative = False