AP_Camera: Add Camera Tracking functionality (WIP) #27773

khanasif786 · 2024-08-07T01:14:44Z

This adds support for a companion computer to perform image tracking using any of the backends

libraries/AP_Camera/AP_Camera_Backend.cpp

rmackay9 · 2024-08-07T01:17:27Z

libraries/AP_Camera/AP_Camera_Backend.cpp

+    }
+}
+
+void AP_Camera_Backend::handle_message_camera_information(mavlink_channel_t chan, const mavlink_message_t &msg)  {


I think we will face conflicts when using a mavlink enabled camera (e.g. CAMx_TYPE = MavlinkCamV2) because we will also receive a camera_information message from these cameras.

got it. now what will happen is if the message is coming from a camera, the camera's handle message will be called and if the message is from the tracking device it will forward it to base class's handle message.

libraries/AP_Camera/AP_Camera_Backend.h

libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp

libraries/AP_Camera/AP_Camera_Backend.h

libraries/AP_Camera/AP_Camera_Tracking.cpp

libraries/AP_Camera/AP_Camera_Tracking.h

rmackay9 · 2024-08-07T01:40:18Z

Could you rebase this and also clarify what testing has been done?

libraries/AP_Camera/AP_Camera_Backend.cpp

libraries/AP_Camera/AP_Camera_Tracking.cpp

libraries/AP_Camera/AP_Camera_Tracking.h

rmackay9 · 2024-08-07T01:42:42Z

Could you create a separate PR with the python script in a new directory AP_Camera/examples?

rmackay9 · 2024-08-07T01:59:34Z

We should also consume the CAMERA_TRACKING_IMAGE_STATUS message and update the user

libraries/AP_Camera/examples/send_camera_information.py

rmackay9 · 2024-08-07T02:05:51Z

libraries/AP_Camera/examples/tracking.py

+import math 
+from ultralytics import YOLO
+
+master = mavutil.mavlink_connection('127.0.0.1:14560')


we should allow the caller to specify the video source as an argument to the script.

instructions on how to install would be good. E.g. what packages are required and how can they be installed? (e.g. pip)

Do you need something like requirements.txt, i actually added a comment on top just for now.

rmackay9 · 2024-08-07T02:09:55Z

libraries/AP_Camera/examples/tracking.py

+            # faces = face_cascade.detectMultiScale(gray, scaleFactor=1.3, minNeighbors=5, minSize=(30, 30))
+            faces= [final_list]
+            if len(faces) == 0:
+                # Do something if no faces were detected


We should be sending the CAMERA_TRACKING_IMAGE_STATUS message at regular intervals (1hz?) so that AP knows if we can't track the object any more

We should support MAV_CMD_CAMERA_STOP_TRACKING

Isnt that should be sent by the FC ? i.e.
CAMERA_TRACKING_IMAGE_STATUS

rmackay9 · 2024-08-07T02:11:29Z

We should implement MAV_CMD_CAMERA_STOP_TRACKING

rmackay9 · 2024-08-07T02:12:54Z

libraries/AP_Camera/examples/tracking.py

+                    center_x = (x + w) // 2
+                    center_y = (y + h) // 2
+                    cv2.circle(frame, (center_x, center_y), 3, (0, 255, 0), -1)
+                    #print("Center of the face: ({}, {})".format(center_x, center_y))


instead of commented out lines, perhaps put a debug definition at the top to allow turning on/off debug

rmackay9 · 2024-08-07T02:18:42Z

libraries/AP_Camera/examples/tracking.py

+
+# gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink sync=false
+
+gst_pipeline = (


let's get a pipeline for at least the siyi camera gimbals using an ethernet connection to the companion computer

I think we should move this definition to the top to make it easier to find and possibly we should try and create this pipe line based on arguments input by the caller

libraries/AP_Camera/examples/send_camera_information.py

peterbarker · 2024-08-07T02:05:47Z

libraries/AP_Camera/examples/send_camera_information.py

+    connection.wait_heartbeat()
+    print("Heartbeat received from system (system %u component %u)" % (connection.target_system, connection.target_component))
+    # Send a heartbeat back to register the device
+    connection.mav.heartbeat_send(


heartbeats must be sent regularly - at, say, 1Hz

in a separate thread right ?

libraries/AP_Camera/examples/send_camera_information.py

libraries/AP_Camera/examples/tracking.py

peterbarker · 2024-08-07T02:15:24Z

libraries/AP_Camera/examples/tracking.py

+    #print("Gimbal manager pitch and yaw angles message sent.")
+send_gimbal_manager_pitch_yaw_angles(0, 0, 0, 0, 0, 0, float("NaN"),float("NaN"))
+def send_command():
+    global centre_x, center_y


this looks odd

libraries/AP_Camera/examples/send_camera_information.py

peterbarker · 2024-08-07T02:19:31Z

libraries/AP_Camera/examples/tracking.py

+            except:
+                pass


This isn't acceptable in any Python script. You'd be surprised at how painful this can be.

Catch the exceptions you know about.

Hwurzburg

needs exclusion in minimize include file and build options/extract features files added

srmainwaring · 2024-08-18T12:33:34Z

libraries/AP_Camera/examples/send_camera_information.py

+
+    # Start listening for MAVLink messages
+    while True:
+        msg = connection.recv_match(type='COMMAND_LONG', blocking=True)


Does the call need to be blocking here?

Dont you think it should be ?

srmainwaring · 2024-08-18T12:41:32Z

libraries/AP_Camera/examples/tracking.py

+    "rtph264depay ! h264parse ! queue ! avdec_h264 ! videoconvert ! appsink"
+)
+
+cap = cv2.VideoCapture(gst_pipeline, cv2.CAP_GSTREAMER)


The default versions of opencv-python and opencv-contrib-python do not have gstreamer support enabled. A custom build of the Python packages from source is required for this (at least this is the case on macOS). To reduce the complexity it may be easier to use the gstreamer Python bindings to capture the video stream and then make this available to opencv. The BlueROV docs have a guide for this: https://www.ardusub.com/developers/opencv.html.

Yes you're right.

srmainwaring · 2024-08-18T12:49:55Z

libraries/AP_Camera/examples/send_camera_information.py

+# Main function
+def main():
+    # Send camera information message
+    send_camera_information(connection)


There seems to be two ways to ensure that the MAV_CMD_CAMERA_TRACK_POINT response expected below is triggered when sending the camera info. One is in AP_Camera::handle_command(const mavlink_command_int_t &packet) and requires a MAV_CMD_CAMERA_TRACK_POINT message be sent. The other is to use an aux switch to enable camera image tracking. This is the setup I used for testing.

RC13_OPTION 174.0 # Camera Image Tracking

Along with

param set CAM1_TRK_ENABLE 1 param set CAM1_TRK_SYSID 245 param set CAM1_TRK_COMPID 0

then run the script, enable tracking with

MANUAL> rc 13 1900

and check the output.

Either way all of this needs to be documented.

That is a good way actually, the thing is RC switch based tracking will tell only to start tracking anything in the frame. atlast we need to send the CAMERA_TRACKING messages to work with specific objects

khanasif786

@rmackay9 @peterbarker @srmainwaring Have a look

libraries/AP_Camera/examples/send_camera_information.py

khanasif786 · 2024-08-20T20:35:41Z

libraries/AP_Camera/examples/send_camera_information.py

+    connection.wait_heartbeat()
+    print("Heartbeat received from system (system %u component %u)" % (connection.target_system, connection.target_component))
+    # Send a heartbeat back to register the device
+    connection.mav.heartbeat_send(


in a separate thread right ?

libraries/AP_Camera/examples/send_camera_information.py

khanasif786 · 2024-08-20T20:57:59Z

libraries/AP_Camera/examples/tracking.py

+import math 
+from ultralytics import YOLO
+
+master = mavutil.mavlink_connection('127.0.0.1:14560')


Do you need something like requirements.txt, i actually added a comment on top just for now.

libraries/AP_Camera/examples/tracking.py

khanasif786 · 2024-08-20T21:39:50Z

libraries/AP_Camera/examples/tracking.py

+    "rtph264depay ! h264parse ! queue ! avdec_h264 ! videoconvert ! appsink"
+)
+
+cap = cv2.VideoCapture(gst_pipeline, cv2.CAP_GSTREAMER)


Yes you're right.

khanasif786 · 2024-08-20T21:42:33Z

libraries/AP_Camera/examples/tracking.py

+            # faces = face_cascade.detectMultiScale(gray, scaleFactor=1.3, minNeighbors=5, minSize=(30, 30))
+            faces= [final_list]
+            if len(faces) == 0:
+                # Do something if no faces were detected


Isnt that should be sent by the FC ? i.e.
CAMERA_TRACKING_IMAGE_STATUS

rmackay9 · 2024-08-21T00:21:39Z

@khanasif786, thanks for all your efforts on this. Could you rebase this on master too?

Let's also make it "ready for review" instead of "WIP"

libraries/AP_Camera/AP_Camera_Tracking.h

libraries/AP_Camera/AP_Camera_Tracking.cpp

amilcarlucas · 2024-08-31T09:03:54Z

 Binary Name      Text [B]         Data [B]     BSS (B)        Total Flash Change [B] (%)      Flash Free After PR (B)
---------------  ---------------  -----------  -------------  ----------------------------  -------------------------
antennatracker   916 (+0.0688%)   0 (0.0000%)  -4 (-0.0015%)  916 (+0.0687%)                                   631372
arduplane        1048 (+0.0583%)  0 (0.0000%)  0 (0.0000%)    1048 (+0.0582%)                                  164728
blimp            916 (+0.0677%)   0 (0.0000%)  4 (+0.0015%)   916 (+0.0675%)                                   609112
arducopter-heli  1076 (+0.0598%)  0 (0.0000%)  4 (+0.0015%)   1076 (+0.0597%)                                  162656
arducopter       1072 (+0.0596%)  0 (0.0000%)  0 (0.0000%)    1072 (+0.0594%)                                  161680
ardusub          1088 (+0.0684%)  0 (0.0000%)  0 (0.0000%)    1088 (+0.0683%)                                  371252
ardurover        1060 (+0.0643%)  0 (0.0000%)  4 (+0.0015%)   1060 (+0.0642%)                                  313360

Hwurzburg

still needs the changes I requested

landswellsong · 2024-09-22T20:29:45Z

libraries/AP_Camera/AP_Camera_Backend.cpp

+    const uint8_t fw_ver_build = (camera_settings._cam_info.firmware_version & 0xFF000000) >> 24;
+
+    // display camera info to user
+    gcs().send_text(MAV_SEVERITY_INFO, "Camera: %s.32 %s.32 fw:%u.%u.%u.%u",


This should be %.32s instead, probably carried over from the code before #27562

srmainwaring · 2024-10-03T18:23:42Z

libraries/AP_Camera/AP_Camera_Tracking.cpp

+    }
+
+    _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt);
+    GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"sent message to device sysid %d and comp %d",tracking_device_sysid,tracking_device_compid);


This could be removed (or should be MAV_SEVERITY_INFO) , and the sysid, compid info added to the Tracking: New Tracking request message on L28

srmainwaring · 2024-10-07T19:32:31Z

@khanasif786, I think perhaps the follow-functionality should be split into a separate PR, with this one focussing on the core camera tracking changes.

The addition of MSG_CAMERA_TRACKING_IMAGE_STATUS is needed though if we are to run the tracker on a companion computer and want to send the GCS information about the current bounding box so it can be displayed in the GUI. I'm running into just this issue in testing.

I've written a MAVProxy module (camtrack) to test this PR in simulation and on hardware. There's a mavproxy GUI module to engage / disengage the tracker, and a separate headless module that may run on either the GCS computer, or on the companion computer.

It's working in simulation, still having a few issues setting up correct sys_id and component_id for all components on hardware (CubeOrangePlus with RPi4 companion). AFAICT the RPi4 copes with the cv2.legacy.TrackerCSTR tracker.

The MAVProxy code is here: https://github.com/srmainwaring/MAVProxy/tree/wips/wip-camera-tracking. It is a WIP with a lot of debug message code included that will eventually be pruned. Let me know if you'd like a walk through.

A rebase of this PR on master here: https://github.com/srmainwaring/ardupilot/tree/mav_cmd_camera_final_rebased. I'd force push, but don't have access. There are a few conflicts resolved.

srmainwaring · 2024-10-03T18:25:25Z

libraries/AP_Camera/AP_Camera_Tracking.cpp

+            break;
+    }
+
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Tracking: New Tracking request");


Message prefix should be consistent: AP_Camera: is used elsewhere.

Perhaps add sysid, compid here as this is useful information when setting up the system, and if they are not correct the function returns early (so the message including them below is not reported).

srmainwaring · 2024-10-07T20:05:37Z

libraries/AP_Camera/AP_Camera_Backend.h

+    enum CAMERA_TRACKING_STATUS_FLAGS : uint8_t {
+        CAMERA_TRACKING_STATUS_FLAGS_IDLE,
+        CAMERA_TRACKING_STATUS_FLAGS_ACTIVE,
+        CAMERA_TRACKING_STATUS_FLAGS_ERROR
+    };


Shadows

#define HAVE_ENUM_CAMERA_TRACKING_STATUS_FLAGS typedef enum CAMERA_TRACKING_STATUS_FLAGS { CAMERA_TRACKING_STATUS_FLAGS_IDLE=0, /* Camera is not tracking | */ CAMERA_TRACKING_STATUS_FLAGS_ACTIVE=1, /* Camera is tracking | */ CAMERA_TRACKING_STATUS_FLAGS_ERROR=2, /* Camera tracking in error state | */ CAMERA_TRACKING_STATUS_FLAGS_ENUM_END=3, /* | */ } CAMERA_TRACKING_STATUS_FLAGS; #endif

in mavlink/v2.0/common/common.h

srmainwaring · 2024-10-07T20:06:44Z

libraries/AP_Camera/AP_Camera_Backend.cpp

+            handle_message_camera_information(chan,msg);
+            break;
+        case MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS:
+            handle_message_camera_tracking_image_status(chan,msg);


Missing break. Compilation with clang fails.

srmainwaring · 2024-10-10T14:42:07Z

libraries/AP_Camera/AP_Camera_Backend.h

@@ -86,6 +86,14 @@ class AP_Camera_Backend
    // p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
    bool set_tracking(TrackingType tracking_type, const Vector2f& top_left, const Vector2f& bottom_right);

+    // returns true if the objkect to be tracked is visible in the frame


sp. objkect -> object

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Backend.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Backend.h Outdated Show resolved Hide resolved

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Backend.h Show resolved Hide resolved

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Tracking.cpp Show resolved Hide resolved

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Tracking.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Tracking.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Tracking.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Tracking.cpp Outdated Show resolved Hide resolved

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Tracking.h Outdated Show resolved Hide resolved

peterbarker requested changes Aug 7, 2024

View reviewed changes

rmackay9 reviewed Aug 7, 2024

View reviewed changes

libraries/AP_Camera/examples/send_camera_information.py Outdated Show resolved Hide resolved

rmackay9 reviewed Aug 7, 2024

View reviewed changes

peterbarker requested changes Aug 7, 2024

View reviewed changes

Hwurzburg requested changes Aug 7, 2024

View reviewed changes

srmainwaring reviewed Aug 18, 2024

View reviewed changes

Hwurzburg added the WikiNeeded needs wiki update label Aug 18, 2024

khanasif786 marked this pull request as draft August 20, 2024 21:44

khanasif786 changed the title ~~AP_Camera: Add Camera Tracking functionality~~ AP_Camera: Add Camera Tracking functionality (WIP) Aug 20, 2024

khanasif786 commented Aug 20, 2024

View reviewed changes

khanasif786 marked this pull request as ready for review August 21, 2024 05:16

khanasif786 changed the title ~~AP_Camera: Add Camera Tracking functionality (WIP)~~ AP_Camera: Add Camera Tracking functionality Aug 21, 2024

khanasif786 mentioned this pull request Aug 22, 2024

AP_camera: Camera tracking example scripts (WIP) #27903

Open

khanasif786 force-pushed the mav_cmd_camera_final branch 2 times, most recently from 8c94c12 to 7dd51ed Compare August 22, 2024 20:10

rmackay9 reviewed Aug 23, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Tracking.h Outdated Show resolved Hide resolved

srmainwaring reviewed Aug 26, 2024

View reviewed changes

libraries/AP_Camera/AP_Camera_Tracking.cpp Outdated Show resolved Hide resolved

AP_Camera: Add Camera Tracking functionality

766f08a

khanasif786 force-pushed the mav_cmd_camera_final branch from 7dd51ed to 766f08a Compare August 30, 2024 05:53

khanasif786 mentioned this pull request Sep 1, 2024

Hack for getting ardupilot work for tracking stuff (JUST FOR TESTING) WIP mavlink/qgroundcontrol#11829

Closed

Hwurzburg requested changes Sep 2, 2024

View reviewed changes

khanasif786 changed the title ~~AP_Camera: Add Camera Tracking functionality~~ AP_Camera: Add Camera Tracking functionality (WIP) Sep 11, 2024

srmainwaring mentioned this pull request Sep 21, 2024

AP_Camera: add Python examples for GStreamer UDP and RTSP video streams #28183

Merged

landswellsong reviewed Sep 22, 2024

View reviewed changes

khanasif786 added 4 commits October 2, 2024 16:03

AP_Camera: add Follow functionality for the tracked object

0cee9f7

AP_Follow: add Follow functionality for the tracked object

48b7971

GCS_MAVLink: add Follow functionality for the tracked object

83b995d

ArduCopter: add Follow functionality for the tracked object

d4b3b55

srmainwaring reviewed Oct 3, 2024

View reviewed changes

srmainwaring suggested changes Oct 7, 2024

View reviewed changes

srmainwaring mentioned this pull request Oct 8, 2024

Add camera tracking module ArduPilot/MAVProxy#1469

Draft

srmainwaring reviewed Oct 10, 2024

View reviewed changes


		# gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink sync=false

		gst_pipeline = (

AP_Camera: Add Camera Tracking functionality (WIP) #27773

Are you sure you want to change the base?

AP_Camera: Add Camera Tracking functionality (WIP) #27773

Conversation

khanasif786 commented Aug 7, 2024 • edited by rmackay9 Loading

Choose a reason for hiding this comment

khanasif786 Aug 15, 2024 • edited Loading

Choose a reason for hiding this comment

rmackay9 commented Aug 7, 2024

rmackay9 commented Aug 7, 2024

rmackay9 commented Aug 7, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rmackay9 commented Aug 7, 2024

Choose a reason for hiding this comment

rmackay9 Aug 7, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Hwurzburg left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

khanasif786 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rmackay9 commented Aug 21, 2024 • edited Loading

amilcarlucas commented Aug 31, 2024

Hwurzburg left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

srmainwaring Oct 3, 2024 • edited Loading

Choose a reason for hiding this comment

srmainwaring commented Oct 7, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

khanasif786 commented Aug 7, 2024 •

edited by rmackay9

Loading

khanasif786 Aug 15, 2024 •

edited

Loading

rmackay9 Aug 7, 2024 •

edited

Loading

rmackay9 commented Aug 21, 2024 •

edited

Loading

Hwurzburg left a comment •

edited

Loading

srmainwaring Oct 3, 2024 •

edited

Loading

srmainwaring commented Oct 7, 2024 •

edited

Loading