Skip to content

Commit

Permalink
only integers for rate index
Browse files Browse the repository at this point in the history
  • Loading branch information
robotastic committed May 13, 2024
1 parent a9916c0 commit 3bb4d23
Show file tree
Hide file tree
Showing 4 changed files with 21 additions and 13 deletions.
6 changes: 5 additions & 1 deletion axis-ptz-controller/axis_ptz_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -846,8 +846,12 @@ def _object_callback(
include_age=self.include_age,
lead_time=self.lead_time,
)

self.object.update_from_msg(data)
self.object.recompute_location()

logging.info(f"Tracking object: {self.object.object_id}")

if self.use_camera:
self._slew_camera(self.object.rho, self.object.tau)
return
Expand All @@ -857,7 +861,7 @@ def _object_callback(

if self.use_camera and self.object.tau < 0:
logging.info(f"Stopping image capture of object: {self.object.object_id}")

self.object = None
self.do_capture = False
self.status = Status.SLEEPING
logging.info(
Expand Down
5 changes: 5 additions & 0 deletions axis-ptz-controller/camera.py
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,8 @@ def _config_log(self) -> None:
"E_XYZ_to_ENz": self.E_XYZ_to_ENz,
"E_XYZ_to_uvw": self.E_XYZ_to_uvw,
"xyz_point": self.xyz_point,
"pan_rate_max": self.pan_rate_max,
"tilt_rate_max": self.tilt_rate_max,
}

logging.info(f"Camera configuration: {config}")
Expand Down Expand Up @@ -411,6 +413,7 @@ def _compute_pan_rate_index(self, rho_dot: float) -> None:

else:
self.pan_rate_index = (100 / self.pan_rate_max) * rho_dot
self.pan_rate_index = round(self.pan_rate_index)

def _compute_tilt_rate_index(self, tau_dot: float) -> None:
"""Compute tilt rate index between -100 and 100 using rates in
Expand All @@ -433,6 +436,8 @@ def _compute_tilt_rate_index(self, tau_dot: float) -> None:

else:
self.tilt_rate_index = (100 / self.tilt_rate_max) * tau_dot

self.tilt_rate_index = round(self.tilt_rate_index)

def get_yaw_pitch_roll(self) -> Tuple[float, float, float]:
"""
Expand Down
1 change: 0 additions & 1 deletion axis-ptz-controller/camera_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@ def continuous_move(self, pan: int = None, tilt: int = None):
Args:
pan: speed of movement of Pan.
tilt: speed of movement of Tilt.
zoom: speed of movement of Zoom.
Returns:
Returns the response from the device to the command sent.
Expand Down
22 changes: 11 additions & 11 deletions axis-ptz-controller/object.py
Original file line number Diff line number Diff line change
Expand Up @@ -322,22 +322,22 @@ def recompute_location(self) -> None:
)


# omega = (
# axis_ptz_utilities.cross(
# self.rst_point_lead_relative_to_tripod,
# self.rst_velocity_msg_relative_to_tripod,
# )
# / axis_ptz_utilities.norm(self.rst_point_lead_relative_to_tripod) ** 2
# )

# The original code use the position when the last MSG was received, not the position with leading, giving this a try
omega = (
axis_ptz_utilities.cross(
self.rst_point_msg_relative_to_tripod,
self.rst_point_lead_relative_to_tripod,
self.rst_velocity_msg_relative_to_tripod,
)
/ axis_ptz_utilities.norm(self.rst_point_msg_relative_to_tripod) ** 2
/ axis_ptz_utilities.norm(self.rst_point_lead_relative_to_tripod) ** 2
)

# The original code use the position when the last MSG was received, not the position with leading, giving this a try
# omega = (
# axis_ptz_utilities.cross(
# self.rst_point_msg_relative_to_tripod,
# self.rst_velocity_msg_relative_to_tripod,
# )
# / axis_ptz_utilities.norm(self.rst_point_msg_relative_to_tripod) ** 2
# )

self.rho_rate = math.degrees(-omega[2])
self.tau_rate = math.degrees(omega[0])
Expand Down

0 comments on commit 3bb4d23

Please sign in to comment.