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 <[email protected]>

Author: srmainwaring

MAVProxy/modules/mavproxy_camtrack/__init__.py

@@ -59,25 +59,23 @@ 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
         self._do_request_autopilot_state_for_gimbal_device = True
         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,33 +127,33 @@ 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)
 
         # not working - limited implementation in AP_Mount
         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()

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