validityfilter and pose estimator variables as ros2 params

Author: iBoot32

src/prm_launch/launch/video2detector.py

@@ -15,7 +15,7 @@ def generate_launch_description():
             emulate_tty=True,
             executable='VideoCaptureNode',
             parameters=[{'source': str(video_path),
-                         'fps': 1,
+                         'fps': 4,
                          'frame_id': 'video',
                          }]    
         ), 

src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp

@@ -92,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++;
     }
 

src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetectorNode.cpp

@@ -68,6 +68,12 @@ rcl_interfaces::msg::SetParametersResult OpenCVArmorDetectorNode::parameters_cal
       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;
@@ -76,7 +82,7 @@ rcl_interfaces::msg::SetParametersResult OpenCVArmorDetectorNode::parameters_cal
   }
 
   // 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;
 }
 

src/prm_vision/pose_estimator/include/PoseEstimator.h

@@ -25,14 +25,18 @@ class PoseEstimator
     PoseEstimator() {}
     ~PoseEstimator() {}
 
+    ValidityFilter validity_filter_ = ValidityFilter();
+
     // Class methods
     double estimateYaw(const double yaw_guess, const std::vector<cv::Point2f> &image_points, const cv::Mat &tvec);
     void estimateTranslation(const std::vector<cv::Point2f> &image_points, bool largeArmor, cv::Mat &tvec, cv::Mat &rvec);
     bool isValid(float x, float y, float z, std::string &auto_aim_status, bool &reset_kalman);
 
+    // Setters
+    void setNumFramesToFireAfter(int num_frames_to_fire_after) { _num_frames_to_fire_after = num_frames_to_fire_after; }
+
 private:
     // Class methods
-    ValidityFilter validity_filter_ = ValidityFilter();
     double computeLoss(double yaw_guess, std::vector<cv::Point2f> image_points, cv::Mat tvec);
     double gradientWrtYawFinitediff(double yaw, std::vector<cv::Point2f> image_points, cv::Mat tvec);
 
@@ -41,6 +45,13 @@ class PoseEstimator
     int _num_frames_to_fire_after = 3;
     std::chrono::time_point<std::chrono::system_clock> _last_fire_time;
 
+    // Validity filter parameters
+    int _lock_in_after = 3;
+    float _max_distance = 10000;
+    float _min_distance = 10;
+    float _max_shift_distance = 150;
+    int _prev_len = 5;
+
     /**
      * @brief Functor for the loss function and gradient computation.
      *

src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp

@@ -17,16 +17,17 @@
 
 // Pose Estimator classes
 #include "PoseEstimator.h"
-#include "ValidityFilter.hpp"
 
 class PoseEstimatorNode : public rclcpp::Node
 {
 public:
     PoseEstimatorNode(const rclcpp::NodeOptions &options);
     ~PoseEstimatorNode();
 
+    PoseEstimator *pose_estimator = new PoseEstimator();
+    ValidityFilter &validity_filter_ = pose_estimator->validity_filter_; // Reference to the validity filter
+
 private:
-    PoseEstimator *pose_estimator;
     double _last_yaw_estimate = 0.0;
     int _allowed_missed_frames_before_no_fire = 0; // Gets set to 5 when we have a valid pose estimate
 
@@ -37,6 +38,8 @@ class PoseEstimatorNode : public rclcpp::Node
     rclcpp::Subscription<vision_msgs::msg::KeyPoints>::SharedPtr key_points_subscriber;
     std::shared_ptr<rclcpp::Publisher<vision_msgs::msg::PredictedArmor>> predicted_armor_publisher;
     void keyPointsCallback(const vision_msgs::msg::KeyPoints::SharedPtr msg);
+    OnSetParametersCallbackHandle::SharedPtr params_callback_handle_;
+    rcl_interfaces::msg::SetParametersResult parameters_callback(const std::vector<rclcpp::Parameter> &parameters);
 };
 
 #endif // POSE_ESTIMATOR_NODE_HPP

src/prm_vision/pose_estimator/include/ValidityFilter.hpp

@@ -17,9 +17,8 @@ enum ValidityFilterState
 class ValidityFilter
 {
 public:
-    ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, float prev_len);
+    ValidityFilter(int lock_in_after = 3, float max_distance = 10000, float min_distance = 10, float max_shift_distance = 150, int prev_len = 5);
 
-    ValidityFilter();
     ~ValidityFilter();
 
     bool shouldResetKalman(float x, float y, float z);
@@ -35,26 +34,33 @@ class ValidityFilter
     float *getPrevX() { return _prev_x; }
     float *getPrevY() { return _prev_y; }
     float *getPrevZ() { return _prev_z; }
+    int getLockInAfter() { return _lock_in_after; }
+
+    // Setters
+    void setLockInAfter(int lock_in_after) { _lock_in_after = lock_in_after; }
+    void setMaxDistance(float max_distance) { _max_distance = max_distance; }
+    void setMinDistance(float min_distance) { _min_distance = min_distance; }
+    void setMaxShiftDistance(float max_shift_distance) { _max_shift_distance = max_shift_distance; }
+    void setPrevLen(int prev_len) { _prev_len = prev_len; }
+    void setMaxDt(double max_dt) { _max_dt = max_dt; }
 
 protected:
-    int _lock_in_after = 3; // lock in after n frames
+    int _lock_in_after;        // lock in after n frames
+    float _max_distance;       // mm
+    float _min_distance;       // mm
+    float _max_shift_distance; // mm
+    int _prev_len;             // check the back n frames for max shift distance vilolation
+
     int _lock_in_counter = 0;
 
     // zero time point
     std::chrono::steady_clock::time_point _last_valid_time = std::chrono::steady_clock::time_point::min();
 
-    float _max_distance = 10000.f; // mm
-    float _min_distance = 10.f;    // mm
-
-    int _prev_len = 5; // check the back n frames for max shift distance vilolation
-
     float _prev_x[20];
     float _prev_y[20];
     float _prev_z[20];
     int _prev_idx = 0;
 
-    float _max_shift_distance = 150.f; // mm
-
     void incrementLockInCounter();
     void decrementLockInCounter();
     void resetLockInCounter();

src/prm_vision/pose_estimator/src/PoseEstimator.cpp

@@ -93,8 +93,8 @@ bool PoseEstimator::isValid(float x, float y, float z, std::string &auto_aim_sta
     // We have enough valid detections to track the target
     else if (validity_filter_.state == TRACKING)
     {
-        // if validity filter valid for last 3 frames, increment locked_in_frames
-        if (validity_filter_.getLockInCounter() == 3)
+        // if validity filter valid for last number of frames, increment locked_in_frames
+        if (validity_filter_.getLockInCounter() == validity_filter_.getLockInAfter())
         {
             _consecutive_tracking_frames_ctr++;
         }

src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp

@@ -4,16 +4,79 @@ PoseEstimatorNode::PoseEstimatorNode(const rclcpp::NodeOptions &options) : Node(
 {
     RCLCPP_INFO(get_logger(), "PoseEstimatorNode has been started.");
 
-    // Initialize the pose estimator
-    pose_estimator = new PoseEstimator();
-
     // Callbacks and pub/sub
     key_points_subscriber = this->create_subscription<vision_msgs::msg::KeyPoints>("key_points", 10, std::bind(&PoseEstimatorNode::keyPointsCallback, this, std::placeholders::_1));
     predicted_armor_publisher = this->create_publisher<vision_msgs::msg::PredictedArmor>("predicted_armor", 10);
+
+    // Dynamic parameters
+    pose_estimator->setNumFramesToFireAfter(this->declare_parameter("_num_frames_to_fire_after", 3));
+    validity_filter_.setLockInAfter(this->declare_parameter("_lock_in_after", 3));
+    validity_filter_.setMaxDistance(this->declare_parameter("_max_distance", 10000));
+    validity_filter_.setMinDistance(this->declare_parameter("_min_distance", 10));
+    validity_filter_.setMaxShiftDistance(this->declare_parameter("_max_shift_distance", 150));
+    validity_filter_.setPrevLen(this->declare_parameter("_prev_len", 5));
+    validity_filter_.setMaxDt(this->declare_parameter("_max_dt", 2000.0));
+    params_callback_handle_ = this->add_on_set_parameters_callback(std::bind(&PoseEstimatorNode::parameters_callback, this, std::placeholders::_1));
 }
 
 PoseEstimatorNode::~PoseEstimatorNode() { delete pose_estimator; }
 
+rcl_interfaces::msg::SetParametersResult PoseEstimatorNode::parameters_callback(
+    const std::vector<rclcpp::Parameter> &parameters)
+{
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    result.reason = "Parameters successfully updated.";
+
+    for (const auto &param : parameters)
+    {
+        if (param.get_name() == "_num_frames_to_fire_after" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
+        {
+            pose_estimator->setNumFramesToFireAfter(param.as_int());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_num_frames_to_fire_after' updated to: %d", param.as_int());
+        }
+        else if (param.get_name() == "_lock_in_after" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
+        {
+            validity_filter_.setLockInAfter(param.as_int());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_lock_in_after' updated to: %d", param.as_int());
+        }
+        else if (param.get_name() == "_max_distance" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
+        {
+            validity_filter_.setMaxDistance(param.as_int());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_max_distance' updated to: %d", param.as_int());
+        }
+        else if (param.get_name() == "_min_distance" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
+        {
+            validity_filter_.setMinDistance(param.as_int());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_min_distance' updated to: %d", param.as_int());
+        }
+        else if (param.get_name() == "_max_shift_distance" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
+        {
+            validity_filter_.setMaxShiftDistance(param.as_int());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_max_shift_distance' updated to: %d", param.as_int());
+        }
+        else if (param.get_name() == "_prev_len" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
+        {
+            validity_filter_.setPrevLen(param.as_int());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_prev_len' updated to: %d", param.as_int());
+        }
+        else if (param.get_name() == "_max_dt" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE || param.get_type())
+        {
+            validity_filter_.setMaxDt(param.as_double());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_max_dt' updated to: %f", param.as_double());
+            RCLCPP_INFO(this->get_logger(), "max dt is %f", validity_filter_._max_dt);
+        }
+        else
+        {
+            result.successful = false;
+            result.reason = "Invalid parameter or type.";
+            RCLCPP_WARN(this->get_logger(), "Failed to update parameter: %s", param.get_name().c_str());
+        }
+    }
+
+    return result;
+}
+
 void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::SharedPtr key_points_msg)
 {
     if (key_points_msg->points.size() != 8 || key_points_msg->points[4] == 0) // idx 4 is x-coord of top right corner, so if it's 0, we know there's no armor

src/prm_vision/pose_estimator/src/ValidityFilter.cpp

@@ -1,12 +1,12 @@
 #include "ValidityFilter.hpp"
 
-ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, float prev_len)
+ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, int prev_len)
 {
     this->_lock_in_after = lock_in_after;
     this->_max_distance = max_distance;
     this->_min_distance = min_distance;
     this->_max_shift_distance = max_shift_distance;
-    this->_prev_len = std::min(prev_len, 20.0f);
+    this->_prev_len = std::min(prev_len, 20);
 
     // Initialize arrays to track previous predictions
     for (int i = 0; i < this->_prev_len; i++)
@@ -17,7 +17,6 @@ ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_
     }
 }
 
-ValidityFilter::ValidityFilter() {}
 ValidityFilter::~ValidityFilter() {}
 
 /**