Skip to content

Commit 29effb6

Browse files
robotasticrobotastic
committed
make it more robust to select object being none
1 parent efa6136 commit 29effb6

File tree

1 file changed

+166
-157
lines changed

1 file changed

+166
-157
lines changed

‎axis-ptz-controller/axis_ptz_controller.py

Lines changed: 166 additions & 157 deletions
Original file line numberDiff line numberDiff line change
@@ -594,172 +594,179 @@ def _track_object(self, time_since_last_update: float) -> None:
594594

595595
start_time = time()
596596

597+
try:
597598

598-
if self.use_camera:
599-
# Get camera pan and tilt
600-
self.rho_c, self.tau_c, _zoom, _focus = self.camera.get_ptz()
601-
# logging.info(
602-
# f"Camera pan, tilt, zoom, and focus: {self.rho_c} [deg], {self.tau_c} [deg], {_zoom}, {_focus}"
603-
# )
604-
else:
605-
logging.debug(f"Controller pan and tilt: {self.rho_c}, {self.tau_c} [deg]")
599+
if self.use_camera:
600+
# Get camera pan and tilt
601+
self.rho_c, self.tau_c, _zoom, _focus = self.camera.get_ptz()
602+
# logging.info(
603+
# f"Camera pan, tilt, zoom, and focus: {self.rho_c} [deg], {self.tau_c} [deg], {_zoom}, {_focus}"
604+
# )
605+
else:
606+
logging.debug(f"Controller pan and tilt: {self.rho_c}, {self.tau_c} [deg]")
606607

607608

608-
if self.object is None:
609-
logging.error(f"Not sure why it is None here, but not earlier")
610-
return
609+
if self.object is None:
610+
logging.error(f"Not sure why it is None here, but not earlier")
611+
return
611612

612-
# recompute the object's current location
613-
# we want to do this after getting the camera's current location because that is a network call
614-
# and it there is latency and jitter in how long it takes.
615-
self.object.recompute_location()
613+
# recompute the object's current location
614+
# we want to do this after getting the camera's current location because that is a network call
615+
# and it there is latency and jitter in how long it takes.
616+
self.object.recompute_location()
616617

617-
# Compute angle delta between camera and object pan and tilt
618-
self.delta_rho = axis_ptz_utilities.compute_angle_delta(
619-
self.camera.rho, self.object.rho
620-
)
621-
self.delta_tau = axis_ptz_utilities.compute_angle_delta(
622-
self.camera.tau, self.object.tau
623-
)
618+
# Compute angle delta between camera and object pan and tilt
619+
self.delta_rho = axis_ptz_utilities.compute_angle_delta(
620+
self.camera.rho, self.object.rho
621+
)
622+
self.delta_tau = axis_ptz_utilities.compute_angle_delta(
623+
self.camera.tau, self.object.tau
624+
)
625+
626+
# Compute slew rate differences
627+
628+
# tracking the rate of change for the object's pan and tilt allows us to amplify the gain
629+
# when the object is moving quickly past the camera
630+
object_rho_derivative = abs(self.object.rho_derivative)
631+
object_tau_derivative = abs(self.object.tau_derivative)
632+
633+
# we want to make sure the object derivative does not have a dampening effect on the gain
634+
if object_rho_derivative < 1:
635+
object_rho_derivative = 1
636+
if object_tau_derivative < 1:
637+
object_tau_derivative = 1
638+
if object_rho_derivative > self.pan_derivative_gain_max:
639+
object_rho_derivative = self.pan_derivative_gain_max
640+
if object_tau_derivative > self.tilt_derivative_gain_max:
641+
object_tau_derivative = self.tilt_derivative_gain_max
642+
643+
self.rho_c_gain = self.pan_gain * self.delta_rho * object_rho_derivative
644+
self.tau_c_gain = self.tilt_gain * self.delta_tau * object_tau_derivative
645+
646+
# Compute position and velocity in the camera fixed (rst)
647+
# coordinate system of the object relative to the tripod at
648+
# time zero after pointing the camera at the object
649+
650+
# Update camera pan and tilt rate
651+
self.rho_dot_c = self.object.rho_rate + self.rho_c_gain #- (self.object.rho_derivative ** 2)
652+
self.tau_dot_c = self.object.tau_rate + self.tau_c_gain #- (self.object.tau_derivative ** 2)
624653

625-
# Compute slew rate differences
626-
627-
# tracking the rate of change for the object's pan and tilt allows us to amplify the gain
628-
# when the object is moving quickly past the camera
629-
object_rho_derivative = abs(self.object.rho_derivative)
630-
object_tau_derivative = abs(self.object.tau_derivative)
631-
632-
# we want to make sure the object derivative does not have a dampening effect on the gain
633-
if object_rho_derivative < 1:
634-
object_rho_derivative = 1
635-
if object_tau_derivative < 1:
636-
object_tau_derivative = 1
637-
if object_rho_derivative > self.pan_derivative_gain_max:
638-
object_rho_derivative = self.pan_derivative_gain_max
639-
if object_tau_derivative > self.tilt_derivative_gain_max:
640-
object_tau_derivative = self.tilt_derivative_gain_max
641-
642-
self.rho_c_gain = self.pan_gain * self.delta_rho * object_rho_derivative
643-
self.tau_c_gain = self.tilt_gain * self.delta_tau * object_tau_derivative
644-
645-
# Compute position and velocity in the camera fixed (rst)
646-
# coordinate system of the object relative to the tripod at
647-
# time zero after pointing the camera at the object
648-
649-
# Update camera pan and tilt rate
650-
self.rho_dot_c = self.object.rho_rate + self.rho_c_gain #- (self.object.rho_derivative ** 2)
651-
self.tau_dot_c = self.object.tau_rate + self.tau_c_gain #- (self.object.tau_derivative ** 2)
652-
653-
# Get, or compute and set focus, command camera pan and tilt
654-
# rates, and begin capturing images, if needed
655-
if self.use_camera:
656-
# Note that focus cannot be negative, since distance_to_tripod3d
657-
# is non-negative
658-
self.camera.update_focus(self.object.distance_to_tripod3d)
659-
self.camera.update_pan_tilt_rates(self.rho_dot_c, self.tau_dot_c)
654+
# Get, or compute and set focus, command camera pan and tilt
655+
# rates, and begin capturing images, if needed
656+
if self.use_camera:
657+
# Note that focus cannot be negative, since distance_to_tripod3d
658+
# is non-negative
659+
self.camera.update_focus(self.object.distance_to_tripod3d)
660+
self.camera.update_pan_tilt_rates(self.rho_dot_c, self.tau_dot_c)
661+
662+
if self.object is None:
663+
logging.error(f"REALLY not sure why it is None here, but not earlier")
664+
return
665+
if not self.do_capture and self.object is not None:
666+
logging.info(
667+
f"Starting image capture of object: {self.object.object_id}"
668+
)
669+
self.do_capture = True
670+
self.capture_time = time()
660671

672+
if self.do_capture and time() - self.capture_time > self.capture_interval:
673+
capture_thread = threading.Thread(target=self._capture_image)
674+
capture_thread.daemon = True
675+
capture_thread.start()
676+
677+
elapsed_time = time() - start_time
678+
# logging.info(
679+
# f"\t⏱️\t {round(elapsed_time,3)}s RATES - 🎥 Pan: {self.rho_dot_c}\tTilt: {self.tau_dot_c}\t🛩️ Pan: {self.object.rho_rate}\tTilt: {self.object.tau_rate} ANGLES: 🎥 Pan: {self.rho_c}\tTilt: {self.tau_c}\t🛩️ Pan: {self.object.rho}\tTilt: {self.object.tau} "
680+
# )
681+
682+
# Compute position of aircraft relative to tripod in ENz, then XYZ,
683+
# then uvw coordinates
661684
if self.object is None:
662-
logging.error(f"REALLY not sure why it is None here, but not earlier")
685+
logging.error(f"Object is None, not tracking")
663686
return
664-
if not self.do_capture and self.object is not None:
665-
logging.info(
666-
f"Starting image capture of object: {self.object.object_id}"
667-
)
668-
self.do_capture = True
669-
self.capture_time = time()
670-
671-
if self.do_capture and time() - self.capture_time > self.capture_interval:
672-
capture_thread = threading.Thread(target=self._capture_image)
673-
capture_thread.daemon = True
674-
capture_thread.start()
675-
676-
elapsed_time = time() - start_time
677-
# logging.info(
678-
# f"\t⏱️\t {round(elapsed_time,3)}s RATES - 🎥 Pan: {self.rho_dot_c}\tTilt: {self.tau_dot_c}\t🛩️ Pan: {self.object.rho_rate}\tTilt: {self.object.tau_rate} ANGLES: 🎥 Pan: {self.rho_c}\tTilt: {self.tau_c}\t🛩️ Pan: {self.object.rho}\tTilt: {self.object.tau} "
679-
# )
680-
681-
# Compute position of aircraft relative to tripod in ENz, then XYZ,
682-
# then uvw coordinates
683-
r_ENz_a_t = np.array(
684-
[
685-
math.sin(math.radians(self.object.rho)) * math.cos(math.radians(self.object.tau)),
686-
math.cos(math.radians(self.object.rho)) * math.cos(math.radians(self.object.tau)),
687-
math.sin(math.radians(self.object.tau)),
688-
]
689-
)
690-
r_XYZ_a_t = np.matmul(self.camera.get_xyz_to_enz_transformation_matrix().transpose(), r_ENz_a_t)
691-
r_uvw_a_t = np.matmul(self.camera.get_xyz_to_uvw_transformation_matrix(), r_XYZ_a_t)
692-
693-
# Compute pan an tilt
694-
self.camera_pan = math.degrees(math.atan2(r_uvw_a_t[0], r_uvw_a_t[1])) # [deg]
695-
self.camera_tilt = math.degrees(
696-
math.atan2(r_uvw_a_t[2], axis_ptz_utilities.norm(r_uvw_a_t[0:2]))
697-
) # [deg]
698-
699-
700-
# Log camera pointing using MQTT
701-
if self.log_to_mqtt:
702-
logger_msg = self.generate_payload_json(
703-
push_timestamp=int(datetime.utcnow().timestamp()),
704-
device_type=os.environ.get("DEVICE_TYPE", "Collector"),
705-
id_=self.hostname,
706-
deployment_id=os.environ.get(
707-
"DEPLOYMENT_ID", f"Unknown-Location-{self.hostname}"
708-
),
709-
current_location=os.environ.get(
710-
"CURRENT_LOCATION",
711-
f"{self.camera.tripod_latitude}, {self.camera.tripod_longitude}",
712-
),
713-
status="Debug",
714-
message_type="Event",
715-
model_version="null",
716-
firmware_version="v0.0.0",
717-
data_payload_type="Logger",
718-
data_payload=json.dumps(
719-
{
720-
"camera-pointing": {
721-
"timestamp_c": self.timestamp_c,
722-
"rho_o": self.object.rho,
723-
"tau_o": self.object.tau,
724-
"rho_camera_o": self.camera_pan,
725-
"tau_camera_o": self.camera_tilt,
726-
"corrected_rho_delta": self.object.rho - self.camera_pan,
727-
"corrected_tau_delta": self.object.tau - self.camera_tilt,
728-
"rho_dot_o": self.object.rho_rate,
729-
"tau_dot_o": self.object.tau_rate,
730-
"rho_c": self.camera.rho,
731-
"tau_c": self.camera.tau,
732-
"rho_dot_c": self.rho_dot_c,
733-
"tau_dot_c": self.tau_dot_c,
734-
"rho_c_gain": self.rho_c_gain,
735-
"tau_c_gain": self.tau_c_gain,
736-
"rst_vel_o_0": self.object.rst_velocity_msg_relative_to_tripod[0],
737-
"rst_vel_o_1": self.object.rst_velocity_msg_relative_to_tripod[1],
738-
"rst_vel_o_2": self.object.rst_velocity_msg_relative_to_tripod[2],
739-
"rst_point_o_0": self.object.rst_point_msg_relative_to_tripod[0],
740-
"rst_point_o_1": self.object.rst_point_msg_relative_to_tripod[1],
741-
"rst_point_o_2": self.object.rst_point_msg_relative_to_tripod[2],
742-
"distance": self.object.distance_to_tripod3d,
743-
"focus": _focus,
744-
"zoom": _zoom,
745-
"object_location_update_period": self.object.location_update_period,
746-
"rho_derivative": self.object.rho_derivative,
747-
"tau_derivative": self.object.tau_derivative,
748-
"pan_rate_index": self.camera.pan_rate_index,
749-
"tilt_rate_index": self.camera.tilt_rate_index,
750-
"delta_rho_dot_c": self.delta_rho_dot_c,
751-
"delta_tau_dot_c": self.delta_tau_dot_c,
752-
"delta_rho": self.delta_rho,
753-
"delta_tau": self.delta_tau,
754-
"tracking_loop_time": elapsed_time,
755-
"time_since_last_update": time_since_last_update,
756-
"object_id": self.object.object_id,
757-
}
758-
}
759-
),
687+
688+
r_ENz_a_t = np.array(
689+
[
690+
math.sin(math.radians(self.object.rho)) * math.cos(math.radians(self.object.tau)),
691+
math.cos(math.radians(self.object.rho)) * math.cos(math.radians(self.object.tau)),
692+
math.sin(math.radians(self.object.tau)),
693+
]
760694
)
761-
self.publish_to_topic(self.logger_topic, logger_msg)
762-
695+
r_XYZ_a_t = np.matmul(self.camera.get_xyz_to_enz_transformation_matrix().transpose(), r_ENz_a_t)
696+
r_uvw_a_t = np.matmul(self.camera.get_xyz_to_uvw_transformation_matrix(), r_XYZ_a_t)
697+
698+
# Compute pan an tilt
699+
self.camera_pan = math.degrees(math.atan2(r_uvw_a_t[0], r_uvw_a_t[1])) # [deg]
700+
self.camera_tilt = math.degrees(
701+
math.atan2(r_uvw_a_t[2], axis_ptz_utilities.norm(r_uvw_a_t[0:2]))
702+
) # [deg]
703+
704+
705+
# Log camera pointing using MQTT
706+
if self.log_to_mqtt:
707+
logger_msg = self.generate_payload_json(
708+
push_timestamp=int(datetime.utcnow().timestamp()),
709+
device_type=os.environ.get("DEVICE_TYPE", "Collector"),
710+
id_=self.hostname,
711+
deployment_id=os.environ.get(
712+
"DEPLOYMENT_ID", f"Unknown-Location-{self.hostname}"
713+
),
714+
current_location=os.environ.get(
715+
"CURRENT_LOCATION",
716+
f"{self.camera.tripod_latitude}, {self.camera.tripod_longitude}",
717+
),
718+
status="Debug",
719+
message_type="Event",
720+
model_version="null",
721+
firmware_version="v0.0.0",
722+
data_payload_type="Logger",
723+
data_payload=json.dumps(
724+
{
725+
"camera-pointing": {
726+
"timestamp_c": self.timestamp_c,
727+
"rho_o": self.object.rho,
728+
"tau_o": self.object.tau,
729+
"rho_camera_o": self.camera_pan,
730+
"tau_camera_o": self.camera_tilt,
731+
"corrected_rho_delta": self.object.rho - self.camera_pan,
732+
"corrected_tau_delta": self.object.tau - self.camera_tilt,
733+
"rho_dot_o": self.object.rho_rate,
734+
"tau_dot_o": self.object.tau_rate,
735+
"rho_c": self.camera.rho,
736+
"tau_c": self.camera.tau,
737+
"rho_dot_c": self.rho_dot_c,
738+
"tau_dot_c": self.tau_dot_c,
739+
"rho_c_gain": self.rho_c_gain,
740+
"tau_c_gain": self.tau_c_gain,
741+
"rst_vel_o_0": self.object.rst_velocity_msg_relative_to_tripod[0],
742+
"rst_vel_o_1": self.object.rst_velocity_msg_relative_to_tripod[1],
743+
"rst_vel_o_2": self.object.rst_velocity_msg_relative_to_tripod[2],
744+
"rst_point_o_0": self.object.rst_point_msg_relative_to_tripod[0],
745+
"rst_point_o_1": self.object.rst_point_msg_relative_to_tripod[1],
746+
"rst_point_o_2": self.object.rst_point_msg_relative_to_tripod[2],
747+
"distance": self.object.distance_to_tripod3d,
748+
"focus": _focus,
749+
"zoom": _zoom,
750+
"object_location_update_period": self.object.location_update_period,
751+
"rho_derivative": self.object.rho_derivative,
752+
"tau_derivative": self.object.tau_derivative,
753+
"pan_rate_index": self.camera.pan_rate_index,
754+
"tilt_rate_index": self.camera.tilt_rate_index,
755+
"delta_rho_dot_c": self.delta_rho_dot_c,
756+
"delta_tau_dot_c": self.delta_tau_dot_c,
757+
"delta_rho": self.delta_rho,
758+
"delta_tau": self.delta_tau,
759+
"tracking_loop_time": elapsed_time,
760+
"time_since_last_update": time_since_last_update,
761+
"object_id": self.object.object_id,
762+
}
763+
}
764+
),
765+
)
766+
self.publish_to_topic(self.logger_topic, logger_msg)
767+
except Exception as e:
768+
logging.error(f"Error in tracking object: {e}")
769+
logging.error(traceback.format_exc())
763770

764771
def _slew_camera(self, rho_target: float, tau_target: float) -> None:
765772
if self.status == Status.SLEWING:
@@ -840,7 +847,9 @@ def _object_callback(
840847
"vertical_velocity",
841848
]
842849
) <= set(data.keys()):
843-
logging.info(f"Required keys missing from object message data: {data}")
850+
#logging.info(f"Required keys missing from object message data: {data}")
851+
self.do_capture = False
852+
self.status = Status.SLEEPING
844853
self.object = None
845854
return
846855
logging.info(

0 commit comments

Comments
 (0)