AP_camera: Add example scripts for camera tracking

libraries/AP_Camera/examples/ReadMe.md

@@ -0,0 +1,79 @@
+
+# Real-Time Object Tracking with Gimbal Control (Simulation)
+
+## Overview
+
+## Requirements and Installations
+1. We assume you have already setup Ardupilot with SITL and gazebo simulation
+2. Install Gstreamer
+
+```
+sudo apt update
+```
+
+Install build tools and Python development libraries and GStreamer related plugins
+```
+
+sudo apt install -y build-essential cmake git pkg-config libgtk-3-dev libavcodec-dev libavformat-dev libswscale-dev
+
+sudo apt install -y python3-dev python3-numpy python3-pip
+
+sudo apt install -y libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev \
+                    gstreamer1.0-plugins-good gstreamer1.0-plugins-bad \
+                    gstreamer1.0-plugins-ugly gstreamer1.0-libav gstreamer1.0-tools
+```
+
+3. default opencv from pip should be uninstalled and again built with gstreamer support (If already did this go to next step)
+```
+git clone https://github.com/opencv/opencv.git
+```
+
+```
+git clone https://github.com/opencv/opencv_contrib.git
+```
+
+```
+cd opencv
+mkdir build
+cd build
+```
+
+Build Configurations
+```
+cmake -D CMAKE_BUILD_TYPE=RELEASE \
+      -D CMAKE_INSTALL_PREFIX=/usr/local \
+      -D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules \
+      -D PYTHON3_EXECUTABLE=$(which python3) \
+      -D PYTHON3_INCLUDE_DIR=$(python3 -c "from distutils.sysconfig import get_python_inc; print(get_python_inc())") \
+      -D PYTHON3_LIBRARY=$(python3 -c "from distutils.sysconfig import get_config_var; print(get_config_var('LIBDIR'))") \
+      -D PYTHON3_PACKAGES_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib())") \
+      -D WITH_GSTREAMER=ON \
+      -D WITH_FFMPEG=ON \
+      -D BUILD_opencv_python3=ON \
+      -D BUILD_EXAMPLES=OFF ..
+
+```
+
+```
+make -j$(nproc)
+```
+
+```
+sudo make install
+sudo ldconfig
+```
+
+
+
+## Description
+There are two main Python scripts:
+
+1. **`tracking.py`**: This script handles video streaming and object tracking. It uses OpenCV to process video frames, applies an object tracking algorithm, and sends commands to the gimbal to adjust its orientation based on the object's position in the frame.
+
+2. **`send_camera_information.py`**: This script communicates with the UAV's autopilot system using MAVLink. It sends the necessary camera and gimbal information to ensure the UAV's camera is correctly oriented.
+
+
+## Runing the scripts
+just run
+
+  ```bash run_tracking.sh```

libraries/AP_Camera/examples/run_tracking.bash

@@ -0,0 +1,26 @@
+#!/bin/bash
+
+cleanup() {
+    echo "Terminating background processes..."
+    kill -9 $pid1 $pid2 2>/dev/null
+    exit 1
+}
+
+trap cleanup SIGINT
+
+# Start the first Python script in the background
+python3 tracking.py &
+
+# Capture the process ID of the first script
+pid1=$!
+
+# Start the second Python script in the background
+python3 send_camera_information.py --sysid 245 --compid 0 --resh 640 --resv 480 &
+
+# Capture the process ID of the second script
+pid2=$!
+
+# Wait for both scripts to complete
+wait $pid1 $pid2
+
+echo "Scripts Ended Cleanly"

libraries/AP_Camera/examples/send_camera_information.py

@@ -20,7 +20,7 @@
 CAMERA_CAP_FLAGS_HAS_TRACKING_GEO_STATUS = 2048             # Camera supports tracking geo status
 
 class CameraTrackingScript:
-    def __init__(self, ip, port, sysid, compid):
+    def __init__(self, ip, port, sysid, compid, resh, resv):
         self.ip = ip
         self.port = port
         self.sysid = sysid
@@ -29,6 +29,8 @@ def __init__(self, ip, port, sysid, compid):
         self.udp_ip = "127.0.0.1"  # Localhost
         self.udp_port = 14580      # Port to send to
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.resh = resh
+        self.resv = resv
 
     def connect_to_mavlink(self):
         self.connection = mavutil.mavlink_connection(f'udp:{self.ip}:{self.port}', source_system=self.sysid)
@@ -70,8 +72,8 @@ def send_camera_information(self):
             float('nan'),                                  # focal_length
             float('nan'),                                  # sensor_size_h
             float('nan'),                                  # sensor_size_v
-            640,                                           # resolution_h
-            480,                                           # resolution_v
+            self.resh,                                     # resolution_h
+            self.resv,                                     # resolution_v
             0,                                             # lens_id
             flags,                                         # flags
             0,                                             # cam_definition_version
@@ -133,15 +135,17 @@ def run(self):
                     print("Received but not for us")
 
 def main():
-    parser = argparse.ArgumentParser(description="A script to demonstrate command-line arguments for sysid and compid.")
+    parser = argparse.ArgumentParser(description="A script to send camera information and Forward track rectangle and track point messages to tracking script")
     parser.add_argument('--sysid', type=int, help='System ID', required=True)
     parser.add_argument('--compid', type=int, help='Component ID', required=True)
+    parser.add_argument('--resh', type=int, help='Video Resolution horizontal', required=True)
+    parser.add_argument('--resv', type=int, help='Video Resolution vertical', required=True)
     args = parser.parse_args()
 
     ip = "127.0.0.1"
-    port = 14570
+    port = 14560
 
-    script = CameraTrackingScript(ip, port, args.sysid, args.compid)
+    script = CameraTrackingScript(ip, port, args.sysid, args.compid, args.resh, args.resv)
     script.run()
 
 if __name__ == "__main__":

libraries/AP_Camera/examples/tracking.py

@@ -69,7 +69,7 @@ def __init__(self, input_port, output_port):
         self.out = self.initialize_video_writer()
         self.frame = None
 
-        self.tracker = cv2.legacy.TrackerKCF_create()  # Change to legacy namespace
+        self.tracker = cv2.legacy.TrackerCSRT_create()  # Change to legacy namespace
         self.initBB = None
         self.last_change_time = 0
 
@@ -99,7 +99,7 @@ def initialize_video_writer(self):
 
     def update_tracker(self, new_roi):
         self.initBB = new_roi
-        self.tracker = cv2.legacy.TrackerKCF_create()  # Change to legacy namespace
+        self.tracker = cv2.legacy.TrackerCSRT_create()  # Change to legacy namespace
         if self.frame is not None:
             self.tracker.init(self.frame, new_roi)
 
@@ -115,14 +115,14 @@ def process_frame(self, gimbal_control):
                 (success, box) = self.tracker.update(self.frame)
                 if success:
                     (x, y, w, h) = [int(v) for v in box]
-                    center_x = x + ((w) // 2)
-                    center_y = y + ((h) // 2)
+                    center_x = x + (w // 2)
+                    center_y = y + (h // 2)
                     gimbal_control.update_center(center_x, center_y)
                     cv2.rectangle(self.frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
                     self.last_change_time = time.time()
                 else:
-                    if time.time() - self.last_change_time >= 0.2:
-                        gimbal_control.update_center(0, 0)
+                    print("Tracking failure detected, reinitializing tracker.")
+                    self.reinitialize_tracker(gimbal_control)
 
             # Write the frame to the RTSP stream
             self.out.write(self.frame)
@@ -135,6 +135,17 @@ def process_frame(self, gimbal_control):
 
         self.cleanup()
 
+    def reinitialize_tracker(self, gimbal_control):
+        """Reinitialize tracker on loss."""
+        gimbal_control.update_center(0, 0)
+        self.initBB = None  # Reset the bounding box
+        # Optionally, use object detection to find the new location of the object and update tracker
+
+        # Example: Use an object detection method to reinitialize the bounding box
+        # detection_box = self.object_detection_method(self.frame)
+        # if detection_box:
+        #     self.update_tracker(detection_box)
+
     def cleanup(self):
         self.cap.release()
         self.out.release()
@@ -162,11 +173,11 @@ def receive_udp_data(self):
 
 
 def main():
-    gimbal_control = GimbalControl('127.0.0.1:14560')
+    gimbal_control = GimbalControl('127.0.0.1:14570')
     video_streamer = VideoStreamer(input_port=5600, output_port=5700)
     udp_receiver = UDPReceiver(gimbal_control, video_streamer)
 
     video_streamer.process_frame(gimbal_control)
 
 if __name__ == "__main__":
-    main()
+    main()