From 8c007d151bc2b657fef70eefa364b356fc319500 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sun, 29 Sep 2024 11:44:55 +0100 Subject: [PATCH 01/28] mp_image: support macOS Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/lib/mp_image.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/MAVProxy/modules/lib/mp_image.py b/MAVProxy/modules/lib/mp_image.py index 3511177243..99fda972f9 100755 --- a/MAVProxy/modules/lib/mp_image.py +++ b/MAVProxy/modules/lib/mp_image.py @@ -9,6 +9,7 @@ ''' import time +from MAVProxy.modules.lib import wx_processguard from MAVProxy.modules.lib.wx_loader import wx import cv2 import numpy as np @@ -953,11 +954,17 @@ def full_size(self): continue if isinstance(event, MPImageTrackPos): continue - if event.ClassName == 'wxMouseEvent': + if ( + hasattr(event, "ClassName") + and event.ClassName == 'wxMouseEvent' + ): if event.leftIsDown and event.shiftDown: im.start_tracker(event.X, event.Y, 50, 50) if event.leftIsDown and event.controlDown: im.end_tracking() - if event.ClassName == 'wxKeyEvent': + if ( + hasattr(event, "ClassName") + and event.ClassName == 'wxKeyEvent' + ): print('key %u' % event.KeyCode) time.sleep(0.1) From e40835e304a2ed858837c231022e5744ffa3baf5 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sun, 29 Sep 2024 23:05:05 +0100 Subject: [PATCH 02/28] mp_image: add check that tracker is not None Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/lib/mp_image.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/MAVProxy/modules/lib/mp_image.py b/MAVProxy/modules/lib/mp_image.py index 99fda972f9..cafd68f303 100755 --- a/MAVProxy/modules/lib/mp_image.py +++ b/MAVProxy/modules/lib/mp_image.py @@ -697,8 +697,11 @@ def video_thread(self, url, cap_options): frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) (width, height) = (frame.shape[1], frame.shape[0]) - if self.tracker: + if self.tracker is not None: self.tracker.update(frame) + # TODO: may need a lock? tracker may be set to None after update() + # but before get_position(). + if self.tracker is not None: pos = self.tracker.get_position() if pos is not None: startX = int(pos.left()) From 0fc69b1853b4caa7931def9337018372eada6d64 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sun, 29 Sep 2024 11:45:15 +0100 Subject: [PATCH 03/28] mavproxy_SIYI: support macOS Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/mavproxy_SIYI/camera_view.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/MAVProxy/modules/mavproxy_SIYI/camera_view.py b/MAVProxy/modules/mavproxy_SIYI/camera_view.py index 8667a0d9de..54880ed3f6 100644 --- a/MAVProxy/modules/mavproxy_SIYI/camera_view.py +++ b/MAVProxy/modules/mavproxy_SIYI/camera_view.py @@ -241,12 +241,16 @@ def check_events(self): self.siyi.log_frame_counter(self.video_idx, self.thermal, self.frame_counter) continue - if event.ClassName == "wxMouseEvent": + if ( + hasattr(event, "ClassName") + and event.ClassName == "wxMouseEvent" + ): if event.pixel is not None: self.siyi.spot_temp = self.get_pixel_temp(event) self.update_title() if ( - event.ClassName == "wxMouseEvent" + hasattr(event, "ClassName") + and event.ClassName == "wxMouseEvent" and event.leftIsDown and event.pixel is not None and self.siyi is not None From 60ef2558278acb7114bd305ac83f4d4674bfcc11 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Fri, 27 Sep 2024 21:37:33 +0100 Subject: [PATCH 04/28] camtrack: camera tracking module - Initial structure for camera tracking module - Add initial structure for module - Use MPImage to render RTSP stream - Subclass MPImage to allow tracker overrides - Abstract an interface for the tracker and provide dlib example - Enable title - Add cv2 CSRT tracker - Document TODOs - Rename image to tracker_image - Add tracker to forward mavlink commands - Add dev notes - Add requests for camera and gimbal info - Preparation for sending mavlink camera track commands - Add event class and enums - Remove event class and simplify - Note: mp_image has a different event model to param_edit etc. - Add mavlink cmds to start / stop tracking - Capture command_acks - Add variables for camera and gimbal sysid and compid - Add initial onboard controller script - Remove print of command_long - Add handlers to onboard controller - Format ack messages - Format debug messages - Onboard controller: send camera info - Onboard controller: use enum for flags - Add onboard controller and gimbal controller classes - Fix missed lock -> _lock rename - camera_view: remove hardcoded tracking coords - onboard controller: add video capture and tracker - camera_view: disable repeated discovery requests - onboard controller: update controller gains - camera_view: add example RTSP URLs - camera_view: suppress mtype checks - onboard controller: thread safe handling of mavlink messages - camera_view: fix handling of image resolution when setting track rectangle - onboard controller: suppress profiling output - camera_view: additional mavlink debug info - onboard controller: additional mavlink debug info - onboard controller: add command line options - camera_view: add command line options - onboard controller: add usage docs - onboard controller: initial support to send tracking image status - camera_view: enable one time discovery requests - camera_view: use MAV_CMD_SET_MESSAGE_INTERVAL to request image status - onboard controller: populate image status using initial target - camera_view: tidy module docstring Signed-off-by: Rhys Mainwaring --- .../modules/mavproxy_camtrack/__init__.py | 423 +++++++ .../modules/mavproxy_camtrack/camera_view.py | 307 +++++ .../mavproxy_camtrack/onboard_controller.py | 1056 +++++++++++++++++ .../mavproxy_camtrack/tracker_image.py | 269 +++++ 4 files changed, 2055 insertions(+) create mode 100644 MAVProxy/modules/mavproxy_camtrack/__init__.py create mode 100644 MAVProxy/modules/mavproxy_camtrack/camera_view.py create mode 100644 MAVProxy/modules/mavproxy_camtrack/onboard_controller.py create mode 100644 MAVProxy/modules/mavproxy_camtrack/tracker_image.py diff --git a/MAVProxy/modules/mavproxy_camtrack/__init__.py b/MAVProxy/modules/mavproxy_camtrack/__init__.py new file mode 100644 index 0000000000..3a18d02e80 --- /dev/null +++ b/MAVProxy/modules/mavproxy_camtrack/__init__.py @@ -0,0 +1,423 @@ +""" +MAVProxy camera tracking module + +Reference +--------- + +MAVLink documentation + +- https://mavlink.io/en/services/gimbal_v2.html +- https://mavlink.io/en/services/camera.html#camera-protocol-v2 + + +ArduPilot MAVLink handlers + +- https://github.com/ArduPilot/ardupilot/blob/master/libraries/GCS_MAVLink/GCS_Common.cpp + + +pymavlink + +- pymavlink.dialects.v20.ardupilotmega.MAVLink +- MAVLink.gimbal_* +- MAVLink.camera_* +""" + +import time + +from MAVProxy.modules.lib import mp_module +from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import mp_settings + +from MAVProxy.modules.mavproxy_camtrack.camera_view import CameraView + +from pymavlink import mavutil + + +class CamTrackModule(mp_module.MPModule): + """A tool to control camera tracking""" + + def __init__(self, mpstate): + super(CamTrackModule, self).__init__( + mpstate, "camtrack", "camera tracking module" + ) + + self.mpstate = mpstate + + # GUI + # TODO: provide args to set RTSP server location + # localhost simulation + rtsp_url = "rtsp://127.0.0.1:8554/camera" + + # home wifi + # rtsp_url = "rtsp://192.168.1.204:8554/fpv_stream" + + # herelink wifi access point + # rtsp_url = "rtsp://192.168.43.1:8554/fpv_stream" + + # SIYI A8 camera + # rtsp_url = "rtsp://192.168.144.25:8554/main.264" + + self.camera_view = CameraView(self.mpstate, "Camera Tracking", rtsp_url) + + # TODO: NOTE: unused + # 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 + + # 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 + self._last_send = 0.0 + self._send_delay = (1.0 / self._fps) * 0.9 + + # commands + self.add_command("camtrack", self.cmd_camtrack, "camera tracking") + + def cmd_camtrack(self, args): + """Control behaviour of commands""" + if len(args) <= 0: + print(self.usage()) + return + + if args[0] == "status": + print(self.status()) + return + + if args[0] == "start": + print("start tracking") + return + + if args[0] == "stop": + print("stop tracking") + return + + print(self.usage()) + + def usage(self): + """Show help on command line options.""" + return "Usage: camtrack " + + def status(self): + """Return information about the camera tracking state""" + return [ + str(self._last_gimbal_manager_information), + str(self._last_gimbal_manager_status), + str(self._last_gimbal_device_information), + str(self._last_gimbal_device_attitude_status), + str(self._last_autopilot_state_for_gimbal_device), + ] + + def mavlink_packet(self, msg): + """Handle mavlink packets.""" + mtype = msg.get_type() + + # heartbeat + if mtype == "HEARTBEAT": + self.handle_heartbeat(msg) + + # working - must be requested + elif mtype == "GIMBAL_MANAGER_INFORMATION": + self.handle_gimbal_manager_information(msg) + + # working - 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 + elif mtype == "GIMBAL_DEVICE_ATTITUDE_STATUS": + self.handle_gimbal_device_attitude_status(msg) + + # working - must be requested + elif mtype == "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE": + self.handle_autopilot_state_for_gimbal_device(msg) + + # working - must be requested + elif mtype == "CAMERA_INFORMATION": + self.handle_camera_information(msg) + + elif mtype == "CAMERA_TRACKING_IMAGE_STATUS": + # TODO: add handler + print(msg) + + # TODO: NOTE: disabled + # check command_ack + elif False: # mtype == "COMMAND_ACK": + if msg.command == mavutil.mavlink.MAV_CMD_CAMERA_TRACK_POINT: + if msg.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: + print("Got COMMAND_ACK: CAMERA_TRACK_POINT: ACCEPTED") + elif msg.result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: + print("Got COMMAND_ACK: CAMERA_TRACK_POINT: REJECTED") + elif msg.result == mavutil.mavlink.MAV_RESULT_DENIED: + print("Got COMMAND_ACK: CAMERA_TRACK_POINT: DENIED") + elif msg.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: + print("Got COMMAND_ACK: CAMERA_TRACK_POINT: UNSUPPORTED") + elif msg.result == mavutil.mavlink.MAV_RESULT_FAILED: + print("Got COMMAND_ACK: CAMERA_TRACK_POINT: FAILED") + else: + print( + "Got COMMAND_ACK: CAMERA_TRACK_POINT: result: {}".format( + msg.result + ) + ) + + elif msg.command == mavutil.mavlink.MAV_CMD_CAMERA_TRACK_RECTANGLE: + if msg.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: + print("Got COMMAND_ACK: CAMERA_TRACK_RECTANGLE: ACCEPTED") + elif msg.result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: + print("Got COMMAND_ACK: CAMERA_TRACK_RECTANGLE: REJECTED") + elif msg.result == mavutil.mavlink.MAV_RESULT_DENIED: + print("Got COMMAND_ACK: CAMERA_TRACK_RECTANGLE: DENIED") + elif msg.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: + print("Got COMMAND_ACK: CAMERA_TRACK_RECTANGLE: UNSUPPORTED") + elif msg.result == mavutil.mavlink.MAV_RESULT_FAILED: + print("Got COMMAND_ACK: CAMERA_TRACK_RECTANGLE: FAILED") + else: + print( + "Got COMMAND_ACK: CAMERA_TRACK_RECTANGLE: result: {}".format( + msg.result + ) + ) + + elif msg.command == mavutil.mavlink.MAV_CMD_CAMERA_STOP_TRACKING: + if msg.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: + print("Got COMMAND_ACK: CAMERA_STOP_TRACKING: ACCEPTED") + elif msg.result == mavutil.mavlink.MAV_RESULT_TEMPORARILY_REJECTED: + print("Got COMMAND_ACK: CAMERA_STOP_TRACKING:tracking REJECTED") + elif msg.result == mavutil.mavlink.MAV_RESULT_DENIED: + print("Got COMMAND_ACK: CAMERA_STOP_TRACKING: DENIED") + elif msg.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: + print("Got COMMAND_ACK: CAMERA_STOP_TRACKING: UNSUPPORTED") + elif msg.result == mavutil.mavlink.MAV_RESULT_FAILED: + print("Got COMMAND_ACK: CAMERA_STOP_TRACKING: FAILED") + else: + print( + "Got COMMAND_ACK: CAMERA_STOP_TRACKING: RESULT: {}".format( + msg.result + ) + ) + + elif msg.command == mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL: + print("Got COMMAND_ACK: MAV_CMD_SET_MESSAGE_INTERVAL") + + # TODO: NOTE: disabled + # check command_long + elif False: # mtype == "COMMAND_LONG": + # TODO: check target_system is for offboard control + if msg.target_system != self.master.source_system: + pass + elif msg.command == mavutil.mavlink.MAV_CMD_CAMERA_TRACK_POINT: + print(msg) + elif msg.command == mavutil.mavlink.MAV_CMD_CAMERA_TRACK_RECTANGLE: + print(msg) + elif msg.command == mavutil.mavlink.MAV_CMD_CAMERA_STOP_TRACKING: + print(msg) + + def handle_heartbeat(self, msg): + sysid = msg.get_srcSystem() + compid = msg.get_srcComponent() + + # is this from an autopilot + if msg.autopilot != mavutil.mavlink.MAV_AUTOPILOT_INVALID: + # print(f"HB: AUTOPILOT: sysid: {sysid}, compid: {compid}") + # print(mavutil.mavlink.MAV_COMP_ID_AUTOPILOT1) + pass + + # What type of component? + if msg.type == mavutil.mavlink.MAV_TYPE_GENERIC: + print("MAV_TYPE_GENERIC") + # elif msg.type == mavutil.mavlink.MAV_TYPE_GCS: + # print("MAV_TYPE_GCS") + # elif msg.type == mavutil.mavlink.MAV_TYPE_FIXED_WING: + # print("MAV_TYPE_FIXED_WING") + # elif msg.type == mavutil.mavlink.MAV_TYPE_QUADROTOR: + # print("MAV_TYPE_QUADROTOR") + elif msg.type == mavutil.mavlink.MAV_TYPE_GIMBAL: + print("MAV_TYPE_GIMBAL") + # handle mavlink gimbal component + elif msg.type == mavutil.mavlink.MAV_TYPE_CAMERA: + print("MAV_TYPE_CAMERA") + # handle mavlink camera component + + def handle_gimbal_manager_information(self, msg): + self._last_gimbal_manager_information = msg + + def handle_gimbal_manager_status(self, msg): + self._last_gimbal_manager_status = msg + + def handle_gimbal_device_information(self, msg): + self._last_gimbal_device_information = msg + + def handle_gimbal_device_attitude_status(self, msg): + self._last_gimbal_device_attitude_status = msg + + 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 + + def check_events(self): + """Check for events on the camera view""" + self.camera_view.check_events() + + # TODO: check which shutdown events are available in MPImage + # tell mavproxy to unload the module if the GUI is closed + # if self.camera_view.close_event.wait(timeout=0.001): + # self.needs_unloading = True + + def send_messages(self): + """Send message list via pipe to GUI at desired update rate""" + if (time.time() - self._last_send) > self._send_delay: + # pipe data to GUI + # TODO: check interface in view for pipe updates + # self.camera_view.parent_pipe_send.send(self._msg_list) + # reset counters etc + self._msg_list = [] + self._last_send = time.time() + + # TODO: implement camera and gimbal discovery correctly + # Discovery - most of these requests are handled in the FC + # by GCS_MAVLINK::try_send_message + if self._do_request_gimbal_manager_information: + self.send_request_message( + mavutil.mavlink.MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION + ) + self._do_request_gimbal_manager_information = False + + if self._do_request_gimbal_manager_status: + self.send_request_message( + mavutil.mavlink.MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS + ) + self._do_request_gimbal_manager_status = False + + # NOTE: only AP_Mount_Gremsy implements handle_gimbal_device_information + if self._do_request_gimbal_device_information: + self.send_request_message( + mavutil.mavlink.MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION + ) + self._do_request_gimbal_device_information = False + + if self._do_request_autopilot_state_for_gimbal_device: + self.send_request_message( + mavutil.mavlink.MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE + ) + self._do_request_autopilot_state_for_gimbal_device = False + + if self._do_request_camera_information: + self.send_request_message(mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_INFORMATION) + self._do_request_camera_information = False + + 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 + response_target=1, # flight-stack default + ) + self._do_request_camera_tracking_image_status = False + + def send_gimbal_manager_configure(self): + # Acquire and release control + primary_sysid = -1 + primary_compid = -1 + secondary_sysid = -1 + secondary_compid = -1 + gimbal_devid = 0 + self.master.mav.command_long_send( + self.target_system, # target_system + self.target_component, + mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, # command + 0, # confirmation + primary_sysid, # param1 + primary_compid, # param2 + secondary_sysid, # param3 + secondary_compid, # param4 + 0, # param5 + 0, # param6 + 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 + self.settings.target_component, # target_component + mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, # command + 0, # confirmation + message_id, # param1 + 0, # param2 + 0, # param3 + 0, # param4 + 0, # param5 + 0, # param6 + 0, # param7 + ) + + def send_set_message_interval_message( + self, message_id, interval_us, response_target=1 + ): + self.master.mav.command_long_send( + self.settings.target_system, # target_system + self.settings.target_component, # target_component + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # command + 0, # confirmation + message_id, # param1 + interval_us, # param2 + 0, # param3 + 0, # param4 + 0, # param5 + 0, # param6 + 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() + self.send_messages() + + def unload(self): + """Close the GUI and unload module""" + + # close the GUI + self.camera_view.close() + + +def init(mpstate): + """Initialise module""" + + return CamTrackModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_camtrack/camera_view.py b/MAVProxy/modules/mavproxy_camtrack/camera_view.py new file mode 100644 index 0000000000..cfb73052fd --- /dev/null +++ b/MAVProxy/modules/mavproxy_camtrack/camera_view.py @@ -0,0 +1,307 @@ +""" +MAVProxy camera view +""" + +import sys +import time + +from MAVProxy.modules.lib import mp_module +from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import mp_settings + +from MAVProxy.modules.lib.mp_image import MPImage +from MAVProxy.modules.lib.mp_menu import MPMenuItem +from MAVProxy.modules.lib.mp_image import MPImageTrackPos +from MAVProxy.modules.lib.mp_image import MPImageFrameCounter + +from MAVProxy.modules.mavproxy_camtrack.tracker_image import TrackerImage + +from pymavlink import mavutil + + +class CameraView: + """Handle a camera view""" + + def __init__(self, mpstate, title, rtsp_url, fps=30): + self.mpstate = mpstate + self.rtsp_url = rtsp_url + + # TODO: remove hardcoded display size + display_width = 640 + display_height = 480 + + self.frame_counter = -1 + + # TODO: gimbal and camera system ids + + # autopilot component may proxy up to 6 cameras + self.camera_sysid = 1 + self.camera_cmpid = mavutil.mavlink.MAV_COMP_ID_ONBOARD_COMPUTER + self.camera_deviceid = 1 # first autopilot attached camera + + self.gimbal_sysid = 1 + self.gimbal_cmpid = mavutil.mavlink.MAV_COMP_ID_ONBOARD_COMPUTER + self.gimbal_deviceid = 1 # first autopilot attached gimbal + + self.im = TrackerImage( + title=title, + mouse_events=True, + mouse_movement_events=False, + width=display_width, + height=display_height, + key_events=True, + can_drag=False, + can_zoom=False, + auto_size=False, + auto_fit=True, + fps=fps, + ) + + # Capture video + # TODO: provide args to set gstreamer pipeline + gst_pipeline = ( + f"rtspsrc location={self.rtsp_url} latency=50 " + "! decodebin " + "! videoconvert " + "! video/x-raw,format=(string)BGR " + "! videoconvert " + "! appsink emit-signals=true sync=false max-buffers=2 drop=true" + ) + self.im.set_gstreamer(gst_pipeline) + + def close(self): + """Close the GUI""" + # TODO: MPImage does not have a close_event + # trigger a close event which is monitored by the + # child gui process - it will close allowing the + # process to be joined + # self.im.close_event.set() + # if self.im.is_alive(): + # self.im.child.join(timeout=2.0) + self.im.terminate() + + def is_alive(self): + """Check if the GUI process is alive""" + return self.im.is_alive() + + def check_events(self): + """Check for events""" + if self.im is None: + return + if not self.is_alive(): + return + for event in self.im.events(): + if isinstance(event, MPImageTrackPos): + continue + 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" + and event.leftIsDown + ): + track_size_pct = 10.0 + if event.shiftDown: + (xres, yres) = (event.shape[1], event.shape[0]) + twidth = int(yres * 0.01 * track_size_pct) + + self.im.end_tracking() + self.send_camera_stop_tracking() + + 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}") + top_left_x = event.X / xres + top_left_y = event.Y / yres + bot_right_x = (event.X + twidth) / xres + bot_right_y = (event.Y + twidth) / yres + self.send_camera_track_rectangle( + top_left_x, top_left_y, bot_right_x, bot_right_y + ) + + elif event.controlDown: + self.im.end_tracking() + self.send_camera_stop_tracking() + 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 + + NOTE: point coords and radius are normalised to [0, 1] + """ + src_sys = self.mpstate.master().source_system + 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}" + ) + target_camera = 0 + self.mpstate.master().mav.command_long_send( + tgt_sys, # target_system + tgt_cmp, # target_component + mavutil.mavlink.MAV_CMD_CAMERA_TRACK_POINT, # command + 0, # confirmation + point_x, # param1 + point_y, # param2 + radius, # param3 + target_camera, # param4 + 0, # param5 + 0, # param6 + 0, # param7 + ) + + def send_camera_track_rectangle( + self, top_left_x, top_left_y, bottom_right_x, bottom_right_y + ): + """ + https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_RECTANGLE + + NOTE: coordinates are normalised to [0, 1] + """ + src_sys = self.mpstate.master().source_system + 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}" + ) + target_camera = 0 + self.mpstate.master().mav.command_long_send( + tgt_sys, # target_system + tgt_cmp, # target_component + mavutil.mavlink.MAV_CMD_CAMERA_TRACK_RECTANGLE, # command + 0, # confirmation + top_left_x, # param1 + top_left_y, # param2 + bottom_right_x, # param3 + bottom_right_y, # param4 + target_camera, # param5 + 0, # param6 + 0, # param7 + ) + + def send_camera_stop_tracking(self): + """ + https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_STOP_TRACKING + """ + src_sys = self.mpstate.master().source_system + 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}" + ) + target_camera = 0 + self.mpstate.master().mav.command_long_send( + tgt_sys, # target_system + tgt_cmp, # target_component + mavutil.mavlink.MAV_CMD_CAMERA_STOP_TRACKING, # command + 0, # confirmation + target_camera, # param1 + 0, # param2 + 0, # param3 + 0, # param4 + 0, # param5 + 0, # param6 + 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 + + parser = OptionParser("camera_view.py [options]") + parser.add_option("--master", default=None, type=str, help="MAVLink device") + parser.add_option("--rtsp-server", default=None, type=str, help="RTSP server URL") + parser.add_option("--sysid", default=255, type=int, help="Source system ID") + parser.add_option("--cmpid", default=1, type=int, help="Source component ID") + + (opts, args) = parser.parse_args() + if opts.master is None: + print("Must specify a MAVLink device") + sys.exit(1) + if opts.rtsp_server is None: + print("Must specify an RTSP server URL") + sys.exit(1) + + class MockMPState: + def __init__(self, device, sysid, compid): + self._device = device + self._sysid = sysid + self._cmpid = compid + + self._connection = mavutil.mavlink_connection( + self._device, + source_system=self._sysid, + source_component=self._cmpid, + ) + print("Searching for vehicle") + while not self._connection.probably_vehicle_heartbeat( + self._connection.wait_heartbeat() + ): + print(".", end="") + print("\nFound vehicle") + print( + "Heartbeat received (system: {} component: {})".format( + self._connection.target_system, self._connection.target_component + ) + ) + + def master(self): + return self._connection + + mpstate = MockMPState(opts.master, opts.sysid, opts.cmpid) + camera_view = CameraView(mpstate, "Camera View", opts.rtsp_server) + + while True: + time.sleep(0.1) + camera_view.check_events() diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py new file mode 100644 index 0000000000..548f208748 --- /dev/null +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -0,0 +1,1056 @@ +""" +An onboard controller for managing a tracking camera and gimbal. + +Usage +----- + +1. Run controller on localhost using an RTSP stream from a Gazebo simulation. + + python ./onboard_controller.py + --master 127.0.0.1:14550 + --rtsp-server rtsp://127.0.0.1:8554/camera + +2. Run controller on localhost using an RTSP stream from a Herelink +connected to a home WiFi network. + + python ./onboard_controller.py + --master 127.0.0.1:14550 + --rtsp-server rtsp://192.168.1.204:8554/fpv_stream + +3. Run controller on RPi4 companion computer using an RTSP stream from +a SIYI A8 camea. + + python ./onboard_controller.py + --master 192.168.144.171:15001 + --rtsp-server rtsp://192.168.144.25:8554/main.264 + +Examples +-------- + +Example devices and RTSP servers used in testing. + +localhost +device = "127.0.0.1:14550 + +companion computer - NET virtual serial port +device = "192.168.144.171:15001" + +localhost simulation +rtsp_url = "rtsp://127.0.0.1:8554/camera" + +home wifi +rtsp_url = "rtsp://192.168.1.204:8554/fpv_stream" + +herelink wifi access point +rtsp_url = "rtsp://192.168.43.1:8554/fpv_stream" + +SIYI A8 camera +rtsp_url = "rtsp://192.168.144.25:8554/main.264" +""" + +import copy +import cv2 +import gi +import math +import numpy as np +import queue +import threading +import time + +from enum import Enum +from pymavlink import mavutil + +from transforms3d import euler +from transforms3d import quaternions + +from MAVProxy.modules.lib import mp_util + +gi.require_version("Gst", "1.0") +from gi.repository import Gst + + +class CameraCapFlags(Enum): + """ + https://mavlink.io/en/messages/common.html#CAMERA_CAP_FLAGS + """ + + CAPTURE_VIDEO = mavutil.mavlink.CAMERA_CAP_FLAGS_CAPTURE_VIDEO + CAPTURE_IMAGE = mavutil.mavlink.CAMERA_CAP_FLAGS_CAPTURE_IMAGE + HAS_MODES = mavutil.mavlink.CAMERA_CAP_FLAGS_HAS_MODES + CAN_CAPTURE_IMAGE_IN_VIDEO_MODE = ( + mavutil.mavlink.CAMERA_CAP_FLAGS_CAN_CAPTURE_IMAGE_IN_VIDEO_MODE + ) + CAN_CAPTURE_VIDEO_IN_IMAGE_MODE = ( + mavutil.mavlink.CAMERA_CAP_FLAGS_CAN_CAPTURE_VIDEO_IN_IMAGE_MODE + ) + HAS_IMAGE_SURVEY_MODE = mavutil.mavlink.CAMERA_CAP_FLAGS_HAS_IMAGE_SURVEY_MODE + HAS_BASIC_ZOOM = mavutil.mavlink.CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM + HAS_BASIC_FOCUS = mavutil.mavlink.CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS + HAS_VIDEO_STREAM = mavutil.mavlink.CAMERA_CAP_FLAGS_HAS_VIDEO_STREAM + HAS_TRACKING_POINT = mavutil.mavlink.CAMERA_CAP_FLAGS_HAS_TRACKING_POINT + HAS_TRACKING_RECTANGLE = mavutil.mavlink.CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE + HAS_TRACKING_GEO_STATUS = mavutil.mavlink.CAMERA_CAP_FLAGS_HAS_TRACKING_GEO_STATUS + + +class CameraTrackingStatusFlags(Enum): + """ + https://mavlink.io/en/messages/common.html#CAMERA_TRACKING_STATUS_FLAGS + """ + + IDLE = mavutil.mavlink.CAMERA_TRACKING_STATUS_FLAGS_IDLE + ACTIVE = mavutil.mavlink.CAMERA_TRACKING_STATUS_FLAGS_ACTIVE + ERROR = mavutil.mavlink.CAMERA_TRACKING_STATUS_FLAGS_ERROR + + +class CameraTrackingMode(Enum): + """ + https://mavlink.io/en/messages/common.html#CAMERA_TRACKING_MODE + """ + + NONE = mavutil.mavlink.CAMERA_TRACKING_MODE_NONE + POINT = mavutil.mavlink.CAMERA_TRACKING_MODE_POINT + RECTANGLE = mavutil.mavlink.CAMERA_TRACKING_MODE_RECTANGLE + + +class CameraTrackingTargetData(Enum): + """ + https://mavlink.io/en/messages/common.html#CAMERA_TRACKING_TARGET_DATA + """ + + NONE = mavutil.mavlink.CAMERA_TRACKING_TARGET_DATA_NONE + EMBEDDED = mavutil.mavlink.CAMERA_TRACKING_TARGET_DATA_EMBEDDED + RENDERED = mavutil.mavlink.CAMERA_TRACKING_TARGET_DATA_RENDERED + IN_STATUS = mavutil.mavlink.CAMERA_TRACKING_TARGET_DATA_IN_STATUS + + +class OnboardController: + def __init__(self, device, sysid, cmpid, rtsp_url): + self._device = device + self._sysid = sysid + self._cmpid = cmpid + self._rtsp_url = rtsp_url + + # mavlink connection + self._connection = None + + # list of controllers to forward mavlink message to + self._controllers = [] + self._camera_controller = None + self._gimbal_controller = None + + print(f"Onboard Controller: src_sys: {self._sysid}, src_cmp: {self._cmpid})") + + def _connect_to_mavlink(self): + """ + Establish a mavlink connection. + """ + self._connection = mavutil.mavlink_connection( + self._device, + source_system=self._sysid, + source_component=self._cmpid, + ) + print("Searching for vehicle") + while not self._connection.probably_vehicle_heartbeat( + self._connection.wait_heartbeat() + ): + print(".", end="") + print("\nFound vehicle") + print( + "Heartbeat received (system: {} component: {})".format( + self._connection.target_system, self._connection.target_component + ) + ) + + def _send_heartbeat_task(self): + """ + Task to send a heartbeat identifying this as an onboard controller. + """ + while True: + self._connection.mav.heartbeat_send( + type=mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER, + autopilot=mavutil.mavlink.MAV_AUTOPILOT_INVALID, + base_mode=0, + custom_mode=0, + system_status=mavutil.mavlink.MAV_STATE_UNINIT, + mavlink_version=3, + ) + time.sleep(1.0) + + def _mavlink_recv_task(self): + """ + Task to receive a mavlink message and forwward to controllers. + """ + update_rate = 1000.0 + update_period = 1.0 / update_rate + + while True: + + 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 + ): + return + + for controller in self._controllers: + if not hasattr(controller, "mavlink_packet"): + return + controller.mavlink_packet(msg) + + start_time = time.time() + __process_message() + elapsed_time = time.time() - start_time + sleep_time = max(0.0, update_period - elapsed_time) + time.sleep(sleep_time) + + def run(self): + """ + Run the onboard controller. + + Connects to mavlink and a video stream then monitors camera track + requests and updates a tracker and gimbal control as required. + """ + self._connect_to_mavlink() + + # Connect to video stream + video_stream = VideoStream(self._rtsp_url) + + # TODO: add retry limit and timeout + print("Waiting for video stream") + while not video_stream.frame_available(): + print(".", end="") + time.sleep(0.1) + print("\nVideo stream available") + + # Create and register controllers + self._camera_controller = CameraTrackController(self._connection) + self._gimbal_controller = GimbalController(self._connection) + self._controllers.append(self._camera_controller) + self._controllers.append(self._gimbal_controller) + + # Start the heartbeat thread + heartbeat_thread = threading.Thread(target=self._send_heartbeat_task) + heartbeat_thread.daemon = True + heartbeat_thread.start() + + # Start the mavlink_recv thread + mavlink_recv_thread = threading.Thread(target=self._mavlink_recv_task) + mavlink_recv_thread.daemon = True + mavlink_recv_thread.start() + + # Create tracker + tracker = TrackerCSTR() + tracking_rect = None + tracking_rect_changed = True + + # TODO: 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() + + if video_stream.frame_available(): + frame_count += 1 + frame = copy.deepcopy(video_stream.frame()) + + track_type = self._camera_controller.track_type() + if track_type is CameraTrackType.NONE: + if tracking_rect is not None: + tracking_rect = None + 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): + return ( + math.isclose(rect1.top_left_x, rect2.top_left_x) + and math.isclose(rect1.top_left_y, rect2.top_left_y) + and math.isclose(rect1.bot_right_x, rect2.bot_right_x) + and math.isclose(rect1.bot_right_y, rect2.bot_right_y) + ) + + if not __compare_rect( + tracking_rect, self._camera_controller.track_rectangle() + ): + tracking_rect_changed = True + + if tracking_rect is None or tracking_rect_changed: + tracking_rect_changed = False + tracking_rect = self._camera_controller.track_rectangle() + nroi = [ + tracking_rect.top_left_x, + tracking_rect.top_left_y, + tracking_rect.bot_right_x - tracking_rect.top_left_x, + tracking_rect.bot_right_y - tracking_rect.top_left_y, + ] + tracker.set_normalised_roi(nroi) + + # update tracker and gimbal if tracking active + if tracking_rect is not None: + success, box = tracker.update(frame) + if success: + (x, y, w, h) = [int(v) for v in box] + u = x + w // 2 + v = y + h // 2 + self._gimbal_controller.update_center(u, v, frame.shape) + 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: + """ + BlueRov video capture class. Adapted to capture a RTSP stream. + + Attributes: + rtsp_url (string): RTSP URL + video_decode (string): Transform YUV (12bits) to BGR (24bits) + video_pipe (object): GStreamer top-level pipeline + video_sink (object): Gstreamer sink element + video_sink_conf (string): Sink configuration + video_source (string): Udp source ip and port + latest_frame (np.ndarray): Latest retrieved video frame + """ + + def __init__(self, rtsp_url, latency=50): + Gst.init(None) + + self.rtsp_url = rtsp_url + self.latency = latency + + self.latest_frame = self._new_frame = None + + self.video_source = f"rtspsrc location={rtsp_url} latency={latency}" + + # Python does not have nibble, convert YUV nibbles (4-4-4) to OpenCV standard BGR bytes (8-8-8) + self.video_decode = ( + "! decodebin ! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert" + ) + # Create a sink to get data + self.video_sink_conf = ( + "! appsink emit-signals=true sync=false max-buffers=2 drop=true" + ) + + self.video_pipe = None + self.video_sink = None + + self.run() + + def start_gst(self, config=None): + """ Start gstreamer pipeline and sink + Pipeline description list e.g: + [ + 'videotestsrc ! decodebin', \ + '! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert', + '! appsink' + ] + + Args: + config (list, optional): Gstreamer pileline description list + """ + + if not config: + config = [ + "videotestsrc ! decodebin", + "! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert", + "! appsink", + ] + + command = " ".join(config) + self.video_pipe = Gst.parse_launch(command) + self.video_pipe.set_state(Gst.State.PLAYING) + self.video_sink = self.video_pipe.get_by_name("appsink0") + + @staticmethod + def gst_to_opencv(sample): + """Transform byte array into np array + + Args: + sample (TYPE): Description + + Returns: + TYPE: Description + """ + buf = sample.get_buffer() + caps_structure = sample.get_caps().get_structure(0) + array = np.ndarray( + (caps_structure.get_value("height"), caps_structure.get_value("width"), 3), + buffer=buf.extract_dup(0, buf.get_size()), + dtype=np.uint8, + ) + return array + + def frame(self): + """Get Frame + + Returns: + np.ndarray: latest retrieved image frame + """ + if self.frame_available: + self.latest_frame = self._new_frame + # reset to indicate latest frame has been 'consumed' + self._new_frame = None + return self.latest_frame + + def frame_available(self): + """Check if a new frame is available + + Returns: + bool: true if a new frame is available + """ + return self._new_frame is not None + + def run(self): + """Get frame to update _new_frame""" + + self.start_gst( + [ + self.video_source, + self.video_decode, + self.video_sink_conf, + ] + ) + + self.video_sink.connect("new-sample", self.callback) + + def callback(self, sink): + sample = sink.emit("pull-sample") + self._new_frame = self.gst_to_opencv(sample) + + return Gst.FlowReturn.OK + + +class CameraTrackType(Enum): + """ " + Camera track types. + """ + + NONE = 0 + POINT = 1 + RECTANGLE = 2 + + +class CameraTrackPoint: + """ + Camera track point (normalised) + + https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_POINT + """ + + def __init__(self, point_x, point_y, radius): + self.point_x = point_x + self.point_y = point_y + self.radius = radius + + +class CameraTrackRectangle: + """ + Camera track rectangle (normalised) + + https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_RECTANGLE + """ + + 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 CameraTrackController: + """ + Controller for onboard camera tracking. + """ + + def __init__(self, connection): + self._connection = connection + self._sysid = self._connection.source_system + self._cmpid = self._connection.source_component + + print( + f"Camera Track Controller: " + f"src_sys: {self._sysid}, src_cmp: {self._cmpid}" + ) + + # TODO: this should be supplied by a camera when using hardware + # camera information + self._vendor_name = "SIYI" + self._model_name = "A8" + self._focal_length = float("nan") + self._sensor_size_h = float("nan") + self._sensor_size_v = float("nan") + self._resolution_h = 0 + self._resolution_v = 0 + self._gimbal_device_id = 1 + + # tracking details + self._lock = threading.Lock() + self._track_type = CameraTrackType.NONE + self._track_point = None + self._track_rect = None + + # Start the mavlink thread + self._mavlink_in_queue = queue.Queue() + self._mavlink_out_queue = queue.Queue() + self._mavlink_thread = threading.Thread(target=self._mavlink_task) + self._mavlink_thread.daemon = True + self._mavlink_thread.start() + + # Start the send tracking status thread + self._send_status_thread = threading.Thread(target=self._send_status_task) + self._send_status_thread.daemon = True + self._send_status_thread.start() + + def track_type(self): + """ + Return the current track type. + """ + with self._lock: + track_type = copy.deepcopy(self._track_type) + return track_type + + def track_point(self): + """ + Return a point to track if the track type is POINT, else None. + """ + with self._lock: + track_point = copy.deepcopy(self._track_point) + return track_point + + def track_rectangle(self): + """ + Return a rectangle to track if the track type is RECTANGLE, else None. + """ + with self._lock: + track_rect = copy.deepcopy(self._track_rect) + return track_rect + + def mavlink_packet(self, msg): + """ + Process a mavlink packet. + """ + self._mavlink_in_queue.put(msg) + + def _send_camera_information(self): + """ + AP_Camera must receive camera information, including capability flags, + before it will accept tracking requests. + + If MAV_CMD_CAMERA_TRACK_POINT or MAV_CMD_CAMERA_TRACK_RECTANGLE result + in ACK UNSUPPORTED, then this may not have been sent. + """ + flags = ( + CameraCapFlags.CAPTURE_VIDEO.value + | CameraCapFlags.CAPTURE_IMAGE.value + | CameraCapFlags.HAS_MODES.value + | CameraCapFlags.CAN_CAPTURE_IMAGE_IN_VIDEO_MODE.value + | CameraCapFlags.CAN_CAPTURE_VIDEO_IN_IMAGE_MODE.value + | CameraCapFlags.HAS_IMAGE_SURVEY_MODE.value + | CameraCapFlags.HAS_BASIC_ZOOM.value + | CameraCapFlags.HAS_BASIC_FOCUS.value + | CameraCapFlags.HAS_VIDEO_STREAM.value + | CameraCapFlags.HAS_TRACKING_POINT.value + | CameraCapFlags.HAS_TRACKING_RECTANGLE.value + | CameraCapFlags.HAS_TRACKING_GEO_STATUS.value + ) + + 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 + to_uint8_t(self._model_name), # model_name + (1 << 24) | (0 << 16) | (0 << 8) | 1, # firmware_version + self._focal_length, # focal_length + self._sensor_size_h, # sensor_size_h + self._sensor_size_v, # sensor_size_v + self._resolution_h, # resolution_h + self._resolution_v, # resolution_v + 0, # lens_id + flags, # flags + 0, # cam_definition_version + b"", # cam_definition_uri + self._gimbal_device_id, # gimbal_device_id + ) + print("Sent camera information") + + 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 + 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: + 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: + 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 + + msg = self._connection.mav.camera_tracking_image_status_encode( + tracking_status, + tracking_mode, + target_data, + point_x, + point_y, + radius, + rec_top_x, + rec_top_y, + rec_bottom_x, + rec_bottom_y, + ) + 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}") + 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}" + ) + with self._lock: + self._track_type = CameraTrackType.RECTANGLE + self._track_rect = CameraTrackRectangle( + top_left_x, top_left_y, bot_right_x, bot_right_y + ) + + def _handle_camera_stop_tracking(self, msg): + print("Got COMMAND_LONG: CAMERA_STOP_TRACKING") + with self._lock: + self._track_type = CameraTrackType.NONE + self._track_point = None + self._track_rect = None + + def _mavlink_task(self): + """ + Process mavlink messages relevant to camera tracking. + """ + self._send_camera_information() + + with self._lock: + sysid = self._sysid + cmpid = self._cmpid + + update_rate = 1000.0 + update_period = 1.0 / update_rate + + while True: + + def __process_message(): + if self._mavlink_in_queue.empty(): + return + + msg = self._mavlink_in_queue.get() + mtype = msg.get_type() + + if msg and mtype == "COMMAND_LONG": + if msg.target_system != sysid: + return + elif msg.command == mavutil.mavlink.MAV_CMD_CAMERA_TRACK_POINT: + self._handle_camera_track_point(msg) + elif msg.command == mavutil.mavlink.MAV_CMD_CAMERA_TRACK_RECTANGLE: + self._handle_camera_track_rectangle(msg) + elif msg.command == mavutil.mavlink.MAV_CMD_CAMERA_STOP_TRACKING: + self._handle_camera_stop_tracking(msg) + else: + pass + + start_time = time.time() + __process_message() + elapsed_time = time.time() - start_time + sleep_time = max(0.0, update_period - elapsed_time) + time.sleep(sleep_time) + + def _send_status_task(self): + """ + Send camera tracking image status. + """ + # TODO: stop sending image status when tracking stopped + # TODO: set streaming rate using MAV_CMD_SET_MESSAGE_INTERVAL + + update_rate = 2.0 + update_period = 1.0 / update_rate + + while True: + + def __send_message(): + self._send_camera_tracking_image_status() + + start_time = time.time() + __send_message() + elapsed_time = time.time() - start_time + sleep_time = max(0.0, update_period - elapsed_time) + time.sleep(sleep_time) + + +class GimbalController: + """ + Gimbal controller for onboard camera tracking. + """ + + def __init__(self, connection): + self._connection = connection + self._sysid = self._connection.source_system + self._cmpid = self._connection.source_component + + print(f"Gimbal Controller: src_sys: {self._sysid}, src_cmp: {self._cmpid})") + + # Shared variables + self._control_lock = threading.Lock() + self._center_x = 0 + self._center_y = 0 + self._width = None + self._height = None + self._tracking = False + + # gimbal state + # TODO: obtain the mount neutral angles from params + # NOTE: q = [w, x, y, z] + self._mavlink_lock = threading.Lock() + self._neutral_x = 0.0 + self._neutral_y = 0.0 + self._neutral_z = 0.0 + self._gimbal_device_flags = None + self._gimbal_orientation = None + self._gimbal_failure_flags = None + + # TODO: add options for PI controller gains + # PI controllers: SIYI A8: 0.1, Gazebo: 0.3 + self._pitch_controller = PI_controller(Pgain=0.5, Igain=0.01, IMAX=1.0) + self._yaw_controller = PI_controller(Pgain=0.5, Igain=0.01, IMAX=1.0) + + # Start the control thread + self._control_in_queue = queue.Queue() + self._control_out_queue = queue.Queue() + self._control_thread = threading.Thread(target=self._control_task) + self._control_thread.daemon = True + self._control_thread.start() + + # Start the mavlink thread + self._mavlink_thread = threading.Thread(target=self._mavlink_task) + self._mavlink_thread.daemon = True + self._mavlink_thread.start() + + def update_center(self, x, y, shape): + with self._control_lock: + self._tracking = True + 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: + self._tracking = False + self._center_x = 0.0 + self._center_y = 0.0 + self._pitch_controller.reset_I() + self._yaw_controller.reset_I() + + def mavlink_packet(self, msg): + self._control_in_queue.put(msg) + + def _send_gimbal_manager_pitch_yaw_angles(self, pitch, yaw, pitch_rate, yaw_rate): + """ + Send a mavlink message to set the gimbal pitch and yaw (radians). + """ + msg = self._connection.mav.gimbal_manager_set_pitchyaw_encode( + self._connection.target_system, + self._connection.target_component, + 0, + 0, + pitch, + yaw, + pitch_rate, + yaw_rate, + ) + self._connection.mav.send(msg) + + def _control_task(self): + """ + Gimbal control task. + + When tracking, move the camera to centre on the bounding box + around the tracked object. + + When not tracking, return the gimbal to its neutral position. + """ + while True: + # Record the start time of the loop + start_time = time.time() + + # Copy shared variables + with self._control_lock: + act_x = self._center_x + act_y = self._center_y + frame_width = self._width + frame_height = self._height + tracking = self._tracking + + # Return gimbal to its neutral orientation when not tracking + with self._mavlink_lock: + gimbal_orientation = self._gimbal_orientation + + if not tracking and gimbal_orientation is not None: + # NOTE: to centre the gimbal when not tracking, we need to know + # the neutral angles from the MNT1_NEUTRAL_x params, and also + # the current mount orientation. + _, ay, az = euler.quat2euler(gimbal_orientation) + + err_pitch = self._neutral_y - ay + pitch_rate_rads = self._pitch_controller.run(err_pitch) + + err_yaw = self._neutral_z - az + yaw_rate_rads = self._yaw_controller.run(err_yaw) + + self._send_gimbal_manager_pitch_yaw_angles( + float("nan"), + float("nan"), + pitch_rate_rads, + yaw_rate_rads, + ) + elif frame_width is not None and frame_height is not None: + tgt_x = 0.5 * frame_width + tgt_y = 0.5 * frame_height + + if math.isclose(act_x, 0.0) and math.isclose(act_y, 0.0): + err_x = 0.0 + err_y = 0.0 + else: + err_x = act_x - tgt_x + err_y = -(act_y - tgt_y) + + err_pitch = math.radians(err_y) + pitch_rate_rads = self._pitch_controller.run(err_pitch) + + 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"), + pitch_rate_rads, + yaw_rate_rads, + ) + + # Update at 50Hz + update_period = 0.02 + elapsed_time = time.time() - start_time + sleep_time = max(0.0, update_period - elapsed_time) + time.sleep(sleep_time) + + def _mavlink_task(self): + """ + Process mavlink messages relevant to gimbal management. + """ + update_rate = 1000.0 + update_period = 1.0 / update_rate + + while True: + + def __process_message(): + if self._control_in_queue.empty(): + return + msg = self._control_in_queue.get() + mtype = msg.get_type() + # NOTE: GIMBAL_DEVICE_ATTITUDE_STATUS is broadcast + # (sysid=0, cmpid=0) + if msg and mtype == "GIMBAL_DEVICE_ATTITUDE_STATUS": + with self._mavlink_lock: + self._gimbal_device_flags = msg.flags + self._gimbal_orientation = msg.q + self._gimbal_failure_flags = msg.failure_flags + + start_time = time.time() + __process_message() + elapsed_time = time.time() - start_time + sleep_time = max(0.0, update_period - elapsed_time) + time.sleep(sleep_time) + + +class PI_controller: + """ + Simple PI controller + + MAVProxy/modules/mavproxy_SIYI/PI_controller (modified) + """ + + def __init__(self, Pgain, Igain, IMAX, gain_mul=1.0, max_rate=math.radians(30.0)): + self.Pgain = Pgain + self.Igain = Igain + self.IMAX = IMAX + self.gain_mul = gain_mul + self.max_rate = max_rate + self.I = 0.0 + + self.last_t = time.time() + + def run(self, err, ff_rate=0.0): + now = time.time() + dt = now - self.last_t + if now - self.last_t > 1.0: + self.reset_I() + dt = 0 + self.last_t = now + P = self.Pgain * self.gain_mul + I = self.Igain * self.gain_mul + IMAX = self.IMAX + max_rate = self.max_rate + + out = P * err + saturated = err > 0 and (out + self.I) >= max_rate + saturated |= err < 0 and (out + self.I) <= -max_rate + if not saturated: + self.I += I * err * dt + self.I = mp_util.constrain(self.I, -IMAX, IMAX) + ret = out + self.I + ff_rate + return mp_util.constrain(ret, -max_rate, max_rate) + + def reset_I(self): + self.I = 0 + + +class TrackerCSTR: + """ + Wrapper for cv2.legacy.TrackerCSRT + """ + + def __init__(self): + self._tracker = cv2.legacy.TrackerCSRT_create() + self._nroi = None + self._nroi_changed = False + + def update(self, frame): + if self._nroi is None or frame is None: + return False, None + + if self._nroi_changed: + self._tracker = cv2.legacy.TrackerCSRT_create() + # denormalise the roi + height, width, _ = frame.shape + roi = [ + int(self._nroi[0] * width), # x + int(self._nroi[1] * height), # y + 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}" + ) + self._tracker.init(frame, roi) + self._nroi_changed = False + + return self._tracker.update(frame) + + def set_normalised_roi(self, nroi): + """ + Set the region of interest + + [top_left_x, top_left_y, width, height] + """ + self._nroi = nroi + self._nroi_changed = True + + +if __name__ == "__main__": + from optparse import OptionParser + + parser = OptionParser("onboard_controller.py [options]") + parser.add_option("--master", default=None, type=str, help="MAVLink device") + parser.add_option("--rtsp-server", default=None, type=str, help="RTSP server URL") + parser.add_option("--sysid", default=1, type=int, help="Source system ID") + parser.add_option( + "--cmpid", + default=mavutil.mavlink.MAV_COMP_ID_ONBOARD_COMPUTER, + type=int, + help="Source component ID", + ) + + (opts, args) = parser.parse_args() + if opts.master is None: + print("Must specify a MAVLink device") + sys.exit(1) + if opts.rtsp_server is None: + print("Must specify an RTSP server URL") + sys.exit(1) + + controller = OnboardController( + opts.master, opts.sysid, opts.cmpid, opts.rtsp_server + ) + controller.run() diff --git a/MAVProxy/modules/mavproxy_camtrack/tracker_image.py b/MAVProxy/modules/mavproxy_camtrack/tracker_image.py new file mode 100644 index 0000000000..6b5a204d48 --- /dev/null +++ b/MAVProxy/modules/mavproxy_camtrack/tracker_image.py @@ -0,0 +1,269 @@ +""" +Override tracker in MPImage +""" + +import cv2 +import dlib +import threading +import time + +from MAVProxy.modules.lib import wx_processguard +from MAVProxy.modules.lib.wx_loader import wx + +from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import win_layout + +from MAVProxy.modules.lib.mp_image import MPImage +from MAVProxy.modules.lib.mp_image import MPImageFrame +from MAVProxy.modules.lib.mp_image import MPImagePanel + + +class TrackerImage(MPImage): + """An MPImage class that allows the tracker type to be overridden""" + + def __init__( + self, + title="MPImage", + width=512, + height=512, + can_zoom=False, + can_drag=False, + mouse_events=False, + mouse_movement_events=False, + key_events=False, + auto_size=False, + report_size_changes=False, + daemon=False, + auto_fit=False, + fps=10, + ): + super(TrackerImage, self).__init__( + title, + width, + height, + can_zoom, + can_drag, + mouse_events, + mouse_movement_events, + key_events, + auto_size, + report_size_changes, + daemon, + auto_fit, + fps, + ) + + def child_task(self): + """Child process - this holds all the GUI elements""" + mp_util.child_close_fds() + + state = self + + self.app = wx.App(False) + self.app.frame = TrackerImageFrame(state=self) + self.app.frame.Show() + self.app.MainLoop() + + +class TrackerImageFrame(wx.Frame): + """Main frame for an image with object tracking""" + + def __init__(self, state): + wx.Frame.__init__(self, None, wx.ID_ANY, state.title) + self.state = state + state.frame = self + self.last_layout_send = time.time() + self.sizer = wx.BoxSizer(wx.VERTICAL) + state.panel = TrackerImagePanel(self, state) + self.sizer.Add(state.panel, 1, wx.EXPAND) + self.SetSizer(self.sizer) + self.Bind(wx.EVT_IDLE, self.on_idle) + self.Bind(wx.EVT_SIZE, state.panel.on_size) + + def on_idle(self, event): + """Prevent the main loop spinning too fast""" + state = self.state + now = time.time() + if now - self.last_layout_send > 1: + self.last_layout_send = now + state.out_queue.put(win_layout.get_wx_window_layout(self)) + time.sleep(0.1) + + +class TrackerImagePanel(MPImagePanel): + """An MPImagePanel that allows the tracker type to be overridden""" + + 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""" + if self.raw_img is None: + return + self.tracker = None + # TODO: accept tracker name as option + # tracker_name = "dlib_correlation" + # tracker_name = "cv2_cstr" + tracker_name = "mavlink" + tracker = create_tracker(tracker_name, self.state) + tracker.start_track(self.raw_img, box) + self.tracker = tracker + + +class TrackerPos: + """Rectangle output by a tracker [(left, top), (right, bottom)]""" + + def __init__(self, left, right, top, bottom): + self._left = left + self._right = right + self._top = top + self._bottom = bottom + + def left(self): + """Rectangle left coordinate (x1)""" + return self._left + + def right(self): + """Rectangle right coordinate (x2)""" + return self._right + + def top(self): + """Rectangle top coordinate (y1)""" + return self._top + + def bottom(self): + """Rectangle bottom coordinate (y2)""" + return self._bottom + + +class Tracker: + """Interface for trackers used by the MPImage class""" + + def __init__(self, state): + self.state = state + + def start_track(self, raw_image, box): + """Start the tracker""" + pass + + def update(self, frame): + """Update the tracker""" + pass + + def get_position(self): + """Return a rectangle at the position of the tracked object""" + return TrackerPos(0, 0, 0, 0) + + +class TrackerDlibCorrelation(Tracker): + """Wrapper for the dlib correlation tracker""" + + def __init__(self, state): + super().__init__(state) + self._tracker = dlib.correlation_tracker() + + def start_track(self, raw_image, box): + maxx = raw_image.shape[1] - 1 + maxy = raw_image.shape[0] - 1 + rect = dlib.rectangle( + max(int(box.x - box.width / 2), 0), + max(int(box.y - box.height / 2), 0), + min(int(box.x + box.width / 2), maxx), + min(int(box.y + box.height / 2), maxy), + ) + self._tracker.start_track(raw_image, rect) + + def update(self, frame): + self._tracker.update(frame) + + def get_position(self): + pos = self._tracker.get_position() + tracker_pos = TrackerPos(pos.left(), pos.right(), pos.top(), pos.bottom()) + return tracker_pos + + +class TrackerCSTR(Tracker): + """ + Wrapper for cv2.legacy.TrackerCSRT + """ + + def __init__(self, state): + super().__init__(state) + self._tracker = cv2.legacy.TrackerCSRT_create() + # TODO: check if None would be better (do callers handle this?) + self._tracker_pos = TrackerPos(0, 0, 0, 0) + + def start_track(self, raw_image, box): + self._tracker = cv2.legacy.TrackerCSRT_create() + x = max(int(box.x), 0) + y = max(int(box.y), 0) + w = min(int(box.width), max(raw_image.shape[1] - x, 1)) + h = min(int(box.height), max(raw_image.shape[0] - y, 1)) + rect = [x, y, w, h] + self._tracker.init(raw_image, rect) + + def update(self, frame): + success, box = self._tracker.update(frame) + if success: + (x, y, w, h) = [v for v in box] + self._tracker_pos = TrackerPos(x, x + w, y, y + h) + + def get_position(self): + return self._tracker_pos + + +class TrackerMAVLink(Tracker): + """ + MAVLink tracker (i.e. runs on a MAVLink gimbal or companion computer) + """ + + def __init__(self, state): + super(TrackerMAVLink, self).__init__(state) + self.event_queue = state.out_queue + self._tracker_pos = TrackerPos(0, 0, 0, 0) + + def start_track(self, raw_image, box): + x = max(int(box.x), 0) + y = max(int(box.y), 0) + w = min(int(box.width), max(raw_image.shape[1] - x, 1)) + h = min(int(box.height), max(raw_image.shape[0] - y, 1)) + + # normalised rectangle + top_left_x = x / w + top_left_y = y / h + bottom_right_x = (x + box.width) / w + 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 create_tracker(name, state): + """ + Factory method to create a tracker. + """ + if name == "dlib_correlation": + return TrackerDlibCorrelation(state) + elif name == "cv2_cstr": + return TrackerCSTR(state) + elif name == "mavlink": + return TrackerMAVLink(state) + else: + return TrackerDlibCorrelation(state) From 99acc6636aa541cff361b8a57998ffbde26d8f0c Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 8 Oct 2024 19:20:57 +0100 Subject: [PATCH 05/28] mp_image: handle each event in a separate function to allow override in sub-classes Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/lib/mp_image.py | 85 +++++++++++++++++--------------- 1 file changed, 45 insertions(+), 40 deletions(-) diff --git a/MAVProxy/modules/lib/mp_image.py b/MAVProxy/modules/lib/mp_image.py index cafd68f303..2298363ece 100755 --- a/MAVProxy/modules/lib/mp_image.py +++ b/MAVProxy/modules/lib/mp_image.py @@ -576,6 +576,49 @@ def handle_osd(self, obj): return self.osd_elements[obj.label] = obj + def process_event(self, obj): + """Process a single event""" + if isinstance(obj, MPImageOSD_Element): + self.handle_osd(obj) + if isinstance(obj, MPImageData): + self.set_image_data(obj.data, obj.width, obj.height) + if isinstance(obj, MPImageTitle): + state.frame.SetTitle(obj.title) + if isinstance(obj, MPImageRecenter): + self.on_recenter(obj.location) + if isinstance(obj, MPImageMenu): + self.set_menu(obj.menu) + if isinstance(obj, MPImagePopupMenu): + self.set_popup_menu(obj.menu) + if isinstance(obj, MPImageBrightness): + state.brightness = obj.brightness + self.need_redraw = True + if isinstance(obj, MPImageFullSize): + self.full_size() + if isinstance(obj, MPImageFitToWindow): + self.fit_to_window() + if isinstance(obj, win_layout.WinLayout): + win_layout.set_wx_window_layout(state.frame, obj) + if isinstance(obj, MPImageGStreamer): + self.start_gstreamer(obj.pipeline) + if isinstance(obj, MPImageVideo): + self.start_video(obj.filename) + if isinstance(obj, MPImageFPSMax): + self.fps_max = obj.fps_max + print("FPS_MAX: ", self.fps_max) + if isinstance(obj, MPImageSeekPercent): + self.seek_video(obj.percent) + if isinstance(obj, MPImageSeekFrame): + self.seek_video_frame(obj.frame) + if isinstance(obj, MPImageColormap): + self.colormap = obj.colormap + if isinstance(obj, MPImageColormapIndex): + self.colormap_index = obj.colormap_index + if isinstance(obj, MPImageStartTracker): + self.start_tracker(obj) + if isinstance(obj, MPImageEndTracker): + self.tracker = None + def on_redraw_timer(self, event): '''the redraw timer ensures we show new map tiles as they are downloaded''' @@ -586,46 +629,8 @@ def on_redraw_timer(self, event): except Exception: time.sleep(0.05) return - if isinstance(obj, MPImageOSD_Element): - self.handle_osd(obj) - if isinstance(obj, MPImageData): - self.set_image_data(obj.data, obj.width, obj.height) - if isinstance(obj, MPImageTitle): - state.frame.SetTitle(obj.title) - if isinstance(obj, MPImageRecenter): - self.on_recenter(obj.location) - if isinstance(obj, MPImageMenu): - self.set_menu(obj.menu) - if isinstance(obj, MPImagePopupMenu): - self.set_popup_menu(obj.menu) - if isinstance(obj, MPImageBrightness): - state.brightness = obj.brightness - self.need_redraw = True - if isinstance(obj, MPImageFullSize): - self.full_size() - if isinstance(obj, MPImageFitToWindow): - self.fit_to_window() - if isinstance(obj, win_layout.WinLayout): - win_layout.set_wx_window_layout(state.frame, obj) - if isinstance(obj, MPImageGStreamer): - self.start_gstreamer(obj.pipeline) - if isinstance(obj, MPImageVideo): - self.start_video(obj.filename) - if isinstance(obj, MPImageFPSMax): - self.fps_max = obj.fps_max - print("FPS_MAX: ", self.fps_max) - if isinstance(obj, MPImageSeekPercent): - self.seek_video(obj.percent) - if isinstance(obj, MPImageSeekFrame): - self.seek_video_frame(obj.frame) - if isinstance(obj, MPImageColormap): - self.colormap = obj.colormap - if isinstance(obj, MPImageColormapIndex): - self.colormap_index = obj.colormap_index - if isinstance(obj, MPImageStartTracker): - self.start_tracker(obj) - if isinstance(obj, MPImageEndTracker): - self.tracker = None + + self.process_event(obj) if self.need_redraw: self.redraw() From 8ebb9a4757dff967e247866811790a2bda5e1f0f Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 8 Oct 2024 19:22:40 +0100 Subject: [PATCH 06/28] 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): """ From 3432d622704f693681c27a28924be85c32a4456b Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 10 Oct 2024 10:32:56 +0100 Subject: [PATCH 07/28] camtrack: camera and gimbal component ids should match autopilot - camera_view: add variable for tracking image status update rate - camera_view: camera and gimbal component ids should match autopilot Signed-off-by: Rhys Mainwaring --- .../modules/mavproxy_camtrack/__init__.py | 31 ++++++------------- .../modules/mavproxy_camtrack/camera_view.py | 24 +++++--------- 2 files changed, 17 insertions(+), 38 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/__init__.py b/MAVProxy/modules/mavproxy_camtrack/__init__.py index 3333bc488f..264a71e624 100644 --- a/MAVProxy/modules/mavproxy_camtrack/__init__.py +++ b/MAVProxy/modules/mavproxy_camtrack/__init__.py @@ -43,7 +43,7 @@ def __init__(self, mpstate): self.mpstate = mpstate - # GUI + # Settings # TODO: provide args to set RTSP server location # localhost simulation rtsp_url = "rtsp://127.0.0.1:8554/camera" @@ -57,6 +57,9 @@ def __init__(self, mpstate): # SIYI A8 camera # rtsp_url = "rtsp://192.168.144.25:8554/main.264" + self._camera_tracking_image_status_rate = 50 # Hz + + # GUI self.camera_view = CameraView(self.mpstate, "Camera Tracking", rtsp_url) # NOTE: unused except for status @@ -76,12 +79,6 @@ def __init__(self, mpstate): self._do_request_camera_information = True self._do_request_camera_tracking_image_status = True - # control update rate to GUI - self._msg_list = [] - self._fps = 30.0 - self._last_send = 0.0 - self._send_delay = (1.0 / self._fps) * 0.9 - # commands self.add_command("camtrack", self.cmd_camtrack, "camera tracking") @@ -155,7 +152,7 @@ def mavlink_packet(self, msg): elif mtype == "CAMERA_TRACKING_IMAGE_STATUS": self.handle_camera_tracking_image_status(msg) - # TODO: NOTE: disabled + # TODO: NOTE: ack checks are disabled # check command_ack elif False: # mtype == "COMMAND_ACK": if msg.command == mavutil.mavlink.MAV_CMD_CAMERA_TRACK_POINT: @@ -215,7 +212,7 @@ def mavlink_packet(self, msg): elif msg.command == mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL: print("Got COMMAND_ACK: MAV_CMD_SET_MESSAGE_INTERVAL") - # TODO: NOTE: disabled + # TODO: NOTE: command processing disabled # check command_long elif False: # mtype == "COMMAND_LONG": # TODO: check target_system is for offboard control @@ -293,15 +290,7 @@ def check_events(self): # self.needs_unloading = True def send_messages(self): - """Send message list via pipe to GUI at desired update rate""" - if (time.time() - self._last_send) > self._send_delay: - # pipe data to GUI - # TODO: check interface in view for pipe updates - # self.camera_view.parent_pipe_send.send(self._msg_list) - # reset counters etc - self._msg_list = [] - self._last_send = time.time() - + """Send messages""" # TODO: implement camera and gimbal discovery correctly # Discovery - most of these requests are handled in the FC # by GCS_MAVLINK::try_send_message @@ -335,9 +324,10 @@ def send_messages(self): self._do_request_camera_information = False if self._do_request_camera_tracking_image_status: + interval_us = int(1e6 / self._camera_tracking_image_status_rate) self.send_set_message_interval_message( mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, - 1000 * 50, # 20Hz + interval_us, # requested interval in microseconds response_target=1, # flight-stack default ) self._do_request_camera_tracking_image_status = False @@ -402,12 +392,9 @@ def idle_task(self): def unload(self): """Close the GUI and unload module""" - - # close the GUI self.camera_view.close() def init(mpstate): """Initialise module""" - return CamTrackModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_camtrack/camera_view.py b/MAVProxy/modules/mavproxy_camtrack/camera_view.py index fef377c977..802e780471 100644 --- a/MAVProxy/modules/mavproxy_camtrack/camera_view.py +++ b/MAVProxy/modules/mavproxy_camtrack/camera_view.py @@ -32,16 +32,15 @@ def __init__(self, mpstate, title, rtsp_url, fps=30): self.frame_counter = -1 - # TODO: gimbal and camera system ids + # Commands to autopilot attached cameras and gimbals are addressed + # to the autopilot component. + self.camera_sysid = 1 # system id matches vehicle + self.camera_cmpid = 1 # component id matches autopilot + self.camera_deviceid = 1 # first flight-stack connected camera - # autopilot component may proxy up to 6 cameras - self.camera_sysid = 1 - self.camera_cmpid = mavutil.mavlink.MAV_COMP_ID_ONBOARD_COMPUTER - self.camera_deviceid = 1 # first autopilot attached camera - - self.gimbal_sysid = 1 - self.gimbal_cmpid = mavutil.mavlink.MAV_COMP_ID_ONBOARD_COMPUTER - self.gimbal_deviceid = 1 # first autopilot attached gimbal + self.gimbal_sysid = 1 # system id matches vehicle + self.gimbal_cmpid = 1 # component id matches autopilot + self.gimbal_deviceid = 1 # first flight-stack connected gimbal self.im = TrackerImage( title=title, @@ -75,13 +74,6 @@ def set_tracked_rectangle(self, 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 - # trigger a close event which is monitored by the - # child gui process - it will close allowing the - # process to be joined - # self.im.close_event.set() - # if self.im.is_alive(): - # self.im.child.join(timeout=2.0) self.im.terminate() def is_alive(self): From 01f58466a4544c865c0b2f8d615f6bc78ee4f5bc Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 10 Oct 2024 10:40:20 +0100 Subject: [PATCH 08/28] camtrack: request gimbal device attitude status - onboard_controller: request gimbal device attitude status - onboard_controller: send image status at 20 Hz - onboard_controller: adjust default controller gains Signed-off-by: Rhys Mainwaring --- .../mavproxy_camtrack/onboard_controller.py | 61 ++++++++++++++----- 1 file changed, 45 insertions(+), 16 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 7f1829a658..0a13966c57 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -18,7 +18,7 @@ --rtsp-server rtsp://192.168.1.204:8554/fpv_stream 3. Run controller on RPi4 companion computer using an RTSP stream from -a SIYI A8 camea. +a SIYI A8 camera. python ./onboard_controller.py --master 192.168.144.171:15001 @@ -133,7 +133,7 @@ def __init__(self, device, sysid, cmpid, rtsp_url): # mavlink connection self._connection = None - # list of controllers to forward mavlink message to + # list of controllers to forward mavlink messages self._controllers = [] self._camera_controller = None self._gimbal_controller = None @@ -180,7 +180,7 @@ def _mavlink_recv_task(self): """ Task to receive a mavlink message and forwward to controllers. """ - update_rate = 1000.0 + update_rate = 1000.0 # Hz update_period = 1.0 / update_rate while True: @@ -248,10 +248,19 @@ def run(self): tracking_rect_changed = True # TODO: how to ensure consistency of frame updates with GCS? - fps = 50 - update_rate = fps + update_rate = 50.0 # Hz update_period = 1.0 / update_rate + # TODO: consolidate common code - also used in GUI + # request gimbal device attitude status + gimbal_device_attitude_status_rate = 50.0 # Hz + interval_us = int(1e6 / gimbal_device_attitude_status_rate) + self.send_set_message_interval_message( + mavutil.mavlink.MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, + interval_us, # requested interval in microseconds + response_target=1, # flight-stack default + ) + frame_count = 0 while True: start_time = time.time() @@ -323,6 +332,23 @@ def __compare_rect(rect1, rect2): sleep_time = max(0.0, update_period - elapsed_time) time.sleep(sleep_time) + def send_set_message_interval_message( + self, message_id, interval_us, response_target=1 + ): + self._connection.mav.command_long_send( + self._connection.target_system, # target_system + self._connection.target_component, # target_component + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # command + 0, # confirmation + message_id, # param1 + interval_us, # param2 + 0, # param3 + 0, # param4 + 0, # param5 + 0, # param6 + response_target, # param7 + ) + class VideoStream: """ @@ -490,8 +516,8 @@ class CameraTrackController: def __init__(self, connection): self._connection = connection - self._sysid = self._connection.source_system - self._cmpid = self._connection.source_component + self._sysid = self._connection.source_system # system id matches vehicle + self._cmpid = 1 # component id matches autopilot print( f"Camera Track Controller: " @@ -721,7 +747,7 @@ def _mavlink_task(self): sysid = self._sysid cmpid = self._cmpid - update_rate = 1000.0 + update_rate = 1000.0 # Hz update_period = 1.0 / update_rate while True: @@ -758,7 +784,7 @@ def _send_status_task(self): # TODO: stop sending image status when tracking stopped # TODO: set streaming rate using MAV_CMD_SET_MESSAGE_INTERVAL - update_rate = 2.0 + update_rate = 20.0 # Hz update_period = 1.0 / update_rate while True: @@ -780,8 +806,8 @@ class GimbalController: def __init__(self, connection): self._connection = connection - self._sysid = self._connection.source_system - self._cmpid = self._connection.source_component + self._sysid = self._connection.source_system # system id matches vehicle + self._cmpid = 1 # component id matches autopilot print(f"Gimbal Controller: src_sys: {self._sysid}, src_cmp: {self._cmpid})") @@ -805,9 +831,8 @@ def __init__(self, connection): self._gimbal_failure_flags = None # TODO: add options for PI controller gains - # PI controllers: SIYI A8: 0.1, Gazebo: 0.3 - self._pitch_controller = PI_controller(Pgain=0.5, Igain=0.01, IMAX=1.0) - self._yaw_controller = PI_controller(Pgain=0.5, Igain=0.01, IMAX=1.0) + self._pitch_controller = PI_controller(Pgain=0.1, Igain=0.01, IMAX=0.1) + self._yaw_controller = PI_controller(Pgain=0.1, Igain=0.1, IMAX=1.0) # Start the control thread self._control_in_queue = queue.Queue() @@ -886,6 +911,9 @@ def _control_task(self): # the current mount orientation. _, ay, az = euler.quat2euler(gimbal_orientation) + self._pitch_controller.gain_mul = 4.0 + self._yaw_controller.gain_mul = 40.0 + err_pitch = self._neutral_y - ay pitch_rate_rads = self._pitch_controller.run(err_pitch) @@ -909,6 +937,9 @@ def _control_task(self): err_x = act_x - tgt_x err_y = -(act_y - tgt_y) + self._pitch_controller.gain_mul = 1.0 + self._yaw_controller.gain_mul = 2.0 + err_pitch = math.radians(err_y) pitch_rate_rads = self._pitch_controller.run(err_pitch) @@ -942,8 +973,6 @@ def __process_message(): return msg = self._control_in_queue.get() mtype = msg.get_type() - # NOTE: GIMBAL_DEVICE_ATTITUDE_STATUS is broadcast - # (sysid=0, cmpid=0) if msg and mtype == "GIMBAL_DEVICE_ATTITUDE_STATUS": with self._mavlink_lock: self._gimbal_device_flags = msg.flags From f134a8a9b2169977f9ff3ee81cfa848da4fff293 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 10 Oct 2024 13:03:17 +0100 Subject: [PATCH 09/28] camtrack: tracker_image: correct setting of tracked image position Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/mavproxy_camtrack/tracker_image.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/tracker_image.py b/MAVProxy/modules/mavproxy_camtrack/tracker_image.py index e78e7de2e1..84e0a5f8c9 100644 --- a/MAVProxy/modules/mavproxy_camtrack/tracker_image.py +++ b/MAVProxy/modules/mavproxy_camtrack/tracker_image.py @@ -136,7 +136,7 @@ def process_event(self, obj): TrackerPos( obj.top_left_x * width, obj.bot_right_x * width, - obj.top_left_x * height, + obj.top_left_y * height, obj.bot_right_y * height, ) ) From feab2000cbda670f51bf75d360803e5552dedc20 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 10 Oct 2024 19:31:59 +0100 Subject: [PATCH 10/28] camtrack: add settings and pid tuning tools - Intial tune of status rates and pid gains - Reduce image status rate to 20 Hz - Set response_target in set message interval to flight-stack default - Add debug info for image status / tracker_pos - onboard_controller: apply formatter - onboard_controller: adjust controller gains - Port basic PID controller from ArduPilot AC_PID - onboard_controller: replace PID controllers - onboard_controller: use normalised coords for tracking - onboard_controller: increase k_i for tracking controllers - onboard_controller: add optional live graphs for pid analysis - onboard_controller: close connection on exit - Add settings and defaults Signed-off-by: Rhys Mainwaring --- .../modules/mavproxy_camtrack/__init__.py | 97 ++++-- .../mavproxy_camtrack/onboard_controller.py | 269 ++++++++++----- .../modules/mavproxy_camtrack/pid_basic.py | 317 ++++++++++++++++++ .../mavproxy_camtrack/tracker_image.py | 7 + 4 files changed, 569 insertions(+), 121 deletions(-) create mode 100644 MAVProxy/modules/mavproxy_camtrack/pid_basic.py diff --git a/MAVProxy/modules/mavproxy_camtrack/__init__.py b/MAVProxy/modules/mavproxy_camtrack/__init__.py index 264a71e624..84e41f5f24 100644 --- a/MAVProxy/modules/mavproxy_camtrack/__init__.py +++ b/MAVProxy/modules/mavproxy_camtrack/__init__.py @@ -1,6 +1,22 @@ """ MAVProxy camera tracking module +Examples +--------- + +localhost simulation +rtsp_url = "rtsp://127.0.0.1:8554/camera" + +home wifi +rtsp_url = "rtsp://192.168.1.204:8554/fpv_stream" + +herelink wifi access point +rtsp_url = "rtsp://192.168.43.1:8554/fpv_stream" + +SIYI A8 camera +rtsp_url = "rtsp://192.168.144.25:8554/main.264" + + Reference --------- @@ -36,6 +52,10 @@ class CamTrackModule(mp_module.MPModule): """A tool to control camera tracking""" + # Module defaults + DEFAULT_RTSP_URL = "rtsp://127.0.0.1:8554/camera" + DEFAULT_IMAGE_STATUS_RATE = 20.0 + def __init__(self, mpstate): super(CamTrackModule, self).__init__( mpstate, "camtrack", "camera tracking module" @@ -44,23 +64,22 @@ def __init__(self, mpstate): self.mpstate = mpstate # Settings - # TODO: provide args to set RTSP server location - # localhost simulation - rtsp_url = "rtsp://127.0.0.1:8554/camera" - - # home wifi - # rtsp_url = "rtsp://192.168.1.204:8554/fpv_stream" - - # herelink wifi access point - # rtsp_url = "rtsp://192.168.43.1:8554/fpv_stream" + self.camtrack_settings = mp_settings.MPSettings( + [ + ("rtsp_url", str, CamTrackModule.DEFAULT_RTSP_URL), + ("image_status_rate", float, CamTrackModule.DEFAULT_IMAGE_STATUS_RATE), + ] + ) - # SIYI A8 camera - # rtsp_url = "rtsp://192.168.144.25:8554/main.264" + # Commands + self.add_command("camtrack", self.cmd_camtrack, "camera tracking") - self._camera_tracking_image_status_rate = 50 # Hz + self._camera_tracking_image_status_rate = ( + self.camtrack_settings.image_status_rate + ) # GUI - self.camera_view = CameraView(self.mpstate, "Camera Tracking", rtsp_url) + self.camera_view = None # NOTE: unused except for status # mavlink messages @@ -79,32 +98,36 @@ def __init__(self, mpstate): self._do_request_camera_information = True self._do_request_camera_tracking_image_status = True - # commands - self.add_command("camtrack", self.cmd_camtrack, "camera tracking") - def cmd_camtrack(self, args): - """Control behaviour of commands""" + """Command parser""" if len(args) <= 0: print(self.usage()) return - - if args[0] == "status": + if args[0] == "set": + if len(args) < 3: + self.camtrack_settings.show_all() + return + self.camtrack_settings.command(args[1:]) + elif args[0] == "status": print(self.status()) - return - - if args[0] == "start": - print("start tracking") - return + elif args[0] == "view": + self.cmd_view() + else: + print(self.usage()) - if args[0] == "stop": - print("stop tracking") + def cmd_view(self): + """Open camera view""" + if self.camera_view is not None: + print("camtrack view already open") return - print(self.usage()) + self.camera_view = CameraView( + self.mpstate, "Camera Tracking", self.camtrack_settings.rtsp_url + ) def usage(self): """Show help on command line options.""" - return "Usage: camtrack " + return "Usage: camtrack " def status(self): """Return information about the camera tracking state""" @@ -272,7 +295,9 @@ def handle_camera_information(self, 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 + self.camera_view is not None + and 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 ): @@ -282,7 +307,8 @@ def handle_camera_tracking_image_status(self, msg): def check_events(self): """Check for events on the camera view""" - self.camera_view.check_events() + if self.camera_view is not None: + self.camera_view.check_events() # TODO: check which shutdown events are available in MPImage # tell mavproxy to unload the module if the GUI is closed @@ -328,7 +354,7 @@ def send_messages(self): self.send_set_message_interval_message( mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, interval_us, # requested interval in microseconds - response_target=1, # flight-stack default + response_target=0, # flight-stack default ) self._do_request_camera_tracking_image_status = False @@ -369,9 +395,9 @@ def send_request_message(self, message_id, p1=0): ) def send_set_message_interval_message( - self, message_id, interval_us, response_target=1 + self, message_id, interval_us, response_target=0 ): - self.master.mav.command_long_send( + msg = self.master.mav.command_long_encode( self.settings.target_system, # target_system self.settings.target_component, # target_component mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, # command @@ -384,6 +410,7 @@ def send_set_message_interval_message( 0, # param6 response_target, # param7 ) + self.master.mav.send(msg) def idle_task(self): """Idle tasks""" @@ -392,7 +419,9 @@ def idle_task(self): def unload(self): """Close the GUI and unload module""" - self.camera_view.close() + if self.camera_view is not None: + self.camera_view.close() + self.camera_view = None def init(mpstate): diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 0a13966c57..7ffa113352 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -60,10 +60,15 @@ from enum import Enum from pymavlink import mavutil +# TODO: remember to add to requirements.txt / setup.py from transforms3d import euler from transforms3d import quaternions -from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import live_graph + +from MAVProxy.modules.mavproxy_camtrack.pid_basic import AC_PID_Basic +from MAVProxy.modules.mavproxy_camtrack.pid_basic import AP_PIDInfo + gi.require_version("Gst", "1.0") from gi.repository import Gst @@ -124,11 +129,12 @@ class CameraTrackingTargetData(Enum): class OnboardController: - def __init__(self, device, sysid, cmpid, rtsp_url): + def __init__(self, device, sysid, cmpid, rtsp_url, enable_graphs=False): self._device = device self._sysid = sysid self._cmpid = cmpid self._rtsp_url = rtsp_url + self._enable_graphs = enable_graphs # mavlink connection self._connection = None @@ -140,6 +146,9 @@ def __init__(self, device, sysid, cmpid, rtsp_url): print(f"Onboard Controller: src_sys: {self._sysid}, src_cmp: {self._cmpid})") + def __del__(self): + self._connection.close() + def _connect_to_mavlink(self): """ Establish a mavlink connection. @@ -228,7 +237,9 @@ def run(self): # Create and register controllers self._camera_controller = CameraTrackController(self._connection) - self._gimbal_controller = GimbalController(self._connection) + self._gimbal_controller = GimbalController( + self._connection, self._enable_graphs + ) self._controllers.append(self._camera_controller) self._controllers.append(self._gimbal_controller) @@ -697,6 +708,12 @@ def _send_camera_tracking_image_status(self): rec_bottom_x, rec_bottom_y, ) + # NOTE: leave for debugging + # print( + # f"image_status: x: {rec_top_x:.2f}, y: {rec_top_y:.2f}, " + # f"w: {(rec_bottom_x - rec_top_x):.2f}, " + # f"h: {(rec_bottom_y - rec_top_y):.2f}" + # ) self._connection.mav.send(msg) def _handle_camera_track_point(self, msg): @@ -804,9 +821,9 @@ class GimbalController: Gimbal controller for onboard camera tracking. """ - def __init__(self, connection): + def __init__(self, connection, enable_graphs=False): self._connection = connection - self._sysid = self._connection.source_system # system id matches vehicle + self._sysid = self._connection.source_system # system id matches vehicle self._cmpid = 1 # component id matches autopilot print(f"Gimbal Controller: src_sys: {self._sysid}, src_cmp: {self._cmpid})") @@ -830,9 +847,57 @@ def __init__(self, connection): self._gimbal_orientation = None self._gimbal_failure_flags = None - # TODO: add options for PI controller gains - self._pitch_controller = PI_controller(Pgain=0.1, Igain=0.01, IMAX=0.1) - self._yaw_controller = PI_controller(Pgain=0.1, Igain=0.1, IMAX=1.0) + # TODO: add options for PID controller gains + + # Pitch controller for centring gimbal + self._pit_controller = AC_PID_Basic( + initial_p=2.0, + initial_i=0.0, + initial_d=0.0, + initial_ff=0.0, + initial_imax=1.0, + initial_filt_E_hz=0.0, + initial_filt_D_hz=20.0, + ) + + # Yaw controller for centring gimbal + self._yaw_controller = AC_PID_Basic( + initial_p=2.0, + initial_i=0.0, + initial_d=0.0, + initial_ff=0.0, + initial_imax=1.0, + initial_filt_E_hz=0.0, + initial_filt_D_hz=20.0, + ) + + # Gimbal pitch controller for tracking + self._pit_track_controller = AC_PID_Basic( + initial_p=2.0, + initial_i=0.2, + initial_d=0.01, + initial_ff=0.0, + initial_imax=1.0, + initial_filt_E_hz=0.0, + initial_filt_D_hz=20.0, + ) + + # Gimbal yaw controller for tracking + self._yaw_track_controller = AC_PID_Basic( + initial_p=2.0, + initial_i=0.2, + initial_d=0.01, + initial_ff=0.0, + initial_imax=1.0, + initial_filt_E_hz=0.0, + initial_filt_D_hz=20.0, + ) + + # Analysis + self._enable_graphs = enable_graphs + self._graph_pid_tune = None + self._graph_pid_pit = None + self._graph_pid_yaw = None # Start the control thread self._control_in_queue = queue.Queue() @@ -858,8 +923,10 @@ def reset(self): self._tracking = False self._center_x = 0.0 self._center_y = 0.0 - self._pitch_controller.reset_I() - self._yaw_controller.reset_I() + self._pit_track_controller.reset_I() + self._pit_track_controller.reset_filter() + self._yaw_track_controller.reset_I() + self._yaw_track_controller.reset_filter() def mavlink_packet(self, msg): self._control_in_queue.put(msg) @@ -889,6 +956,10 @@ def _control_task(self): When not tracking, return the gimbal to its neutral position. """ + if self._enable_graphs: + self._add_livegraphs() + + last_time_s = time.time() while True: # Record the start time of the loop start_time = time.time() @@ -905,54 +976,61 @@ def _control_task(self): with self._mavlink_lock: gimbal_orientation = self._gimbal_orientation + # Update dt for the controller I and D terms + time_now_s = time.time() + dt = time_now_s - last_time_s + last_time_s = time_now_s + if not tracking and gimbal_orientation is not None: # NOTE: to centre the gimbal when not tracking, we need to know # the neutral angles from the MNT1_NEUTRAL_x params, and also # the current mount orientation. _, ay, az = euler.quat2euler(gimbal_orientation) - self._pitch_controller.gain_mul = 4.0 - self._yaw_controller.gain_mul = 40.0 + pit_rate_rads = self._pit_controller.update_all(self._neutral_y, ay, dt) + yaw_rate_rads = self._yaw_controller.update_all(self._neutral_z, az, dt) - err_pitch = self._neutral_y - ay - pitch_rate_rads = self._pitch_controller.run(err_pitch) - - err_yaw = self._neutral_z - az - yaw_rate_rads = self._yaw_controller.run(err_yaw) + pit_pid_info = self._pit_controller.pid_info + yaw_pid_info = self._yaw_controller.pid_info self._send_gimbal_manager_pitch_yaw_angles( float("nan"), float("nan"), - pitch_rate_rads, + pit_rate_rads, yaw_rate_rads, ) + + if self._enable_graphs: + self._update_livegraphs(pit_pid_info, yaw_pid_info) + elif frame_width is not None and frame_height is not None: - tgt_x = 0.5 * frame_width - tgt_y = 0.5 * frame_height + # work with normalised screen [-1, 1] + tgt_x = 0.0 + tgt_y = 0.0 if math.isclose(act_x, 0.0) and math.isclose(act_y, 0.0): - err_x = 0.0 - err_y = 0.0 + act_x = 0.0 + act_y = 0.0 else: - err_x = act_x - tgt_x - err_y = -(act_y - tgt_y) - - self._pitch_controller.gain_mul = 1.0 - self._yaw_controller.gain_mul = 2.0 + act_x = (act_x / frame_width - 0.5) * -1.0 + act_y = act_y / frame_height - 0.5 - err_pitch = math.radians(err_y) - pitch_rate_rads = self._pitch_controller.run(err_pitch) + pit_rate_rads = self._pit_track_controller.update_all(tgt_y, act_y, dt) + yaw_rate_rads = self._yaw_track_controller.update_all(tgt_x, act_x, dt) - err_yaw = math.radians(err_x) - yaw_rate_rads = self._yaw_controller.run(err_yaw) + pit_pid_info = self._pit_track_controller.pid_info + yaw_pid_info = self._yaw_track_controller.pid_info self._send_gimbal_manager_pitch_yaw_angles( float("nan"), float("nan"), - pitch_rate_rads, + pit_rate_rads, yaw_rate_rads, ) + if self._enable_graphs: + self._update_livegraphs(pit_pid_info, yaw_pid_info) + # Update at 50Hz update_period = 0.02 elapsed_time = time.time() - start_time @@ -985,47 +1063,61 @@ def __process_message(): sleep_time = max(0.0, update_period - elapsed_time) time.sleep(sleep_time) + def _add_livegraphs(self): + self._graph_pid_tune = live_graph.LiveGraph( + ["PID.PitTgt", "PID.PitAct", "PID.YawTgt", "PID.YawAct"], + timespan=30, + title="PID_TUNING", + ) + self._graph_pid_pit = live_graph.LiveGraph( + [ + "PID.Out", + "PID.P", + "PID.I", + "PID.D", + ], + timespan=30, + title="PID_TUNING: Pitch", + ) + self._graph_pid_yaw = live_graph.LiveGraph( + [ + "PID.Out", + "PID.P", + "PID.I", + "PID.D", + ], + timespan=30, + title="PID_TUNING: Yaw", + ) -class PI_controller: - """ - Simple PI controller - - MAVProxy/modules/mavproxy_SIYI/PI_controller (modified) - """ - - def __init__(self, Pgain, Igain, IMAX, gain_mul=1.0, max_rate=math.radians(30.0)): - self.Pgain = Pgain - self.Igain = Igain - self.IMAX = IMAX - self.gain_mul = gain_mul - self.max_rate = max_rate - self.I = 0.0 - - self.last_t = time.time() - - def run(self, err, ff_rate=0.0): - now = time.time() - dt = now - self.last_t - if now - self.last_t > 1.0: - self.reset_I() - dt = 0 - self.last_t = now - P = self.Pgain * self.gain_mul - I = self.Igain * self.gain_mul - IMAX = self.IMAX - max_rate = self.max_rate - - out = P * err - saturated = err > 0 and (out + self.I) >= max_rate - saturated |= err < 0 and (out + self.I) <= -max_rate - if not saturated: - self.I += I * err * dt - self.I = mp_util.constrain(self.I, -IMAX, IMAX) - ret = out + self.I + ff_rate - return mp_util.constrain(ret, -max_rate, max_rate) - - def reset_I(self): - self.I = 0 + def _update_livegraphs(self, pit_pid_info, yaw_pid_info): + if self._graph_pid_tune.is_alive(): + self._graph_pid_tune.add_values( + [ + pit_pid_info.target, + pit_pid_info.actual, + yaw_pid_info.target, + yaw_pid_info.actual, + ] + ) + if self._graph_pid_pit.is_alive(): + self._graph_pid_pit.add_values( + [ + pit_pid_info.out, + pit_pid_info.P, + pit_pid_info.I, + pit_pid_info.D, + ] + ) + if self._graph_pid_yaw.is_alive(): + self._graph_pid_yaw.add_values( + [ + yaw_pid_info.out, + yaw_pid_info.P, + yaw_pid_info.I, + yaw_pid_info.D, + ] + ) class TrackerCSTR: @@ -1073,34 +1165,37 @@ def set_normalised_roi(self, nroi): if __name__ == "__main__": import os import sys - from optparse import OptionParser + from argparse import ArgumentParser - parser = OptionParser("onboard_controller.py [options]") - parser.add_option("--master", default=None, type=str, help="MAVLink device") - parser.add_option("--rtsp-server", default=None, type=str, help="RTSP server URL") - parser.add_option("--sysid", default=1, type=int, help="Source system ID") - parser.add_option( + parser = ArgumentParser("onboard_controller.py [options]") + parser.add_argument("--master", required=True, type=str, help="MAVLink device") + parser.add_argument( + "--rtsp-server", required=True, type=str, help="RTSP server URL" + ) + parser.add_argument("--sysid", default=1, type=int, help="Source system ID") + parser.add_argument( "--cmpid", default=mavutil.mavlink.MAV_COMP_ID_ONBOARD_COMPUTER, type=int, help="Source component ID", ) - - (opts, args) = parser.parse_args() - if opts.master is None: - print("Must specify a MAVLink device") - sys.exit(1) - if opts.rtsp_server is None: - print("Must specify an RTSP server URL") - sys.exit(1) + parser.add_argument( + "--enable-graphs", + action="store_const", + default=False, + const=True, + help="Enable live graphs", + ) + args = parser.parse_args() controller = OnboardController( - opts.master, opts.sysid, opts.cmpid, opts.rtsp_server + args.master, args.sysid, args.cmpid, args.rtsp_server, args.enable_graphs ) while True: try: controller.run() except KeyboardInterrupt: + controller = None EXIT_CODE_CTRL_C = 130 sys.exit(EXIT_CODE_CTRL_C) diff --git a/MAVProxy/modules/mavproxy_camtrack/pid_basic.py b/MAVProxy/modules/mavproxy_camtrack/pid_basic.py new file mode 100644 index 0000000000..accc694379 --- /dev/null +++ b/MAVProxy/modules/mavproxy_camtrack/pid_basic.py @@ -0,0 +1,317 @@ +""" +Python port of the ArduPilot basic PID controller. + +AC_PID_Basic.h +Generic PID algorithm, with EEPROM-backed storage of constants. +""" + +import math +import sys + + +# fftype.h: L63 +def is_zero(value): + return math.fabs(value) < sys.float_info.epsilon + + +# AP_Math.h: L75 +def is_negative(value): + return value <= (-1.0 * sys.float_info.epsilon) + + +# AP_Math.h: L64 +def is_positive(value): + return value >= sys.float_info.epsilon + + +# AP_Math.cpp: L266 +def constrain_value_line(amt, low, high): + # the check for NaN as a float prevents propagation of floating point + # errors through any function that uses constrain_value(). The normal + # float semantics already handle -Inf and +Inf + if amt is float("nan"): + raise Exception("constraining nan") + return (low + high) / 2.0 + + if amt < low: + return low + + if amt > high: + return high + + return amt + + +# AP_Math.h: L173 +def constrain_float(amt, low, high): + return constrain_value_line(float(amt), float(low), float(high)) + + +# AP_Math.cpp: L400 +# calculate a low pass filter alpha value +def calc_lowpass_alpha_dt(dt, cutoff_freq): + if is_negative(dt) or is_negative(cutoff_freq): + raise Exception("invalid_arg_or_result") + return 1.0 + if is_zero(cutoff_freq): + return 1.0 + if is_zero(dt): + return 0.0 + rc = 1.0 / (2.0 * math.pi * cutoff_freq) + return dt / (dt + rc) + + +class AP_PIDInfo: + """ + Data used in PID controllers + + This structure provides information on the internal member data of + a PID. It provides an abstract way to pass PID information around, + useful for logging and sending mavlink messages. + """ + + def __init__(self): + self.target = float(0.0) + self.actual = float(0.0) + self.error = float(0.0) + self.out = float(0.0) + self.P = float(0.0) + self.I = float(0.0) + self.D = float(0.0) + self.FF = float(0.0) + self.DFF = float(0.0) + self.Dmod = float(0.0) + self.slew_rate = float(0.0) + self.limit = bool(False) + self.PD_limit = bool(False) + self.reset = bool(False) + self.I_term_set = bool(False) + + +class AC_PID_Basic: + """ + AC_PID_Basic + + Copter PID control class + """ + + def __init__( + self, + initial_p, + initial_i, + initial_d, + initial_ff, + initial_imax, + initial_filt_E_hz, + initial_filt_D_hz, + ): + """ + Constructor for PID + """ + # parameters + self._kp = initial_p + self._ki = initial_i + self._kd = initial_d + self._kff = initial_ff + self._kimax = initial_imax + self._filt_E_hz = initial_filt_E_hz # PID error filter frequency in Hz + self._filt_D_hz = initial_filt_D_hz # PID derivative filter frequency in Hz + + # internal variables + self._target = 0.0 # target value to enable filtering + self._error = 0.0 # error value to enable filtering + self._derivative = 0.0 # last derivative for low-pass filter + self._integrator = 0.0 # integrator value + + # true when input filter should be reset during next call to set_input + self._reset_filter = True + + self._pid_info = AP_PIDInfo() + + # set target and measured inputs to PID controller and calculate outputs + # target and error are filtered + # the derivative is then calculated and filtered + # the integral is then updated based on the setting of the limit flag + def update_all(self, target, measurement, dt, limit=False): + return self.update_all_1( + target, + measurement, + dt, + (limit and is_negative(self._integrator)), + (limit and is_positive(self._integrator)), + ) + + def update_all_1(self, target, measurement, dt, limit_neg, limit_pos): + """ + update_all - set target and measured inputs to PID controller and calculate outputs + target and error are filtered + the derivative is then calculated and filtered + the integral is then updated based on the setting of the limit flag + """ + # don't process inf or NaN + if ( + not math.isfinite(target) + or math.isnan(target) + or not math.isfinite(measurement) + or math.isnan(measurement) + ): + raise Exception("invalid_arg_or_result") + return 0.0 + + self._target = target + + # reset input filter to value received + if self._reset_filter: + self._reset_filter = False + self._error = self._target - measurement + self._derivative = 0.0 + else: + error_last = self._error + self._error += self.get_filt_E_alpha(dt) * ( + (self._target - measurement) - self._error + ) + + # calculate and filter derivative + if is_positive(dt): + derivative = (self._error - error_last) / dt + self._derivative += self.get_filt_D_alpha(dt) * ( + derivative - self._derivative + ) + + # update I term + self.update_i(dt, limit_neg, limit_pos) + + P_out = self._error * self._kp + D_out = self._derivative * self._kd + + self._pid_info.target = self._target + self._pid_info.actual = measurement + self._pid_info.error = self._error + self._pid_info.P = self._error * self._kp + self._pid_info.I = self._integrator + self._pid_info.D = self._derivative * self._kd + self._pid_info.FF = self._target * self._kff + self._pid_info.out = P_out + self._integrator + D_out + self._target * self._kff + + return self._pid_info.out + + def update_i(self, dt, limit_neg, limit_pos): + """ + update the integral + if the limit flags are set the integral is only allowed to shrink + """ + if not is_zero(self._ki): + # Ensure that integrator can only be reduced if the output is saturated + if not ( + (limit_neg and is_negative(self._error)) + or (limit_pos and is_positive(self._error)) + ): + self._integrator += (self._error * self._ki) * dt + self._integrator = constrain_float( + self._integrator, -self._kimax, self._kimax + ) + + else: + self._integrator = 0.0 + + # get results from pid controller + def get_p(self): + return self._error * _kp + + def get_i(self): + return self._integrator + + def get_d(self): + return self._derivative * _kd + + def get_ff(self): + return self._target * _kff + + def get_error(self): + return self._error + + # reset the integrator + def reset_I(self): + self._integrator = 0.0 + + # input and D term filter will be reset to the next value provided to set_input() + def reset_filter(self): + self._reset_filter = True + + # get accessors + @property + def kP(self): + return self._kp + + @property + def kI(self): + return self._ki + + @property + def kD(self): + return self._kd + + @property + def ff(self): + return self._kff + + @property + def filt_E_hz(self): + return self._filt_E_hz + + @property + def filt_D_hz(self): + return self._filt_D_hz + + @property + def imax(self): + return self._kimax + + def get_filt_E_alpha(self, dt): + return calc_lowpass_alpha_dt(dt, self._filt_E_hz) + + def get_filt_D_alpha(self, dt): + return calc_lowpass_alpha_dt(dt, self._filt_D_hz) + + # set accessors + @kP.setter + def kP(self, value): + self._kp = value + + @kI.setter + def kI(self, value): + self._ki = value + + @kD.setter + def kD(self, value): + self._kd = value + + @ff.setter + def ff(self, value): + self._kff = value + + @imax.setter + def imax(self, value): + self._kimax = math.fabs(value) + + @filt_E_hz.setter + def filt_E_hz(self, hz): + self._filt_E_hz = math.fabs(hz) + + @filt_D_hz.setter + def filt_D_hz(self, hz): + self._filt_D_hz = math.fabs(hz) + + # integrator setting functions + def set_integrator_2(self, target, measurement, i): + self.set_integrator_1(target - measurement, i) + + def set_integrator_1(self, error, i): + self.set_integrator(i - error * self._kp) + + def set_integrator(self, i): + self._integrator = constrain_float(i, -self._kimax, self._kimax) + + @property + def pid_info(self): + return self._pid_info diff --git a/MAVProxy/modules/mavproxy_camtrack/tracker_image.py b/MAVProxy/modules/mavproxy_camtrack/tracker_image.py index 84e0a5f8c9..42a221ecc5 100644 --- a/MAVProxy/modules/mavproxy_camtrack/tracker_image.py +++ b/MAVProxy/modules/mavproxy_camtrack/tracker_image.py @@ -279,6 +279,13 @@ def get_position(self): def set_position(self, tracker_pos): self._tracker_pos = tracker_pos + # NOTE: leave for debugging + # print( + # f"tracker_pos: x: {self._tracker_pos.left():.2f}, " + # f"y: {self._tracker_pos.top():.2f}, " + # f"w: {(self._tracker_pos.right() - self._tracker_pos.left()):.2f}, " + # f"h: {(self._tracker_pos.bottom() - self._tracker_pos.top()):.2f}" + # ) def create_tracker(name, state): From 6f06e355a3ad2c99a43dd93056bfefc8264a7239 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 15 Oct 2024 12:41:22 +0100 Subject: [PATCH 11/28] camtrack: onboard_controller: add option for tracker algo - onboard_controller: move pid_basic to lib - Prevent transitive import of dlib and wx - onboard_controller: check connection valid before close - onboard_controller: add tracker option Signed-off-by: Rhys Mainwaring --- .../{mavproxy_camtrack => lib}/pid_basic.py | 0 .../mavproxy_camtrack/onboard_controller.py | 149 +++++++++++++----- 2 files changed, 110 insertions(+), 39 deletions(-) rename MAVProxy/modules/{mavproxy_camtrack => lib}/pid_basic.py (100%) diff --git a/MAVProxy/modules/mavproxy_camtrack/pid_basic.py b/MAVProxy/modules/lib/pid_basic.py similarity index 100% rename from MAVProxy/modules/mavproxy_camtrack/pid_basic.py rename to MAVProxy/modules/lib/pid_basic.py diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 7ffa113352..efaeceaaec 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -66,8 +66,8 @@ from MAVProxy.modules.lib import live_graph -from MAVProxy.modules.mavproxy_camtrack.pid_basic import AC_PID_Basic -from MAVProxy.modules.mavproxy_camtrack.pid_basic import AP_PIDInfo +from MAVProxy.modules.lib.pid_basic import AC_PID_Basic +from MAVProxy.modules.lib.pid_basic import AP_PIDInfo gi.require_version("Gst", "1.0") @@ -129,11 +129,14 @@ class CameraTrackingTargetData(Enum): class OnboardController: - def __init__(self, device, sysid, cmpid, rtsp_url, enable_graphs=False): + def __init__( + self, device, sysid, cmpid, rtsp_url, tracker_name="CSTR", enable_graphs=False + ): self._device = device self._sysid = sysid self._cmpid = cmpid self._rtsp_url = rtsp_url + self._tracker_name = tracker_name self._enable_graphs = enable_graphs # mavlink connection @@ -147,7 +150,8 @@ def __init__(self, device, sysid, cmpid, rtsp_url, enable_graphs=False): print(f"Onboard Controller: src_sys: {self._sysid}, src_cmp: {self._cmpid})") def __del__(self): - self._connection.close() + if self._connection is not None: + self._connection.close() def _connect_to_mavlink(self): """ @@ -211,6 +215,14 @@ def __process_message(): sleep_time = max(0.0, update_period - elapsed_time) time.sleep(sleep_time) + def _create_tracker(self, name): + if name == "CSTR": + return TrackerCSTR() + elif name == "KCF": + return TrackerKCF() + else: + raise Exception(f"Invalid tracker name: {name}") + def run(self): """ Run the onboard controller. @@ -221,6 +233,7 @@ def run(self): self._connect_to_mavlink() # Connect to video stream + print(f"Using RTSP server: {self._rtsp_url}") video_stream = VideoStream(self._rtsp_url) # Timeout if video not available @@ -254,7 +267,8 @@ def run(self): mavlink_recv_thread.start() # Create tracker - tracker = TrackerCSTR() + print(f"Using tracker: {self._tracker_name}") + tracker = self._create_tracker(self._tracker_name) tracking_rect = None tracking_rect_changed = True @@ -336,6 +350,7 @@ def __compare_rect(rect1, rect2): else: print("Tracking failure detected.") tracking_rect = None + self._camera_controller.stop_tracking() self._gimbal_controller.reset() # Rate limit @@ -606,6 +621,15 @@ def set_curr_track_rectangle(self, rect): with self._lock: self._curr_track_rect = copy.deepcopy(rect) + def stop_tracking(self): + 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_packet(self, msg): """ Process a mavlink packet. @@ -746,13 +770,7 @@ def _handle_camera_track_rectangle(self, msg): ) def _handle_camera_stop_tracking(self, msg): - 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 + self.stop_tracking() def _mavlink_task(self): """ @@ -832,17 +850,17 @@ def __init__(self, connection, enable_graphs=False): self._control_lock = threading.Lock() self._center_x = 0 self._center_y = 0 - self._width = None - self._height = None + self._frame_width = None + self._frame_height = None self._tracking = False # Gimbal state # TODO: obtain the mount neutral angles from params # NOTE: q = [w, x, y, z] self._mavlink_lock = threading.Lock() - self._neutral_x = 0.0 - self._neutral_y = 0.0 - self._neutral_z = 0.0 + self._mnt1_neutral_x = 0.0 # roll + self._mnt1_neutral_y = 0.0 # pitch + self._mnt1_neutral_z = 0.0 # yaw self._gimbal_device_flags = None self._gimbal_orientation = None self._gimbal_failure_flags = None @@ -916,7 +934,7 @@ def update_center(self, x, y, shape): self._tracking = True self._center_x = x self._center_y = y - self._height, self._width, _ = shape + self._frame_height, self._frame_width, _ = shape def reset(self): with self._control_lock: @@ -968,12 +986,14 @@ def _control_task(self): with self._control_lock: act_x = self._center_x act_y = self._center_y - frame_width = self._width - frame_height = self._height + frame_width = self._frame_width + frame_height = self._frame_height tracking = self._tracking # Return gimbal to its neutral orientation when not tracking with self._mavlink_lock: + pit_tgt_rad = self._mnt1_neutral_y + yaw_tgt_rad = self._mnt1_neutral_z gimbal_orientation = self._gimbal_orientation # Update dt for the controller I and D terms @@ -985,14 +1005,32 @@ def _control_task(self): # NOTE: to centre the gimbal when not tracking, we need to know # the neutral angles from the MNT1_NEUTRAL_x params, and also # the current mount orientation. - _, ay, az = euler.quat2euler(gimbal_orientation) + _, pit_act_rad, yaw_act_rad = euler.quat2euler(gimbal_orientation) - pit_rate_rads = self._pit_controller.update_all(self._neutral_y, ay, dt) - yaw_rate_rads = self._yaw_controller.update_all(self._neutral_z, az, dt) + pit_rate_rads = self._pit_controller.update_all( + pit_tgt_rad, pit_act_rad, dt + ) + yaw_rate_rads = self._yaw_controller.update_all( + yaw_tgt_rad, yaw_act_rad, dt + ) pit_pid_info = self._pit_controller.pid_info yaw_pid_info = self._yaw_controller.pid_info + # NOTE: leave for debugging + # print( + # f"Pit: Tgt: {pit_pid_info.target:.2f}, " + # f"Act: {pit_pid_info.actual:.2f}, " + # f"Out: {pit_pid_info.out:.2f}" + # ) + + # NOTE: leave for debugging + # print( + # f"Yaw: Tgt: {yaw_pid_info.target:.2f}, " + # f"Act: {yaw_pid_info.actual:.2f}, " + # f"Out: {yaw_pid_info.out:.2f}" + # ) + self._send_gimbal_manager_pitch_yaw_angles( float("nan"), float("nan"), @@ -1120,13 +1158,13 @@ def _update_livegraphs(self, pit_pid_info, yaw_pid_info): ) -class TrackerCSTR: +class TrackerOpenCV: """ - Wrapper for cv2.legacy.TrackerCSRT + Base class for wrappers of OpenCV trackers """ def __init__(self): - self._tracker = cv2.legacy.TrackerCSRT_create() + self._tracker = self._create() self._nroi = None self._nroi_changed = False @@ -1135,17 +1173,17 @@ def update(self, frame): return False, None if self._nroi_changed: - self._tracker = cv2.legacy.TrackerCSRT_create() + self._tracker = self._create() # Denormalise the roi - height, width, _ = frame.shape + frame_height, frame_width, _ = frame.shape roi = [ - int(self._nroi[0] * width), # x - int(self._nroi[1] * height), # y - int(self._nroi[2] * width), # w - int(self._nroi[3] * height), # h + int(self._nroi[0] * frame_width), # x + int(self._nroi[1] * frame_height), # y + int(self._nroi[2] * frame_width), # w + int(self._nroi[3] * frame_height), # h ] # print( - # f"TrackerCSTR: nroi: {self._nroi}, roi: {roi}, frame_width: {width}, frame_height: {height}" + # f"Tracker: nroi: {self._nroi}, roi: {roi}, frame_width: {frame_width}, frame_height: {frame_height}" # ) self._tracker.init(frame, roi) self._nroi_changed = False @@ -1161,6 +1199,33 @@ def set_normalised_roi(self, nroi): self._nroi = nroi self._nroi_changed = True + def _create(self): + raise Exception("TrackerOpenCV must not be used directly.") + + +class TrackerCSTR(TrackerOpenCV): + """ + Wrapper for cv2.legacy.TrackerCSRT + """ + + def __init__(self): + super().__init__() + + def _create(self): + return cv2.legacy.TrackerCSRT_create() + + +class TrackerKCF(TrackerOpenCV): + """ + Wrapper for cv2.legacy.TrackerKCF + """ + + def __init__(self): + super().__init__() + + def _create(self): + return cv2.legacy.TrackerKCF_create() + if __name__ == "__main__": import os @@ -1168,28 +1233,34 @@ def set_normalised_roi(self, nroi): from argparse import ArgumentParser parser = ArgumentParser("onboard_controller.py [options]") - parser.add_argument("--master", required=True, type=str, help="MAVLink device") + parser.add_argument("--master", required=True, type=str, help="mavlink device") parser.add_argument( - "--rtsp-server", required=True, type=str, help="RTSP server URL" + "--rtsp-server", required=True, type=str, help="rtsp server url" ) - parser.add_argument("--sysid", default=1, type=int, help="Source system ID") + parser.add_argument("--sysid", default=1, type=int, help="source system id") parser.add_argument( "--cmpid", default=mavutil.mavlink.MAV_COMP_ID_ONBOARD_COMPUTER, type=int, - help="Source component ID", + help="source component id", ) + parser.add_argument("--tracker-name", default="CSTR", type=str, help="tracker name") parser.add_argument( "--enable-graphs", action="store_const", default=False, const=True, - help="Enable live graphs", + help="enable live graphs", ) args = parser.parse_args() controller = OnboardController( - args.master, args.sysid, args.cmpid, args.rtsp_server, args.enable_graphs + args.master, + args.sysid, + args.cmpid, + args.rtsp_server, + args.tracker_name, + args.enable_graphs, ) while True: From d9587c1522bb979e6f7fcc3d8464bd1a0064dff3 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 16 Oct 2024 10:42:25 +0100 Subject: [PATCH 12/28] camtrack: onboard_controller: add profiling to main loop Signed-off-by: Rhys Mainwaring --- .../mavproxy_camtrack/onboard_controller.py | 122 +++++++++++++++++- 1 file changed, 119 insertions(+), 3 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index efaeceaaec..7df1a61cbc 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -128,9 +128,74 @@ class CameraTrackingTargetData(Enum): IN_STATUS = mavutil.mavlink.CAMERA_TRACKING_TARGET_DATA_IN_STATUS +class Profiler: + """ + Basic profiler for measuring execution times. + """ + + def __init__(self): + self._has_started = False + self._has_ended = True + self._start_time_s = 0 + + self._count = 0 + self._last_duration_s = -1.0 + self._total_duration_s = 0.0 + + def start(self): + if self._has_started or not self._has_ended: + raise Exception(f"Profiler already in progress") + return + + self._has_started = True + self._has_ended = False + self._start_time_s = time.time() + + def end(self): + if not self._has_started or self._has_ended: + raise Exception(f"Profiler cannot end before start") + return + + self._has_started = False + self._has_ended = True + dt = time.time() - self._start_time_s + self._count += 1 + self._last_duration_s = dt + self._total_duration_s += dt + + @property + def count(self): + return self._count + + @property + def last_duration_ms(self): + if self._last_duration_s >= 0: + return int(self._last_duration_s * 1e3) + else: + return int(-1) + + @property + def avg_duration_ms(self): + if self._count > 0: + return int(self._total_duration_s / self._count * 1e3) + else: + return int(-1) + + class OnboardController: + """ + Onboard gimbal controller. + """ + def __init__( - self, device, sysid, cmpid, rtsp_url, tracker_name="CSTR", enable_graphs=False + self, + device, + sysid, + cmpid, + rtsp_url, + tracker_name="CSTR", + enable_graphs=False, + enable_profiler=False, ): self._device = device self._sysid = sysid @@ -138,6 +203,7 @@ def __init__( self._rtsp_url = rtsp_url self._tracker_name = tracker_name self._enable_graphs = enable_graphs + self._enable_profiler = enable_profiler # mavlink connection self._connection = None @@ -286,12 +352,18 @@ def run(self): response_target=1, # flight-stack default ) - frame_count = 0 + # Always collect profile data (low-cost) + tracker_profiler = Profiler() + loop_profiler = Profiler() + loop_with_tracker_profiler = Profiler() + profile_print_last_time = time.time() + profile_print_period = 2 + while True: start_time = time.time() if video_stream.frame_available(): - frame_count += 1 + loop_profiler.start() frame = copy.deepcopy(video_stream.frame()) track_type = self._camera_controller.track_type() @@ -329,7 +401,11 @@ def __compare_rect(rect1, rect2): # update tracker and gimbal if tracking active if tracking_rect is not None: + loop_with_tracker_profiler.start() + tracker_profiler.start() success, box = tracker.update(frame) + tracker_profiler.end() + if success: (x, y, w, h) = [int(v) for v in box] u = x + w // 2 @@ -352,6 +428,38 @@ def __compare_rect(rect1, rect2): tracking_rect = None self._camera_controller.stop_tracking() self._gimbal_controller.reset() + loop_with_tracker_profiler.end() + + loop_profiler.end() + + # Profile + if self._enable_profiler and ( + (time.time() - profile_print_last_time) > profile_print_period + ): + profile_print_last_time = time.time() + print("Onboard controller main loop profile") + print("-------------------------------------") + print("time in ms avg\tlast\tcount") + print( + f"loop: " + f"{loop_profiler.avg_duration_ms}\t" + f"{loop_profiler.last_duration_ms}\t" + f"{loop_profiler.count}" + ) + print( + f"loop_with_track: " + f"{loop_with_tracker_profiler.avg_duration_ms}\t" + f"{loop_with_tracker_profiler.last_duration_ms}\t" + f"{loop_with_tracker_profiler.count}" + ) + print( + f"tracker: " + f"{tracker_profiler.avg_duration_ms}\t" + f"{tracker_profiler.last_duration_ms}\t" + f"{tracker_profiler.count}" + ) + print("-------------------------------------") + print() # Rate limit elapsed_time = time.time() - start_time @@ -1252,6 +1360,13 @@ def _create(self): const=True, help="enable live graphs", ) + parser.add_argument( + "--enable-profiler", + action="store_const", + default=False, + const=True, + help="enable main loop profiler", + ) args = parser.parse_args() controller = OnboardController( @@ -1261,6 +1376,7 @@ def _create(self): args.rtsp_server, args.tracker_name, args.enable_graphs, + args.enable_profiler, ) while True: From fd056173bd38458771ff97c9cb63ea6748346274 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 16 Oct 2024 16:42:34 +0100 Subject: [PATCH 13/28] camtrack: onboard_controller: add globals for update rates Signed-off-by: Rhys Mainwaring --- .../mavproxy_camtrack/onboard_controller.py | 115 +++++++++++------- 1 file changed, 73 insertions(+), 42 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 7df1a61cbc..1131d1c917 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -73,6 +73,12 @@ gi.require_version("Gst", "1.0") from gi.repository import Gst +# TODO: move update rate constants to CLI options +MAIN_LOOP_RATE = 30.0 +GIMBAL_CONTROL_LOOP_RATE = 30.0 +CAMERA_SEND_IMAGE_STATUS_RATE = 5.0 +MAVLINK_RECV_RATE = 1000.0 + class CameraCapFlags(Enum): """ @@ -259,7 +265,7 @@ def _mavlink_recv_task(self): """ Task to receive a mavlink message and forwward to controllers. """ - update_rate = 1000.0 # Hz + update_rate = MAVLINK_RECV_RATE # Hz update_period = 1.0 / update_rate while True: @@ -281,14 +287,6 @@ def __process_message(): sleep_time = max(0.0, update_period - elapsed_time) time.sleep(sleep_time) - def _create_tracker(self, name): - if name == "CSTR": - return TrackerCSTR() - elif name == "KCF": - return TrackerKCF() - else: - raise Exception(f"Invalid tracker name: {name}") - def run(self): """ Run the onboard controller. @@ -334,12 +332,12 @@ def run(self): # Create tracker print(f"Using tracker: {self._tracker_name}") - tracker = self._create_tracker(self._tracker_name) + tracker = TrackerFactory.create_tracker(self._tracker_name) tracking_rect = None tracking_rect_changed = True # TODO: how to ensure consistency of frame updates with GCS? - update_rate = 50.0 # Hz + update_rate = MAIN_LOOP_RATE # Hz update_period = 1.0 / update_rate # TODO: consolidate common code - also used in GUI @@ -890,7 +888,7 @@ def _mavlink_task(self): sysid = self._sysid cmpid = self._cmpid - update_rate = 1000.0 # Hz + update_rate = MAVLINK_RECV_RATE # Hz update_period = 1.0 / update_rate while True: @@ -927,7 +925,7 @@ def _send_status_task(self): # TODO: stop sending image status when tracking stopped # TODO: set streaming rate using MAV_CMD_SET_MESSAGE_INTERVAL - update_rate = 20.0 # Hz + update_rate = CAMERA_SEND_IMAGE_STATUS_RATE # Hz update_period = 1.0 / update_rate while True: @@ -977,9 +975,9 @@ def __init__(self, connection, enable_graphs=False): # Pitch controller for centring gimbal self._pit_controller = AC_PID_Basic( - initial_p=2.0, - initial_i=0.0, - initial_d=0.0, + initial_p=1.0, + initial_i=0.01, + initial_d=0.01, initial_ff=0.0, initial_imax=1.0, initial_filt_E_hz=0.0, @@ -988,9 +986,9 @@ def __init__(self, connection, enable_graphs=False): # Yaw controller for centring gimbal self._yaw_controller = AC_PID_Basic( - initial_p=2.0, - initial_i=0.0, - initial_d=0.0, + initial_p=1.0, + initial_i=0.01, + initial_d=0.01, initial_ff=0.0, initial_imax=1.0, initial_filt_E_hz=0.0, @@ -999,9 +997,9 @@ def __init__(self, connection, enable_graphs=False): # Gimbal pitch controller for tracking self._pit_track_controller = AC_PID_Basic( - initial_p=2.0, - initial_i=0.2, - initial_d=0.01, + initial_p=1.0, + initial_i=0.01, + initial_d=0.02, initial_ff=0.0, initial_imax=1.0, initial_filt_E_hz=0.0, @@ -1010,9 +1008,9 @@ def __init__(self, connection, enable_graphs=False): # Gimbal yaw controller for tracking self._yaw_track_controller = AC_PID_Basic( - initial_p=2.0, - initial_i=0.2, - initial_d=0.01, + initial_p=1.0, + initial_i=0.01, + initial_d=0.02, initial_ff=0.0, initial_imax=1.0, initial_filt_E_hz=0.0, @@ -1057,7 +1055,7 @@ def reset(self): def mavlink_packet(self, msg): self._control_in_queue.put(msg) - def _send_gimbal_manager_pitch_yaw_angles(self, pitch, yaw, pitch_rate, yaw_rate): + def _send_gimbal_manager_pitch_yaw_angle(self, pitch, yaw): """ Send a mavlink message to set the gimbal pitch and yaw (radians). """ @@ -1068,6 +1066,22 @@ def _send_gimbal_manager_pitch_yaw_angles(self, pitch, yaw, pitch_rate, yaw_rate 0, pitch, yaw, + float("nan"), + float("nan"), + ) + self._connection.mav.send(msg) + + def _send_gimbal_manager_pitch_yaw_rate(self, pitch_rate, yaw_rate): + """ + Send a mavlink message to set the gimbal pitch and yaw (radians). + """ + msg = self._connection.mav.gimbal_manager_set_pitchyaw_encode( + self._connection.target_system, + self._connection.target_component, + 0, + 0, + float("nan"), + float("nan"), pitch_rate, yaw_rate, ) @@ -1082,6 +1096,9 @@ def _control_task(self): When not tracking, return the gimbal to its neutral position. """ + update_rate = GIMBAL_CONTROL_LOOP_RATE + update_period = 1.0 / update_rate + if self._enable_graphs: self._add_livegraphs() @@ -1139,12 +1156,11 @@ def _control_task(self): # f"Out: {yaw_pid_info.out:.2f}" # ) - self._send_gimbal_manager_pitch_yaw_angles( - float("nan"), - float("nan"), - pit_rate_rads, - yaw_rate_rads, - ) + # NOTE: Set pitch and yaw rates to help determine PID gains for tracking + self._send_gimbal_manager_pitch_yaw_rate(pit_rate_rads, yaw_rate_rads) + + # NOTE: Set pitch and yaw angles directly (no PID controller needed) + # self._send_gimbal_manager_pitch_yaw_angle(pit_tgt_rad, yaw_tgt_rad) if self._enable_graphs: self._update_livegraphs(pit_pid_info, yaw_pid_info) @@ -1167,18 +1183,11 @@ def _control_task(self): pit_pid_info = self._pit_track_controller.pid_info yaw_pid_info = self._yaw_track_controller.pid_info - self._send_gimbal_manager_pitch_yaw_angles( - float("nan"), - float("nan"), - pit_rate_rads, - yaw_rate_rads, - ) + self._send_gimbal_manager_pitch_yaw_rate(pit_rate_rads, yaw_rate_rads) if self._enable_graphs: self._update_livegraphs(pit_pid_info, yaw_pid_info) - # Update at 50Hz - update_period = 0.02 elapsed_time = time.time() - start_time sleep_time = max(0.0, update_period - elapsed_time) time.sleep(sleep_time) @@ -1187,7 +1196,7 @@ def _mavlink_task(self): """ Process mavlink messages relevant to gimbal management. """ - update_rate = 1000.0 + update_rate = MAVLINK_RECV_RATE update_period = 1.0 / update_rate while True: @@ -1335,6 +1344,22 @@ def _create(self): return cv2.legacy.TrackerKCF_create() +class TrackerFactory: + + @staticmethod + def choices(): + return ["CSTR", "KCF"] + + @staticmethod + def create_tracker(name): + if name == "CSTR": + return TrackerCSTR() + elif name == "KCF": + return TrackerKCF() + else: + raise Exception(f"Invalid tracker name: {name}") + + if __name__ == "__main__": import os import sys @@ -1352,7 +1377,13 @@ def _create(self): type=int, help="source component id", ) - parser.add_argument("--tracker-name", default="CSTR", type=str, help="tracker name") + parser.add_argument( + "--tracker-name", + choices=TrackerFactory.choices(), + default="CSTR", + type=str, + help="tracker name", + ) parser.add_argument( "--enable-graphs", action="store_const", From ae658ad583f46c65dc3caa7e6f22af9ee99af8d0 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 17 Oct 2024 10:30:56 +0100 Subject: [PATCH 14/28] camtrack: onboard_controller: add constant for attitude status update rate Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/mavproxy_camtrack/onboard_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 1131d1c917..ef58eeac2a 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -76,6 +76,7 @@ # TODO: move update rate constants to CLI options MAIN_LOOP_RATE = 30.0 GIMBAL_CONTROL_LOOP_RATE = 30.0 +GIMBAL_DEVICE_ATTITUDE_STATUS_RATE = 30.0 CAMERA_SEND_IMAGE_STATUS_RATE = 5.0 MAVLINK_RECV_RATE = 1000.0 @@ -342,8 +343,7 @@ def run(self): # TODO: consolidate common code - also used in GUI # request gimbal device attitude status - gimbal_device_attitude_status_rate = 50.0 # Hz - interval_us = int(1e6 / gimbal_device_attitude_status_rate) + interval_us = int(1e6 / GIMBAL_DEVICE_ATTITUDE_STATUS_RATE) self.send_set_message_interval_message( mavutil.mavlink.MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, interval_us, # requested interval in microseconds From e442637bb9a6feebc34d6e0fc60741f008cc59fb Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 17 Oct 2024 10:31:53 +0100 Subject: [PATCH 15/28] camtrack: onboard_controller: add micro-second props to profiler Signed-off-by: Rhys Mainwaring --- .../mavproxy_camtrack/onboard_controller.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index ef58eeac2a..f4b51f1c06 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -181,6 +181,13 @@ def last_duration_ms(self): else: return int(-1) + @property + def last_duration_us(self): + if self._last_duration_s >= 0: + return int(self._last_duration_s * 1e6) + else: + return int(-1) + @property def avg_duration_ms(self): if self._count > 0: @@ -188,6 +195,13 @@ def avg_duration_ms(self): else: return int(-1) + @property + def avg_duration_us(self): + if self._count > 0: + return int(self._total_duration_s / self._count * 1e6) + else: + return int(-1) + class OnboardController: """ From 5f5922eb02e975359e7432e444c90cd8e78c1368 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 17 Oct 2024 10:34:47 +0100 Subject: [PATCH 16/28] camtrack: onboard_controller: add profiling to gimbal control loop Signed-off-by: Rhys Mainwaring --- .../mavproxy_camtrack/onboard_controller.py | 54 +++++++++++++++++-- 1 file changed, 49 insertions(+), 5 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index f4b51f1c06..45ed942406 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -330,7 +330,7 @@ def run(self): # Create and register controllers self._camera_controller = CameraTrackController(self._connection) self._gimbal_controller = GimbalController( - self._connection, self._enable_graphs + self._connection, self._enable_graphs, self._enable_profiler ) self._controllers.append(self._camera_controller) self._controllers.append(self._gimbal_controller) @@ -959,7 +959,7 @@ class GimbalController: Gimbal controller for onboard camera tracking. """ - def __init__(self, connection, enable_graphs=False): + def __init__(self, connection, enable_graphs=False, enable_profiler=False): self._connection = connection self._sysid = self._connection.source_system # system id matches vehicle self._cmpid = 1 # component id matches autopilot @@ -1031,6 +1031,9 @@ def __init__(self, connection, enable_graphs=False): initial_filt_D_hz=20.0, ) + # Profiler + self._enable_profiler = enable_profiler + # Analysis self._enable_graphs = enable_graphs self._graph_pid_tune = None @@ -1113,9 +1116,19 @@ def _control_task(self): update_rate = GIMBAL_CONTROL_LOOP_RATE update_period = 1.0 / update_rate - if self._enable_graphs: + with self._control_lock: + enable_graphs = self._enable_graphs + enable_profiler = self._enable_profiler + + if enable_graphs: self._add_livegraphs() + # Profiler - two activities are centring and tracking + centring_profiler = Profiler() + tracking_profiler = Profiler() + profile_print_last_time = time.time() + profile_print_period = 2 + last_time_s = time.time() while True: # Record the start time of the loop @@ -1141,6 +1154,8 @@ def _control_task(self): last_time_s = time_now_s if not tracking and gimbal_orientation is not None: + centring_profiler.start() + # NOTE: to centre the gimbal when not tracking, we need to know # the neutral angles from the MNT1_NEUTRAL_x params, and also # the current mount orientation. @@ -1176,10 +1191,14 @@ def _control_task(self): # NOTE: Set pitch and yaw angles directly (no PID controller needed) # self._send_gimbal_manager_pitch_yaw_angle(pit_tgt_rad, yaw_tgt_rad) - if self._enable_graphs: + centring_profiler.end() + + if enable_graphs: self._update_livegraphs(pit_pid_info, yaw_pid_info) elif frame_width is not None and frame_height is not None: + tracking_profiler.start() + # work with normalised screen [-1, 1] tgt_x = 0.0 tgt_y = 0.0 @@ -1199,9 +1218,34 @@ def _control_task(self): self._send_gimbal_manager_pitch_yaw_rate(pit_rate_rads, yaw_rate_rads) - if self._enable_graphs: + tracking_profiler.end() + + if enable_graphs: self._update_livegraphs(pit_pid_info, yaw_pid_info) + # Profile + if enable_profiler and ( + (time.time() - profile_print_last_time) > profile_print_period + ): + profile_print_last_time = time.time() + print("Gimbal controller control loop profile") + print("-------------------------------------") + print("time in us avg\tlast\tcount") + print( + f"centring: " + f"{centring_profiler.avg_duration_us}\t" + f"{centring_profiler.last_duration_us}\t" + f"{centring_profiler.count}" + ) + print( + f"tracking: " + f"{tracking_profiler.avg_duration_us}\t" + f"{tracking_profiler.last_duration_us}\t" + f"{tracking_profiler.count}" + ) + print("-------------------------------------") + print() + elapsed_time = time.time() - start_time sleep_time = max(0.0, update_period - elapsed_time) time.sleep(sleep_time) From 5c79cb5aa1b34540cb69254163871111e34da89f Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 17 Oct 2024 10:36:22 +0100 Subject: [PATCH 17/28] camtrack: onboard_controller: update legacy cv2 trackers to latest Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/mavproxy_camtrack/onboard_controller.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 45ed942406..b66e630289 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -1380,26 +1380,26 @@ def _create(self): class TrackerCSTR(TrackerOpenCV): """ - Wrapper for cv2.legacy.TrackerCSRT + Wrapper for cv2.TrackerCSRT """ def __init__(self): super().__init__() def _create(self): - return cv2.legacy.TrackerCSRT_create() + return cv2.TrackerCSRT.create() class TrackerKCF(TrackerOpenCV): """ - Wrapper for cv2.legacy.TrackerKCF + Wrapper for cv2.TrackerKCF """ def __init__(self): super().__init__() def _create(self): - return cv2.legacy.TrackerKCF_create() + return cv2.TrackerKCF.create() class TrackerFactory: From de99ae26d17524e24d9a59a80d1f49d7069c6e67 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 17 Oct 2024 10:36:58 +0100 Subject: [PATCH 18/28] camtrack: onboard_controller: add rate limiter to gimbal controller Signed-off-by: Rhys Mainwaring --- .../modules/mavproxy_camtrack/onboard_controller.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index b66e630289..fb2adf515d 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -1213,6 +1213,17 @@ def _control_task(self): pit_rate_rads = self._pit_track_controller.update_all(tgt_y, act_y, dt) yaw_rate_rads = self._yaw_track_controller.update_all(tgt_x, act_x, dt) + # Apply rate limits (30 deg/s) + MAX_RATE_RAD_S = math.radians(30.0) + pit_rate_rads = constrain_float( + pit_rate_rads, -MAX_RATE_RAD_S, MAX_RATE_RAD_S + ) + pit_pid_info.out = pit_rate_rads + yaw_rate_rads = constrain_float( + yaw_rate_rads, -MAX_RATE_RAD_S, MAX_RATE_RAD_S + ) + yaw_pid_info.out = yaw_rate_rads + pit_pid_info = self._pit_track_controller.pid_info yaw_pid_info = self._yaw_track_controller.pid_info From 0452b6af34e1656ee5c107ed8db80de4c363e0e6 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 17 Oct 2024 10:37:36 +0100 Subject: [PATCH 19/28] camtrack: onboard_controller: add rate limiter to gimbal controller - Add missing import Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/mavproxy_camtrack/onboard_controller.py | 1 + 1 file changed, 1 insertion(+) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index fb2adf515d..9262593eff 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -68,6 +68,7 @@ from MAVProxy.modules.lib.pid_basic import AC_PID_Basic from MAVProxy.modules.lib.pid_basic import AP_PIDInfo +from MAVProxy.modules.lib.pid_basic import constrain_float gi.require_version("Gst", "1.0") From aec522a5ebaeef91b28922d7a20bcae566933978 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 17 Oct 2024 11:29:29 +0100 Subject: [PATCH 20/28] camtrack: onboard_controller: check if cv2 has gstreamer Signed-off-by: Rhys Mainwaring --- .../mavproxy_camtrack/onboard_controller.py | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 9262593eff..a96a21c29a 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -54,6 +54,7 @@ import math import numpy as np import queue +import re import threading import time @@ -204,6 +205,27 @@ def avg_duration_us(self): return int(-1) +def cv2_has_gstreamer(): + """ + Check whether cv2 was compiled with GStreamer support. + + The check involves searching the cv2 build information for a line + such as: + + GStreamer: YES (1.24.7) + """ + build_info = cv2.getBuildInformation() + match = re.search(".*GStreamer.*", build_info) + if match is None: + return False + + match = re.search("YES", match.group(0)) + if match is None: + return False + + return match.group(0) == "YES" + + class OnboardController: """ Onboard gimbal controller. @@ -237,6 +259,9 @@ def __init__( print(f"Onboard Controller: src_sys: {self._sysid}, src_cmp: {self._cmpid})") + self._cv2_has_gstreamer = cv2_has_gstreamer() + print(f"OpenCV has GStreamer: {self._cv2_has_gstreamer}") + def __del__(self): if self._connection is not None: self._connection.close() From 90deeeadc5cd38c32a20b4829f03ef7a89734abc Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 17 Oct 2024 11:31:08 +0100 Subject: [PATCH 21/28] camtrack: onboard_controller: use cv2 gstreamer if available Signed-off-by: Rhys Mainwaring --- .../mavproxy_camtrack/onboard_controller.py | 52 ++++++++++++++----- 1 file changed, 39 insertions(+), 13 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index a96a21c29a..2b91b072dc 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -339,19 +339,34 @@ def run(self): # Connect to video stream print(f"Using RTSP server: {self._rtsp_url}") - video_stream = VideoStream(self._rtsp_url) - - # 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: + if self._cv2_has_gstreamer: + gst_pipeline = ( + f"rtspsrc location={self._rtsp_url} latency=50 " + "! decodebin " + "! videoconvert " + "! video/x-raw,format=(string)BGR " + "! videoconvert " + "! appsink emit-signals=true sync=false max-buffers=2 drop=true" + ) + cap_options = cv2.CAP_GSTREAMER + video_stream = cv2.VideoCapture(gst_pipeline, cap_options) + if not video_stream or not video_stream.isOpened(): print("\nVideo stream not available - restarting") return - time.sleep(0.1) - print("\nVideo stream available") + print("\nVideo stream available") + else: + video_stream = VideoStream(self._rtsp_url) + # 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") # Create and register controllers self._camera_controller = CameraTrackController(self._connection) @@ -400,10 +415,21 @@ def run(self): while True: start_time = time.time() - if video_stream.frame_available(): + # Check next frame available + if self._cv2_has_gstreamer: + frame_available, frame = video_stream.read() + else: + frame_available = video_stream.frame_available() + + if frame_available: loop_profiler.start() - frame = copy.deepcopy(video_stream.frame()) + # Get RGB frame + if self._cv2_has_gstreamer: + frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + else: + frame = video_stream.frame() + track_type = self._camera_controller.track_type() if track_type is CameraTrackType.NONE: if tracking_rect is not None: From b2a9a24402893b1047ba698a349935bbc53917c2 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 21 Oct 2024 22:47:24 +0100 Subject: [PATCH 22/28] camtrack: onboard_controller: add option to use hardware pipeline for RPi Signed-off-by: Rhys Mainwaring --- .../mavproxy_camtrack/onboard_controller.py | 83 +++++++++++++++---- 1 file changed, 66 insertions(+), 17 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 2b91b072dc..5f7a436a3b 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -75,11 +75,19 @@ gi.require_version("Gst", "1.0") from gi.repository import Gst -# TODO: move update rate constants to CLI options -MAIN_LOOP_RATE = 30.0 +# The main loop rate (Hz) is max rate that the main loop will run at. +# In practice it is limited by the video framerate and tracker update time. +MAIN_LOOP_RATE = 100.0 + +# The requested rates (Hz) for the current gimbal attitude and rate at which +# the commands to update the gimbal pitch and yaw are sent. GIMBAL_CONTROL_LOOP_RATE = 30.0 GIMBAL_DEVICE_ATTITUDE_STATUS_RATE = 30.0 -CAMERA_SEND_IMAGE_STATUS_RATE = 5.0 + +# The rate (Hz) to send information about the camera image. +CAMERA_SEND_IMAGE_STATUS_RATE = 10.0 + +# The rate (Hz) at which mavlink messages are pulled off the input queues. MAVLINK_RECV_RATE = 1000.0 @@ -240,6 +248,7 @@ def __init__( tracker_name="CSTR", enable_graphs=False, enable_profiler=False, + enable_rpi_pipeline=False, ): self._device = device self._sysid = sysid @@ -248,6 +257,7 @@ def __init__( self._tracker_name = tracker_name self._enable_graphs = enable_graphs self._enable_profiler = enable_profiler + self._enable_rpi_pipeline = enable_rpi_pipeline # mavlink connection self._connection = None @@ -340,20 +350,49 @@ def run(self): # Connect to video stream print(f"Using RTSP server: {self._rtsp_url}") if self._cv2_has_gstreamer: - gst_pipeline = ( - f"rtspsrc location={self._rtsp_url} latency=50 " - "! decodebin " - "! videoconvert " - "! video/x-raw,format=(string)BGR " - "! videoconvert " - "! appsink emit-signals=true sync=false max-buffers=2 drop=true" - ) + if self._enable_rpi_pipeline: + # Pipeline for RPi4 using hardware decode / convert + gst_pipeline = ( + f"rtspsrc location={self._rtsp_url} latency=50 " + f"! rtph264depay " + f"! h264parse " + f"! v4l2h264dec " + f"! v4l2convert " + f"! videoscale " + f"! videorate " + f"! video/x-raw,format=BGR,width=640,height=360,framerate=20/1 " + f"! appsink emit-signals=true sync=false max-buffers=2 drop=true" + ) + else: + # Generic autodetected pipeline + gst_pipeline = ( + f"rtspsrc location={self._rtsp_url} latency=50 " + "! decodebin " + "! videoconvert " + "! video/x-raw,format=(string)BGR " + "! videoconvert " + "! appsink emit-signals=true sync=false max-buffers=2 drop=true" + ) + + # From mavproxy_SIYI for H.265. + # NOTE: RPi4 does not have a hardware decoder for H.265 suitable for + # GStreamer. + # gst_pipeline = ( + # f"rtspsrc location={self.rtsp_url} latency=0 buffer-mode=auto " + # f"! rtph265depay " + # f"! queue " + # f"! h265parse " + # f"! avdec_h265 " + # f"! videoconvert " + # f"! video/x-raw,format=BGRx " + # f"! appsink" + # ) + cap_options = cv2.CAP_GSTREAMER video_stream = cv2.VideoCapture(gst_pipeline, cap_options) if not video_stream or not video_stream.isOpened(): - print("\nVideo stream not available - restarting") + print("\nVideo stream not available") return - print("\nVideo stream available") else: video_stream = VideoStream(self._rtsp_url) # Timeout if video not available @@ -363,10 +402,11 @@ def run(self): while not video_stream.frame_available(): print(".", end="") if (time.time() - video_last_update_time) > video_timeout_period: - print("\nVideo stream not available - restarting") + print("\nVideo stream not available") return time.sleep(0.1) - print("\nVideo stream available") + + print("\nVideo stream available") # Create and register controllers self._camera_controller = CameraTrackController(self._connection) @@ -413,7 +453,7 @@ def run(self): profile_print_period = 2 while True: - start_time = time.time() + loop_start_time = time.time() # Check next frame available if self._cv2_has_gstreamer: @@ -466,6 +506,7 @@ def __compare_rect(rect1, rect2): # update tracker and gimbal if tracking active if tracking_rect is not None: loop_with_tracker_profiler.start() + tracker_profiler.start() success, box = tracker.update(frame) tracker_profiler.end() @@ -526,7 +567,7 @@ def __compare_rect(rect1, rect2): print() # Rate limit - elapsed_time = time.time() - start_time + elapsed_time = time.time() - loop_start_time sleep_time = max(0.0, update_period - elapsed_time) time.sleep(sleep_time) @@ -1519,6 +1560,13 @@ def create_tracker(name): const=True, help="enable main loop profiler", ) + parser.add_argument( + "--enable-rpi-pipeline", + action="store_const", + default=False, + const=True, + help="enable hardware pipelane for rpi4", + ) args = parser.parse_args() controller = OnboardController( @@ -1529,6 +1577,7 @@ def create_tracker(name): args.tracker_name, args.enable_graphs, args.enable_profiler, + args.enable_rpi_pipeline, ) while True: From c1dbace6df8d0eb582dac199087e3fb11a4f3323 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 21 Oct 2024 22:49:37 +0100 Subject: [PATCH 23/28] camtrack: onboard_controller: more robust handling of frame shape Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/mavproxy_camtrack/onboard_controller.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 5f7a436a3b..0e3b869f14 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -518,7 +518,7 @@ def __compare_rect(rect1, rect2): self._gimbal_controller.update_center(u, v, frame.shape) # update camera controller current tracking state - frame_height, frame_width, _ = frame.shape + frame_height, frame_width = frame.shape[:2] top_left_x = x / frame_width top_left_y = y / frame_height bot_right_x = (x + w) / frame_width @@ -1150,7 +1150,7 @@ def update_center(self, x, y, shape): self._tracking = True self._center_x = x self._center_y = y - self._frame_height, self._frame_width, _ = shape + self._frame_height, self._frame_width = shape[:2] def reset(self): with self._control_lock: @@ -1454,7 +1454,7 @@ def update(self, frame): if self._nroi_changed: self._tracker = self._create() # Denormalise the roi - frame_height, frame_width, _ = frame.shape + frame_height, frame_width = frame.shape[:2] roi = [ int(self._nroi[0] * frame_width), # x int(self._nroi[1] * frame_height), # y From 6ffb593600eefd377dbcea7bfa774499e7e99510 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 21 Oct 2024 22:50:38 +0100 Subject: [PATCH 24/28] camtrack: onboard_controller: rescale and rate limit default pipeline Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/mavproxy_camtrack/onboard_controller.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 0e3b869f14..11fe8433c9 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -367,10 +367,11 @@ def run(self): # Generic autodetected pipeline gst_pipeline = ( f"rtspsrc location={self._rtsp_url} latency=50 " - "! decodebin " - "! videoconvert " - "! video/x-raw,format=(string)BGR " - "! videoconvert " + f"! decodebin " + f"! videoconvert " + f"! videoscale " + f"! videorate " + f"! video/x-raw,format=BGR,width=640,height=360,framerate=20/1 " "! appsink emit-signals=true sync=false max-buffers=2 drop=true" ) From 42f0358f5ffa12461b9d303016463b20975ceb41 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 21 Oct 2024 23:01:38 +0100 Subject: [PATCH 25/28] camtrack: onboard_controller: add MOSSE tracker Signed-off-by: Rhys Mainwaring --- .../mavproxy_camtrack/onboard_controller.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 11fe8433c9..67e25be00b 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -1507,11 +1507,23 @@ def _create(self): return cv2.TrackerKCF.create() +class TrackerMOSSE(TrackerOpenCV): + """ + Wrapper for cv2.legacy.TrackerMOSSE + """ + + def __init__(self): + super().__init__() + + def _create(self): + return cv2.legacy.TrackerMOSSE_create() + + class TrackerFactory: @staticmethod def choices(): - return ["CSTR", "KCF"] + return ["CSTR", "KCF", "MOSSE"] @staticmethod def create_tracker(name): @@ -1519,6 +1531,8 @@ def create_tracker(name): return TrackerCSTR() elif name == "KCF": return TrackerKCF() + elif name == "MOSSE": + return TrackerMOSSE() else: raise Exception(f"Invalid tracker name: {name}") From ba44feebd2b6f683894708c9c25d29e9fc111b6a Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 21 Oct 2024 23:02:38 +0100 Subject: [PATCH 26/28] camtrack: onboard_controller: update tracker pids Signed-off-by: Rhys Mainwaring --- MAVProxy/modules/mavproxy_camtrack/onboard_controller.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 67e25be00b..0325914300 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -1105,9 +1105,9 @@ def __init__(self, connection, enable_graphs=False, enable_profiler=False): # Gimbal pitch controller for tracking self._pit_track_controller = AC_PID_Basic( - initial_p=1.0, + initial_p=0.75, initial_i=0.01, - initial_d=0.02, + initial_d=0.01, initial_ff=0.0, initial_imax=1.0, initial_filt_E_hz=0.0, @@ -1116,9 +1116,9 @@ def __init__(self, connection, enable_graphs=False, enable_profiler=False): # Gimbal yaw controller for tracking self._yaw_track_controller = AC_PID_Basic( - initial_p=1.0, + initial_p=0.7, initial_i=0.01, - initial_d=0.02, + initial_d=0.01, initial_ff=0.0, initial_imax=1.0, initial_filt_E_hz=0.0, From 87e3bf651b68323fc4f6d0a3b1f5c8c89c1d51c7 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 21 Oct 2024 23:04:31 +0100 Subject: [PATCH 27/28] camtrack: onboard_controller: display average fps Signed-off-by: Rhys Mainwaring --- .../modules/mavproxy_camtrack/onboard_controller.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 0325914300..0d277fadf1 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -453,6 +453,11 @@ def run(self): profile_print_last_time = time.time() profile_print_period = 2 + fps_print_last_time = time.time() + fps_print_period = 2 + fps_start_time = time.time() + frame_count = 0 + while True: loop_start_time = time.time() @@ -463,6 +468,8 @@ def run(self): frame_available = video_stream.frame_available() if frame_available: + frame_count += 1 + loop_profiler.start() # Get RGB frame @@ -567,6 +574,12 @@ def __compare_rect(rect1, rect2): print("-------------------------------------") print() + # FPS + if (time.time() - fps_print_last_time) > fps_print_period: + fps_print_last_time = time.time() + fps = frame_count / (time.time() - fps_start_time) + print(f"fps: {fps:.1f}, count: {frame_count}, shape: {frame.shape}") + # Rate limit elapsed_time = time.time() - loop_start_time sleep_time = max(0.0, update_period - elapsed_time) From c55c1bb19c1d3238f2d1f9f12ac6217eead17d39 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 22 Oct 2024 13:36:41 +0100 Subject: [PATCH 28/28] camtrack: onboard_controller: update video frame rate and tracker pids Signed-off-by: Rhys Mainwaring --- .../mavproxy_camtrack/onboard_controller.py | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py index 0d277fadf1..fab250051d 100644 --- a/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py +++ b/MAVProxy/modules/mavproxy_camtrack/onboard_controller.py @@ -81,7 +81,7 @@ # The requested rates (Hz) for the current gimbal attitude and rate at which # the commands to update the gimbal pitch and yaw are sent. -GIMBAL_CONTROL_LOOP_RATE = 30.0 +GIMBAL_CONTROL_LOOP_RATE = 20.0 GIMBAL_DEVICE_ATTITUDE_STATUS_RATE = 30.0 # The rate (Hz) to send information about the camera image. @@ -360,7 +360,7 @@ def run(self): f"! v4l2convert " f"! videoscale " f"! videorate " - f"! video/x-raw,format=BGR,width=640,height=360,framerate=20/1 " + f"! video/x-raw,format=BGR,width=640,height=360,framerate=10/1 " f"! appsink emit-signals=true sync=false max-buffers=2 drop=true" ) else: @@ -371,7 +371,7 @@ def run(self): f"! videoconvert " f"! videoscale " f"! videorate " - f"! video/x-raw,format=BGR,width=640,height=360,framerate=20/1 " + f"! video/x-raw,format=BGR,width=640,height=360,framerate=10/1 " "! appsink emit-signals=true sync=false max-buffers=2 drop=true" ) @@ -1096,45 +1096,45 @@ def __init__(self, connection, enable_graphs=False, enable_profiler=False): # Pitch controller for centring gimbal self._pit_controller = AC_PID_Basic( - initial_p=1.0, + initial_p=2.0, initial_i=0.01, - initial_d=0.01, + initial_d=0.05, initial_ff=0.0, initial_imax=1.0, - initial_filt_E_hz=0.0, + initial_filt_E_hz=20.0, initial_filt_D_hz=20.0, ) # Yaw controller for centring gimbal self._yaw_controller = AC_PID_Basic( - initial_p=1.0, + initial_p=2.0, initial_i=0.01, - initial_d=0.01, + initial_d=0.05, initial_ff=0.0, initial_imax=1.0, - initial_filt_E_hz=0.0, + initial_filt_E_hz=20.0, initial_filt_D_hz=20.0, ) # Gimbal pitch controller for tracking self._pit_track_controller = AC_PID_Basic( - initial_p=0.75, + initial_p=1.5, initial_i=0.01, - initial_d=0.01, + initial_d=0.05, initial_ff=0.0, initial_imax=1.0, - initial_filt_E_hz=0.0, + initial_filt_E_hz=20.0, initial_filt_D_hz=20.0, ) # Gimbal yaw controller for tracking self._yaw_track_controller = AC_PID_Basic( - initial_p=0.7, + initial_p=1.5, initial_i=0.01, - initial_d=0.01, + initial_d=0.05, initial_ff=0.0, initial_imax=1.0, - initial_filt_E_hz=0.0, + initial_filt_E_hz=20.0, initial_filt_D_hz=20.0, ) @@ -1320,8 +1320,8 @@ def _control_task(self): pit_rate_rads = self._pit_track_controller.update_all(tgt_y, act_y, dt) yaw_rate_rads = self._yaw_track_controller.update_all(tgt_x, act_x, dt) - # Apply rate limits (30 deg/s) - MAX_RATE_RAD_S = math.radians(30.0) + # Apply rate limits (90 deg/s) + MAX_RATE_RAD_S = math.radians(90.0) pit_rate_rads = constrain_float( pit_rate_rads, -MAX_RATE_RAD_S, MAX_RATE_RAD_S )