From 9119a92e009e6b8d5e58c7c34df7be32615cde78 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 8 Oct 2024 19:22:40 +0100 Subject: [PATCH] camtrack: handle camera tracking image status updates - tracker_image: override proces_event to handle update to tracked rectangle - onboard controller: image status sends current tracked rectangle - camera_view: update mavlink handlers - camera_view: remove unused code - camera_view: handle tracking image status - tracker_image: override proces_event to handle update to tracked rectangle (fix) - camera_view: request image status at 20Hz - camera_view: remove unused code - camera_view: remove unused code - camera_view: suppress debug prints - tracker_image: remove unused code - onboard_controller: remove unused code - onboard_controller: suppress debug prints - tracker_image: update docstring - tracker_image: add set_position method to Tracker base class - tracker_image: add set_position method to TrackerCSTR - tracker_image: set position of tracked rectangle - onboard_controller: clean up track request print statements - camtrack: onboard_controller: add timeout and restart Signed-off-by: Rhys Mainwaring --- .../modules/mavproxy_camtrack/__init__.py | 56 +++---- .../modules/mavproxy_camtrack/camera_view.py | 75 +++------- .../mavproxy_camtrack/onboard_controller.py | 141 ++++++++++-------- .../mavproxy_camtrack/tracker_image.py | 54 +++++-- 4 files changed, 165 insertions(+), 161 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/__init__.py b/MAVProxy/modules/mavproxy_camtrack/__init__.py index 3a18d02e80..3333bc488f 100644 --- a/MAVProxy/modules/mavproxy_camtrack/__init__.py +++ b/MAVProxy/modules/mavproxy_camtrack/__init__.py @@ -59,16 +59,16 @@ def __init__(self, mpstate): self.camera_view = CameraView(self.mpstate, "Camera Tracking", rtsp_url) - # TODO: NOTE: unused + # NOTE: unused except for status # mavlink messages self._last_gimbal_device_information = None self._last_gimbal_manager_status = None self._last_gimbal_device_information = None self._last_gimbal_device_attitude_status = None self._last_autopilot_state_for_gimbal_device = None - self._last_camera_tracking_image_status = None + self._last_camera_information = None - # Discovery + # discovery self._do_request_gimbal_manager_information = True self._do_request_gimbal_manager_status = True self._do_request_gimbal_device_information = True @@ -76,8 +76,6 @@ def __init__(self, mpstate): self._do_request_camera_information = True self._do_request_camera_tracking_image_status = True - # data - # control update rate to GUI self._msg_list = [] self._fps = 30.0 @@ -129,11 +127,11 @@ def mavlink_packet(self, msg): if mtype == "HEARTBEAT": self.handle_heartbeat(msg) - # working - must be requested + # must be requested elif mtype == "GIMBAL_MANAGER_INFORMATION": self.handle_gimbal_manager_information(msg) - # working - must be requested (should be broadcast) + # must be requested (should be broadcast) elif mtype == "GIMBAL_MANAGER_STATUS": self.handle_gimbal_manager_status(msg) @@ -141,21 +139,21 @@ def mavlink_packet(self, msg): elif mtype == "GIMBAL_DEVICE_INFORMATION": self.handle_gimbal_device_information(msg) - # working - boradcast + # broadcast elif mtype == "GIMBAL_DEVICE_ATTITUDE_STATUS": self.handle_gimbal_device_attitude_status(msg) - # working - must be requested + # must be requested elif mtype == "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE": self.handle_autopilot_state_for_gimbal_device(msg) - # working - must be requested + # must be requested elif mtype == "CAMERA_INFORMATION": self.handle_camera_information(msg) + # must be requested elif mtype == "CAMERA_TRACKING_IMAGE_STATUS": - # TODO: add handler - print(msg) + self.handle_camera_tracking_image_status(msg) # TODO: NOTE: disabled # check command_ack @@ -272,8 +270,18 @@ def handle_autopilot_state_for_gimbal_device(self, msg): self._last_autopilot_state_for_gimbal_device = msg def handle_camera_information(self, msg): - # print(msg) - pass + self._last_camera_information = msg + + def handle_camera_tracking_image_status(self, msg): + # TODO: refactor to use enums in onboard_controller.py + if ( + msg.tracking_status == mavutil.mavlink.CAMERA_TRACKING_STATUS_FLAGS_ACTIVE + and msg.tracking_mode == mavutil.mavlink.CAMERA_TRACKING_MODE_RECTANGLE + and msg.target_data == mavutil.mavlink.CAMERA_TRACKING_TARGET_DATA_IN_STATUS + ): + self.camera_view.set_tracked_rectangle( + msg.rec_top_x, msg.rec_top_y, msg.rec_bottom_x, msg.rec_bottom_y + ) def check_events(self): """Check for events on the camera view""" @@ -329,7 +337,7 @@ def send_messages(self): if self._do_request_camera_tracking_image_status: self.send_set_message_interval_message( mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, - 1000 * 1000, # 1Hz + 1000 * 50, # 20Hz response_target=1, # flight-stack default ) self._do_request_camera_tracking_image_status = False @@ -355,7 +363,6 @@ def send_gimbal_manager_configure(self): gimbal_devid, # param7 ) - # MAVProxy.modules.mavproxy_misc.py def send_request_message(self, message_id, p1=0): self.master.mav.command_long_send( self.settings.target_system, # target_system @@ -388,23 +395,6 @@ def send_set_message_interval_message( response_target, # param7 ) - def request_camera_information(self): - # send CAMERA_INFORMATION request - # mavutil.mavlink.MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION - pass - - def request_gimbal_manager_information(self): - pass - - def request_gimbal_manager_status(self): - pass - - def request_gimbal_device_information(self): - pass - - def request_autopilot_state_for_gimbal_device(self): - pass - def idle_task(self): """Idle tasks""" self.check_events() diff --git a/MAVProxy/modules/mavproxy_camtrack/camera_view.py b/MAVProxy/modules/mavproxy_camtrack/camera_view.py index cfb73052fd..fef377c977 100644 --- a/MAVProxy/modules/mavproxy_camtrack/camera_view.py +++ b/MAVProxy/modules/mavproxy_camtrack/camera_view.py @@ -69,6 +69,10 @@ def __init__(self, mpstate, title, rtsp_url, fps=30): ) self.im.set_gstreamer(gst_pipeline) + def set_tracked_rectangle(self, top_left_x, top_left_y, bot_right_x, bot_right_y): + """Set the tracked rectangle (normalised coords)""" + self.im.set_tracked_rectangle(top_left_x, top_left_y, bot_right_x, bot_right_y) + def close(self): """Close the GUI""" # TODO: MPImage does not have a close_event @@ -96,13 +100,6 @@ def check_events(self): if isinstance(event, MPImageFrameCounter): self.frame_counter = event.frame continue - - # if isinstance(event, MPImageTrackPoint): - # continue - - # if isinstance(event, MPImageTrackRectangle): - # continue - if ( hasattr(event, "ClassName") and event.ClassName == "wxMouseEvent" @@ -119,8 +116,8 @@ def check_events(self): self.im.start_tracker(event.X, event.Y, twidth, twidth) # TODO: move / encapsulate - print(f"xres: {xres}, yres: {yres}") - print(f"event.X: {event.X}, event.Y: {event.X}, twidth: {twidth}") + # print(f"xres: {xres}, yres: {yres}") + # print(f"event.X: {event.X}, event.Y: {event.X}, twidth: {twidth}") top_left_x = event.X / xres top_left_y = event.Y / yres bot_right_x = (event.X + twidth) / xres @@ -135,8 +132,6 @@ def check_events(self): else: pass - # Camera tracking commands. Communication is GCS -> FC - def send_camera_track_point(self, point_x, point_y, radius): """ https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_POINT @@ -147,11 +142,11 @@ def send_camera_track_point(self, point_x, point_y, radius): src_cmp = self.mpstate.master().source_component tgt_sys = self.camera_sysid tgt_cmp = self.camera_cmpid - print( - f"Send COMMAND_LONG: CAMERA_TRACK_POINT: " - f"src_sys: {src_sys}, src_cmp: {src_cmp} " - f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}" - ) + # print( + # f"Send COMMAND_LONG: CAMERA_TRACK_POINT: " + # f"src_sys: {src_sys}, src_cmp: {src_cmp} " + # f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}" + # ) target_camera = 0 self.mpstate.master().mav.command_long_send( tgt_sys, # target_system @@ -179,11 +174,11 @@ def send_camera_track_rectangle( src_cmp = self.mpstate.master().source_component tgt_sys = self.camera_sysid tgt_cmp = self.camera_cmpid - print( - f"Send COMMAND_LONG: CAMERA_TRACK_RECTANGLE: " - f"src_sys: {src_sys}, src_cmp: {src_cmp} " - f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}" - ) + # print( + # f"Send COMMAND_LONG: CAMERA_TRACK_RECTANGLE: " + # f"src_sys: {src_sys}, src_cmp: {src_cmp} " + # f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}" + # ) target_camera = 0 self.mpstate.master().mav.command_long_send( tgt_sys, # target_system @@ -207,11 +202,11 @@ def send_camera_stop_tracking(self): src_cmp = self.mpstate.master().source_component tgt_sys = self.camera_sysid tgt_cmp = self.camera_cmpid - print( - f"Send COMMAND_LONG: CAMERA_STOP_TRACKING: " - f"src_sys: {src_sys}, src_cmp: {src_cmp} " - f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}" - ) + # print( + # f"Send COMMAND_LONG: CAMERA_STOP_TRACKING: " + # f"src_sys: {src_sys}, src_cmp: {src_cmp} " + # f"tgt_sys: {tgt_sys}, tgt_cmp: {tgt_cmp}" + # ) target_camera = 0 self.mpstate.master().mav.command_long_send( tgt_sys, # target_system @@ -227,34 +222,6 @@ def send_camera_stop_tracking(self): 0, # param7 ) - def set_message_interval_image_status(self): - """ - https://mavlink.io/en/messages/common.html#CAMERA_TRACKING_IMAGE_STATUS - https://mavlink.io/en/messages/common.html#MAV_CMD_SET_MESSAGE_INTERVAL - """ - tgt_sys = self.camera_sysid - tgt_comp = self.camera_cmpid - print( - f"Send COMMAND_LONG: SET_MESSAGE_INTERVAL: CAMERA_TRACKING_IMAGE_STATUS: " - f"tgt_sys: {tgt_sys}, tgt_comp: {tgt_comp}" - ) - message_id = mavutil.mavlink.CAMERA_TRACKING_IMAGE_STATUS - interval = 0 # default rate - response_target = 1 # address of requestor - self.mpstate.master().mav.command_long_send( - tgt_sys, # target_system - tgt_comp, # target_component - mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # command - 0, # confirmation - message_id, # param1 - interval, # param2 - 0, # param3 - 0, # param4 - 0, # param5 - 0, # param6 - response_target, # param7 - ) - if __name__ == "__main__": from optparse import OptionParser diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 548f208748..7f1829a658 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -188,14 +188,7 @@ def _mavlink_recv_task(self): def __process_message(): msg = self._connection.recv_match(blocking=True) # Apply filters - if ( - msg is None - or msg.get_type() == "BAD_DATA" - # or not hasattr(msg, "target_system") - # or not hasattr(msg, "target_component") - # or msg.target_system != sysid - # or msg.target_component != cmpid - ): + if msg is None or msg.get_type() == "BAD_DATA": return for controller in self._controllers: @@ -221,10 +214,15 @@ def run(self): # Connect to video stream video_stream = VideoStream(self._rtsp_url) - # TODO: add retry limit and timeout + # Timeout if video not available + video_last_update_time = time.time() + video_timeout_period = 5.0 print("Waiting for video stream") while not video_stream.frame_available(): print(".", end="") + if (time.time() - video_last_update_time) > video_timeout_period: + print("\nVideo stream not available - restarting") + return time.sleep(0.1) print("\nVideo stream available") @@ -249,12 +247,12 @@ def run(self): tracking_rect = None tracking_rect_changed = True - # TODO: ensure consistency of frame updates with GCS. + # TODO: how to ensure consistency of frame updates with GCS? fps = 50 update_rate = fps update_period = 1.0 / update_rate + frame_count = 0 - av_update_time = 0.0 while True: start_time = time.time() @@ -269,7 +267,6 @@ def run(self): self._gimbal_controller.reset() elif track_type is CameraTrackType.RECTANGLE: - # TODO: not handling when the tracking rectange changes if tracking_rect is not None: def __compare_rect(rect1, rect2): @@ -304,22 +301,27 @@ def __compare_rect(rect1, rect2): u = x + w // 2 v = y + h // 2 self._gimbal_controller.update_center(u, v, frame.shape) + + # update camera controller current tracking state + frame_height, frame_width, _ = frame.shape + top_left_x = x / frame_width + top_left_y = y / frame_height + bot_right_x = (x + w) / frame_width + bot_right_y = (y + h) / frame_height + self._camera_controller.set_curr_track_rectangle( + CameraTrackRectangle( + top_left_x, top_left_y, bot_right_x, bot_right_y + ) + ) else: print("Tracking failure detected.") tracking_rect = None self._gimbal_controller.reset() - # TODO: profiling stats - # if frame_count % 10 == 0: - # av_update_time = av_update_time / 10.0 * 1000 - # print(f"gimbal controller update: {av_update_time:.0f} ms") - # av_update_time = 0.0 - # Rate limit elapsed_time = time.time() - start_time sleep_time = max(0.0, update_period - elapsed_time) time.sleep(sleep_time) - av_update_time += elapsed_time class VideoStream: @@ -507,12 +509,16 @@ def __init__(self, connection): self._resolution_v = 0 self._gimbal_device_id = 1 - # tracking details + # Tracking details - tracking requested self._lock = threading.Lock() self._track_type = CameraTrackType.NONE self._track_point = None self._track_rect = None + # Current track state + self._curr_track_point = None + self._curr_track_rect = None + # Start the mavlink thread self._mavlink_in_queue = queue.Queue() self._mavlink_out_queue = queue.Queue() @@ -549,6 +555,20 @@ def track_rectangle(self): track_rect = copy.deepcopy(self._track_rect) return track_rect + def set_curr_track_point(self, point): + """ + Set the currently tracked point. + """ + with self._lock: + self._curr_track_point = copy.deepcopy(point) + + def set_curr_track_rectangle(self, rect): + """ + Set the currently tracked rectangle. + """ + with self._lock: + self._curr_track_rect = copy.deepcopy(rect) + def mavlink_packet(self, msg): """ Process a mavlink packet. @@ -582,8 +602,6 @@ def to_uint8_t(string): string_encode = string.encode("utf-8") return string_encode + b"\0" * (32 - len(string)) - # print(to_uint8_t(self.vendor_name), len(to_uint8_t(self.vendor_name))) - self._connection.mav.camera_information_send( int(time.time() * 1000) & 0xFFFFFFFF, # time_boot_ms to_uint8_t(self._vendor_name), # vendor_name @@ -605,43 +623,41 @@ def to_uint8_t(string): def _send_camera_tracking_image_status(self): with self._lock: track_type = self._track_type - track_point = self._track_point - track_rect = self._track_rect - - # TODO: use trackers bounding box instead of initial target + track_point = self._curr_track_point + track_rect = self._curr_track_rect + + # Set default values + point_x = float("nan") + point_y = float("nan") + radius = float("nan") + rec_top_x = float("nan") + rec_top_y = float("nan") + rec_bottom_x = float("nan") + rec_bottom_y = float("nan") + + # TODO: track_point and track_rect should not be None + # if track_type is not NONE - poss sync issue? if track_type is CameraTrackType.NONE: tracking_status = CameraTrackingStatusFlags.IDLE.value tracking_mode = CameraTrackingMode.NONE.value target_data = CameraTrackingTargetData.NONE.value - point_x = float("nan") - point_y = float("nan") - radius = float("nan") - rec_top_x = float("nan") - rec_top_y = float("nan") - rec_bottom_x = float("nan") - rec_bottom_y = float("nan") - elif track_type is CameraTrackType.POINT: + elif track_type is CameraTrackType.POINT and track_point is not None: tracking_status = CameraTrackingStatusFlags.ACTIVE.value tracking_mode = CameraTrackingMode.POINT.value target_data = CameraTrackingTargetData.IN_STATUS.value point_x = track_point.point_x point_y = track_point.point_y radius = track_point.radius - rec_top_x = float("nan") - rec_top_y = float("nan") - rec_bottom_x = float("nan") - rec_bottom_y = float("nan") - elif track_type is CameraTrackType.RECTANGLE: + elif track_type is CameraTrackType.RECTANGLE and track_rect is not None: tracking_status = CameraTrackingStatusFlags.ACTIVE.value tracking_mode = CameraTrackingMode.RECTANGLE.value target_data = CameraTrackingTargetData.IN_STATUS.value - point_x = float("nan") - point_y = float("nan") - radius = float("nan") rec_top_x = track_rect.top_left_x rec_top_y = track_rect.top_left_y rec_bottom_x = track_rect.bot_right_x rec_bottom_y = track_rect.bot_right_y + else: + return msg = self._connection.mav.camera_tracking_image_status_encode( tracking_status, @@ -658,26 +674,27 @@ def _send_camera_tracking_image_status(self): self._connection.mav.send(msg) def _handle_camera_track_point(self, msg): - print("Got COMMAND_LONG: CAMERA_TRACK_POINT") # Parameters are a normalised point. point_x = msg.param1 point_y = msg.param2 radius = msg.param3 - print(f"Track point: x: {point_x}, y: {point_y}, radius: {radius}") + print( + f"CAMERA_TRACK_POINT: x: {point_x:.3f}, y: {point_y:.3f}, " + f"radius: {radius:.3f}" + ) with self._lock: self._track_type = CameraTrackType.POINT self._track_point = CameraTrackPoint(point_x, point_y, radius) def _handle_camera_track_rectangle(self, msg): - print("Got COMMAND_LONG: CAMERA_TRACK_RECTANGLE") # Parameters are a normalised rectangle. top_left_x = msg.param1 top_left_y = msg.param2 bot_right_x = msg.param3 bot_right_y = msg.param4 print( - f"Track rectangle: x1: {top_left_x}, y1: {top_left_y}, " - f"x2: {bot_right_x}, y2: {bot_right_y}" + f"CAMERA_TRACK_RECTANGLE: x1: {top_left_x:.3f}, y1: {top_left_y:.3f}, " + f"x2: {bot_right_x:.3f}, y2: {bot_right_y:.3f}" ) with self._lock: self._track_type = CameraTrackType.RECTANGLE @@ -686,11 +703,13 @@ def _handle_camera_track_rectangle(self, msg): ) def _handle_camera_stop_tracking(self, msg): - print("Got COMMAND_LONG: CAMERA_STOP_TRACKING") + print("CAMERA_STOP_TRACKING") with self._lock: self._track_type = CameraTrackType.NONE self._track_point = None self._track_rect = None + self._curr_track_point = None + self._curr_track_rect = None def _mavlink_task(self): """ @@ -774,7 +793,7 @@ def __init__(self, connection): self._height = None self._tracking = False - # gimbal state + # Gimbal state # TODO: obtain the mount neutral angles from params # NOTE: q = [w, x, y, z] self._mavlink_lock = threading.Lock() @@ -808,7 +827,6 @@ def update_center(self, x, y, shape): self._center_x = x self._center_y = y self._height, self._width, _ = shape - # print(f"width: {self._width}, height: {self._height}, center: [{x}, {y}]") def reset(self): with self._control_lock: @@ -897,11 +915,6 @@ def _control_task(self): err_yaw = math.radians(err_x) yaw_rate_rads = self._yaw_controller.run(err_yaw) - # print( - # f"err_x: {err_x}, err_y: {err_y}, " - # f"pitch_rate: {pitch_rate_rads}, yaw_rate: {yaw_rate_rads}" - # ) - self._send_gimbal_manager_pitch_yaw_angles( float("nan"), float("nan"), @@ -1002,7 +1015,7 @@ def update(self, frame): if self._nroi_changed: self._tracker = cv2.legacy.TrackerCSRT_create() - # denormalise the roi + # Denormalise the roi height, width, _ = frame.shape roi = [ int(self._nroi[0] * width), # x @@ -1010,9 +1023,9 @@ def update(self, frame): int(self._nroi[2] * width), # w int(self._nroi[3] * height), # h ] - print( - f"TrackerCSTR: nroi: {self._nroi}, roi: {roi}, frame_width: {width}, frame_height: {height}" - ) + # print( + # f"TrackerCSTR: nroi: {self._nroi}, roi: {roi}, frame_width: {width}, frame_height: {height}" + # ) self._tracker.init(frame, roi) self._nroi_changed = False @@ -1029,6 +1042,8 @@ def set_normalised_roi(self, nroi): if __name__ == "__main__": + import os + import sys from optparse import OptionParser parser = OptionParser("onboard_controller.py [options]") @@ -1053,4 +1068,10 @@ def set_normalised_roi(self, nroi): controller = OnboardController( opts.master, opts.sysid, opts.cmpid, opts.rtsp_server ) - controller.run() + + while True: + try: + controller.run() + except KeyboardInterrupt: + EXIT_CODE_CTRL_C = 130 + sys.exit(EXIT_CODE_CTRL_C) diff --git a/MAVProxy/modules/mavproxy_camtrack/tracker_image.py b/MAVProxy/modules/mavproxy_camtrack/tracker_image.py index 6b5a204d48..e78e7de2e1 100644 --- a/MAVProxy/modules/mavproxy_camtrack/tracker_image.py +++ b/MAVProxy/modules/mavproxy_camtrack/tracker_image.py @@ -18,6 +18,15 @@ from MAVProxy.modules.lib.mp_image import MPImagePanel +# Events +class MPImageTrackedRectangle: + def __init__(self, top_left_x, top_left_y, bot_right_x, bot_right_y): + self.top_left_x = top_left_x + self.top_left_y = top_left_y + self.bot_right_x = bot_right_x + self.bot_right_y = bot_right_y + + class TrackerImage(MPImage): """An MPImage class that allows the tracker type to be overridden""" @@ -64,6 +73,12 @@ def child_task(self): self.app.frame.Show() self.app.MainLoop() + def set_tracked_rectangle(self, top_left_x, top_left_y, bot_right_x, bot_right_y): + """Set the tracked rectangle (normalised coords)""" + self.in_queue.put( + MPImageTrackedRectangle(top_left_x, top_left_y, bot_right_x, bot_right_y) + ) + class TrackerImageFrame(wx.Frame): """Main frame for an image with object tracking""" @@ -97,7 +112,7 @@ def __init__(self, parent, state): super(TrackerImagePanel, self).__init__(parent, state) def start_tracker(self, box): - """Start a tracker on an object identified by a box""" + """Start a tracker on an object identified by a box (override)""" if self.raw_img is None: return self.tracker = None @@ -109,6 +124,23 @@ def start_tracker(self, box): tracker.start_track(self.raw_img, box) self.tracker = tracker + def process_event(self, obj): + """Process a single event (override)""" + super().process_event(obj) + + # handle events defined for this class + if isinstance(obj, MPImageTrackedRectangle): + if self.tracker is not None: + height, width, _ = self.raw_img.shape + self.tracker.set_position( + TrackerPos( + obj.top_left_x * width, + obj.bot_right_x * width, + obj.top_left_x * height, + obj.bot_right_y * height, + ) + ) + class TrackerPos: """Rectangle output by a tracker [(left, top), (right, bottom)]""" @@ -154,6 +186,10 @@ def get_position(self): """Return a rectangle at the position of the tracked object""" return TrackerPos(0, 0, 0, 0) + def set_position(self, tracker_pos): + """Set the tracker position""" + pass + class TrackerDlibCorrelation(Tracker): """Wrapper for the dlib correlation tracker""" @@ -235,25 +271,15 @@ def start_track(self, raw_image, box): bottom_right_y = (y + box.height) / h self._tracker_pos = TrackerPos(x, x + w, y, y + h) - # print("Starting MAVLink tracker") - # self.event_queue.put( - # TrackerImageEvent( - # TrackerImageEventType.TRACK_RECTANGLE, - # track_rectangle=[ - # top_left_x, - # top_left_y, - # bottom_right_x, - # bottom_right_y, - # ], - # ) - # ) - def update(self, frame): pass def get_position(self): return self._tracker_pos + def set_position(self, tracker_pos): + self._tracker_pos = tracker_pos + def create_tracker(name, state): """