Merge branch 'main' of https://github.com/RoboMaster-Club/auto-aiming

.gitignore

@@ -0,0 +1 @@
+.vscode/

.vscode/settings.json

@@ -67,6 +67,12 @@
         "typeindex": "cpp",
         "typeinfo": "cpp",
         "variant": "cpp",
-        "bit": "cpp"
+        "bit": "cpp",
+        "any": "cpp",
+        "compare": "cpp",
+        "concepts": "cpp",
+        "numbers": "cpp",
+        "semaphore": "cpp",
+        "stop_token": "cpp"
     }
 }

README.md

@@ -1,6 +1,29 @@
 # auto-aiming
 Revitalized auto-aiming suite for Purdue RoboMaster Club 2024-2025.
 
+![build status](https://github.com/RoboMaster-Club/auto-aiming/actions/workflows/colcon-tests.yml/badge.svg)
+
+## Usage  
+Clone this repository into your `ros2-ws` directory. We provide a `run` script that can be used to build, run, test, and clean the workspace with no hassle. The script supports the following functionalities:
+
+- **Building**
+  - `./auto-aiming/run build`
+- **Launch ROS2 code**
+  - `./auto-aiming/run run <launch_file>`
+  - "launch files" basically run several ROS2 nodes at once. The main one is `video2detector.py`.
+- **Run automated tests (GTest)**
+  - `./auto-aiming/run test`
+- **Clean the workspace (remove build and install folders)**
+  - `./auto-aiming/run clean`
+- _Optional flags_
+  - `--quiet`: Suppresses console output, logs output to `command_output.log`.
+  - `--debug`: Builds with debug flags enabled. Used to display a detection results window and debug logs.
+
+### Example to run the detector:
+```
+./auto-aiming/run --debug --quiet run video2detector.py
+```
+
 ## Overall Suite Requirements
 ### Functional Requirements:
 - [ ] **Detect an enemy armor plate in the camera's FOV.**
@@ -30,27 +53,6 @@ Revitalized auto-aiming suite for Purdue RoboMaster Club 2024-2025.
   - Provide thorough documentation, including doxygen-style comments for functions and README files for modules.
 
 
-
-## Usage  
-Clone this repository into your `ros2-ws` directory. We provide a `run` script that can be used to build, run, test, and clean the workspace with the following options:
-
-- **Build**
-  - `./auto-aiming/run build`
-- **Launch ROS2 code**
-  - `./auto-aiming/run run <launch_file>`
-- **Run automated tests (GTest)**
-  - `./auto-aiming/run test`
-- **Clean the workspace**
-  - `./auto-aiming/run clean`
-- _Optional flags_
-  - `--quiet`: Suppresses console output, logs output to `command_output.log`.
-  - `--debug`: Builds with debug flags enabled.
-
-### Example:
-```
-./auto-aiming/run --debug --quiet run video2detector.py
-```
-
 ## Architecture Diagram
 
 <div style="max-width: 600px; margin: auto;">

run

@@ -32,17 +32,17 @@ clean() {
 build() {
     print_green "[*] Building project with Release configuration and optimization flags."
 
-    build_args="-DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS_RELEASE=-O3"
+    build_args="-DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS_RELEASE=-O3 -DPYTHON_EXECUTABLE=/usr/bin/python3.8"
 
     # Add the DEBUG flag if '--debug' or '-d' was provided
     if [[ "$debug" == "true" ]]; then
         build_args="-DCMAKE_CXX_FLAGS=-DDEBUG $build_args"
     fi
 
     if [[ "$quiet" == "true" ]]; then
-        colcon build --cmake-args $build_args --allow-overriding opencv_armor_detector >> "$LOG_FILE" 2>&1
+        colcon build --symlink-install --cmake-args $build_args --allow-overriding opencv_armor_detector >> "$LOG_FILE" 2>&1
     else
-        colcon build --cmake-args $build_args --allow-overriding opencv_armor_detector
+        colcon build --symlink-install --cmake-args $build_args --allow-overriding opencv_armor_detector
     fi
 
     if [ $? -eq 0 ]; then
@@ -57,18 +57,33 @@ build() {
 
 # Function to run tests for each ROS module (non-recursively at first)
 test() {
-    print_green "Running tests for each ROS module..."
+    print_green "Running tests for ROS modules..."
 
     # Run build first
     build
 
-    # Run tests quietly for both
-    print_green "[*] Running tests..."
+    # Check if a package name is provided
+    local package_name=$1
+
+    # Run tests
+    if [[ -z "$package_name" ]]; then
+        print_green "Running all tests..."
+    else
+        print_green "Running tests for package: $package_name"
+    fi
     if [[ "$quiet" == "true" ]]; then
-        colcon test >> "$LOG_FILE" 2>&1
+        if [[ -n "$package_name" ]]; then
+            colcon test --packages-select "$package_name" >> "$LOG_FILE" 2>&1
+        else
+            colcon test >> "$LOG_FILE" 2>&1
+        fi
         colcon test-result --verbose >> "$LOG_FILE" 2>&1
     else
-        colcon test
+        if [[ -n "$package_name" ]]; then
+            colcon test --packages-select "$package_name"
+        else
+            colcon test
+        fi
         colcon test-result --verbose
     fi
 
@@ -122,7 +137,8 @@ while [[ $# -gt 0 ]]; do
             shift
             ;;
         test)
-            test
+            shift
+            test "$1" # Pass the next argument (package name) to the test function
             shift
             ;;
         run)
@@ -131,8 +147,10 @@ while [[ $# -gt 0 ]]; do
             shift $#
             ;;
         *)
-            echo "Usage: $0 [--quiet] [--debug] {build|clean|test|run <launch_file>}"
+            echo "Usage: $0 [--quiet] [--debug] {build|clean|test [package_name]|run <launch_file>}"
             exit 1
             ;;
     esac
 done
+
+exit 0

src/prm_camera/webcam_publisher/CMakeLists.txt

@@ -3,6 +3,8 @@ project(webcam_publisher)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
 
 
 find_package(ament_cmake_auto REQUIRED)

src/prm_launch/launch/video2detector.py

@@ -7,20 +7,24 @@
 
 def generate_launch_description():
     webcam_publisher = get_package_share_path('webcam_publisher')
-    video_path = "/home/tom/Videos/close.avi" # example, can change to your liking
+    video_path = "/home/user-accounts/public/spintop/moving_but_no_spinning.avi" # example, can change to your liking
     return LaunchDescription([
         Node(
             package='webcam_publisher',
             output='screen',
             emulate_tty=True,
             executable='VideoCaptureNode',
             parameters=[{'source': str(video_path),
-                         'fps': 1,
+                         'fps': 4,
                          'frame_id': 'video',
                          }]    
         ), 
         Node(
             package='opencv_armor_detector',
             executable='OpenCVArmorDetectorNode',  
-        )
+        ),
+        Node(
+            package='pose_estimator',
+            executable='PoseEstimatorNode',
+        ),
     ])

src/prm_vision/opencv_armor_detector/CMakeLists.txt

@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 3.8)
 project(opencv_armor_detector CXX)
 set(CMAKE_CXX_STANDARD 17) # for filesystem support
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
 
 # Dependencies
 find_package(ament_cmake REQUIRED)
@@ -11,6 +13,7 @@ find_package(OpenCV 4.6.0 REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(vision_msgs REQUIRED)
 find_package(ament_index_cpp REQUIRED) # Filesystem search in tests
+find_package(Eigen3 REQUIRED) # Eigen3
 find_package(ament_cmake_gtest REQUIRED)  # GTest
 
 # Include directories
@@ -19,7 +22,7 @@ include_directories(include)
 
 # Build target
 add_library(OpenCVArmorDetector STATIC src/OpenCVArmorDetector.cpp)
-target_link_libraries(OpenCVArmorDetector ${OpenCV_LIBS})
+target_link_libraries(OpenCVArmorDetector ${OpenCV_LIBS} Eigen3::Eigen)
 
 add_executable(OpenCVArmorDetectorNode src/OpenCVArmorDetectorNode.cpp)
 target_link_libraries(OpenCVArmorDetectorNode OpenCVArmorDetector ${OpenCV_LIBS})
@@ -30,6 +33,7 @@ ament_target_dependencies(OpenCVArmorDetectorNode
   image_transport
   cv_bridge
   vision_msgs
+  Eigen3
   ament_index_cpp
 )
 

src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h

@@ -2,9 +2,9 @@
 #define _OPENCVARMORDETECTOR_HPP
 
 #include <vector>
-
-#include "rclcpp/rclcpp.hpp"
 #include <opencv2/opencv.hpp>
+#include <stdio.h>
+#include <math.h>
 
 // Detector Constants
 #define LIGHT_BAR_ANGLE_LIMIT 10.0

src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp

@@ -1,7 +1,3 @@
-#include <stdio.h>
-#include <math.h>
-
-#include <opencv2/opencv.hpp>
 #include "OpenCVArmorDetector.h"
 
 void OpenCVArmorDetector::setConfig(DetectorConfig config)
@@ -40,16 +36,6 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame)
 
     // Detect the armor in the cropped frame
     std::vector<cv::Point2f> points = detectArmorsInFrame(croppedFrame);
-
-    // Print FPS every 500 frames
-    if (_frame_count % 500 == 0 && _frame_count != 0)
-    {
-        // Calculate and print FPS
-        auto current_time = std::chrono::steady_clock::now();
-        double elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_time).count();
-        last_time = current_time;
-        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), " [*] Detecting Armor (%d) FPS: %.2f", _frame_count, 500.0 / (elapsed_time / 1000.0));
-    }
     _frame_count++;
 
 // Display the cropped frame for debugging
@@ -60,13 +46,13 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame)
     const std::string window_name = "Detection Results";
     cv::imshow(window_name, croppedFrame);
 
-    // Update the window title (OpenCV >= 4.5)
+    // Update the window title
     cv::setWindowTitle(window_name,
                        "detected: " + std::to_string(_detected_frame) + " / " +
                            std::to_string(_frame_count) + " (" +
                            std::to_string(_detected_frame * 100 / _frame_count) + "%) and missed: " + std::to_string(_missed_frames) + std::string(" frames"));
 
-    cv::waitKey(1000);
+    cv::waitKey(30);
 #endif
 
     // If we didn't find an armor for a few frames (ROS2 param), reset the search area
@@ -82,10 +68,13 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame)
     {
         // We found an armor, so reset the missed frames and return the keypoints
         _missed_frames = 0;
+        std::vector<cv::Point2f> image_points;
         for (int i = 0; i < 4; i++)
         {
             detected_keypoints[i * 2] = points.at(i).x + _search_area[0];
             detected_keypoints[i * 2 + 1] = points.at(i).y + _search_area[1];
+
+            image_points.emplace_back(cv::Point2f(points.at(i).x + _search_area[0], points.at(i).y + _search_area[1]));
         }
 
         if (_reduce_search_area)
@@ -101,6 +90,12 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame)
             _search_area[2] = std::min(x_max + 50, WIDTH);
             _search_area[3] = std::min(y_max + 50, HEIGHT);
         }
+        else
+        {
+            // Reset the search area to the full frame
+            _reset_search_area = true;
+        }
+
         _detected_frame++;
     }
 

src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetectorNode.cpp

@@ -1,8 +1,6 @@
 #include "OpenCVArmorDetectorNode.hpp"
 
-OpenCVArmorDetectorNode::OpenCVArmorDetectorNode(
-    const rclcpp::NodeOptions &options)
-    : Node("opencv_armor_detector", options)
+OpenCVArmorDetectorNode::OpenCVArmorDetectorNode(const rclcpp::NodeOptions &options) : Node("opencv_armor_detector", options)
 {
   RCLCPP_INFO(get_logger(), "OpenCVArmorDetectorNode has been started.");
 
@@ -34,9 +32,7 @@ void OpenCVArmorDetectorNode::imageTransportInitilization()
                              std::placeholders::_1));
 }
 
-rcl_interfaces::msg::SetParametersResult
-OpenCVArmorDetectorNode::parameters_callback(
-    const std::vector<rclcpp::Parameter> &parameters)
+rcl_interfaces::msg::SetParametersResult OpenCVArmorDetectorNode::parameters_callback(const std::vector<rclcpp::Parameter> &parameters)
 {
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
@@ -72,6 +68,12 @@ OpenCVArmorDetectorNode::parameters_callback(
       RCLCPP_INFO(get_logger(), "New max missed frames: %d",
                   _max_missed_frames);
     }
+    else if (parameter.get_name() == "_reduce_search_area")
+    {
+      _reduce_search_area = parameter.as_bool();
+      RCLCPP_INFO(get_logger(), "New reduce search area: %s",
+                  _reduce_search_area ? "true" : "false");
+    }
     else
     {
       result.successful = false;
@@ -80,7 +82,7 @@ OpenCVArmorDetectorNode::parameters_callback(
   }
 
   // Update the detector's config
-  detector->setConfig({_target_color, _hue_range_limit, _saturation_lower_limit, _value_lower_limit, _max_missed_frames});
+  detector->setConfig({_target_color, _hue_range_limit, _saturation_lower_limit, _value_lower_limit, _max_missed_frames, _reduce_search_area});
   return result;
 }
 
@@ -97,14 +99,12 @@ void OpenCVArmorDetectorNode::imageCallback(
   vision_msgs::msg::KeyPoints keypoints_msg;
   std::array<float, 8> points_array;
   std::copy(points.begin(), points.end(), points_array.begin());
-  float h = std::min(cv::norm(points.at(1) - points.at(0)),
-                     cv::norm(points.at(3) - points.at(2)));
-  float w = cv::norm((points.at(0) + points.at(1)) / 2 -
-                     (points.at(2) + points.at(3)) / 2);
+  float h = std::min(cv::norm(points.at(1) - points.at(0)), cv::norm(points.at(3) - points.at(2)));
+  float w = cv::norm((points.at(0) + points.at(1)) / 2 - (points.at(2) + points.at(3)) / 2);
 
   keypoints_msg.header = image_msg->header;
   keypoints_msg.points = points_array;
-  keypoints_msg.large_armor = (w / h) > 3; // 3 is the width ratio threshold before it is considered a large armor
+  keypoints_msg.is_large_armor = (w / h) > 3.5; // 3.3 is the width ratio threshold before it is considered a large armor
 
   // Publish the message
   keypoints_publisher->publish(keypoints_msg);