Add PnP Solving, Validity Filter, and Yaw Estimator

.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"
     }
 }

run

@@ -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/tom/Videos/far_back_spin_and_move.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,12 +68,17 @@ 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]));
         }
 
+        // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), " [*] Detected Armor: (%.2f, %.2f), (%.2f, %.2f), (%.2f, %.2f), (%.2f, %.2f)", image_points.at(0).x, image_points.at(0).y, image_points.at(1).x, image_points.at(1).y, image_points.at(2).x, image_points.at(2).y, image_points.at(3).x, image_points.at(3).y);
+
         if (_reduce_search_area)
         {
             // Change the search area to the bounding box of the armor with a 50 pixel buffer
@@ -101,6 +92,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++;
     }
 
@@ -172,6 +169,8 @@ std::vector<cv::Point2f> OpenCVArmorDetector::detectArmorsInFrame(cv::Mat &frame
                 rect.angle -= 90;
             }
 
+            // cv::RotatedRect rect = cv::fitEllipse(contour);
+
             if (isLightBar(rect))
             {
                 light_bar_candidates.push_back(rect);

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; // 3 is the width ratio threshold before it is considered a large armor
 
   // Publish the message
   keypoints_publisher->publish(keypoints_msg);

src/prm_vision/pose_estimator/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.8)
+project(pose_estimator CXX)
+set(CMAKE_CXX_STANDARD 17) # for filesystem support
+
+# Dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(vision_msgs REQUIRED)
+find_package(OpenCV 4.6.0 REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(ament_cmake_gtest REQUIRED)  # GTest
+find_package(Eigen3 REQUIRED) # Eigen3
+
+# Include directories
+include_directories(/usr/include)
+include_directories(include)
+
+# Build target
+add_library(PoseEstimator STATIC src/PoseEstimator.cpp)
+target_link_libraries(PoseEstimator ${OpenCV_LIBS} Eigen3::Eigen)
+
+add_library(ValidityFilter STATIC src/ValidityFilter.cpp)
+
+add_executable(PoseEstimatorNode src/PoseEstimatorNode.cpp)
+target_link_libraries(PoseEstimatorNode PoseEstimator ValidityFilter ${OpenCV_LIBS})
+
+ament_target_dependencies(PoseEstimatorNode 
+  rclcpp
+  sensor_msgs
+  cv_bridge
+  tf2
+  tf2_ros
+  vision_msgs
+  Eigen3
+)
+
+ament_target_dependencies(PoseEstimator
+  rclcpp
+)
+
+# GTest setup
+ament_add_gtest(test_PoseEstimator test/test_PoseEstimator.cpp)
+ament_add_gtest(test_ValidityFilter test/test_ValidityFilter.cpp)
+target_include_directories(test_PoseEstimator PRIVATE include)
+target_include_directories(test_ValidityFilter PRIVATE include)
+target_link_libraries(test_PoseEstimator PoseEstimator)
+target_link_libraries(test_ValidityFilter ValidityFilter)
+
+# Install the node
+install(TARGETS PoseEstimatorNode
+  DESTINATION lib/${PROJECT_NAME})
+
+ament_export_include_directories(include)
+ament_export_dependencies(rclcpp sensor_msgs cv_bridge vision_msgs Eigen3 tf2)
+ament_package()