work on firing and auto aim status

.gitignore

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

src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp

@@ -77,8 +77,6 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame)
             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
@@ -169,8 +167,6 @@ 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/pose_estimator/include/PoseEstimator.h

@@ -34,6 +34,7 @@ class PoseEstimator
 
     // Setters
     void setNumFramesToFireAfter(int num_frames_to_fire_after) { _num_frames_to_fire_after = num_frames_to_fire_after; }
+    void setAllowedMissedFramesBeforeNoFire(int allowed_missed_frames_before_no_fire) { _allowed_missed_frames_before_no_fire = allowed_missed_frames_before_no_fire; }
 
 private:
     // Class methods
@@ -43,6 +44,8 @@ class PoseEstimator
     // Class variables
     int _consecutive_tracking_frames_ctr = 0;
     int _num_frames_to_fire_after = 3;
+    int _allowed_missed_frames_before_no_fire = 15;
+    int _remaining_missed_frames_before_no_fire = 0; // Gets set to 5 when we have a valid pose estimate
     std::chrono::time_point<std::chrono::system_clock> _last_fire_time;
 
     // Validity filter parameters

src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp

@@ -29,7 +29,6 @@ class PoseEstimatorNode : public rclcpp::Node
 
 private:
     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
 
     // Class methods
     void publishZeroPredictedArmor(std_msgs::msg::Header header);

src/prm_vision/pose_estimator/src/PoseEstimator.cpp

@@ -108,10 +108,20 @@ bool PoseEstimator::isValid(float x, float y, float z, std::string &auto_aim_sta
         if (_consecutive_tracking_frames_ctr >= _num_frames_to_fire_after)
         {
             auto_aim_status = "FIRE";
+            _remaining_missed_frames_before_no_fire = _allowed_missed_frames_before_no_fire;
         }
         else
         {
-            auto_aim_status = "TRACKING";
+            // If currently firing, allow for a few more frames to fire
+            if (_remaining_missed_frames_before_no_fire > 0)
+            {
+                _remaining_missed_frames_before_no_fire--;
+                auto_aim_status = "FIRE";
+            }
+            else
+            {
+                auto_aim_status = "TRACKING";
+            }
         }
     }
 

src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp

@@ -9,6 +9,7 @@ PoseEstimatorNode::PoseEstimatorNode(const rclcpp::NodeOptions &options) : Node(
     predicted_armor_publisher = this->create_publisher<vision_msgs::msg::PredictedArmor>("predicted_armor", 10);
 
     // Dynamic parameters
+    pose_estimator->setAllowedMissedFramesBeforeNoFire(this->declare_parameter("_allowed_missed_frames_before_no_fire", 15));
     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));
@@ -64,7 +65,11 @@ rcl_interfaces::msg::SetParametersResult PoseEstimatorNode::parameters_callback(
         {
             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 if (param.get_name() == "_allowed_missed_frames_before_no_fire" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
+        {
+            pose_estimator->setAllowedMissedFramesBeforeNoFire(param.as_int());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_allowed_missed_frames_before_no_fire' updated to: %d", param.as_int());
         }
         else
         {
@@ -82,7 +87,7 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha
     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
     {
         // No armor detected
-        publishZeroPredictedArmor(key_points_msg->header);
+        publishZeroPredictedArmor(key_points_msg->header, "NO_ARMOR");
         return;
     }
 
@@ -105,8 +110,6 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha
     // Publish the predicted armor if the pose is valid (we are tracking or firing)
     if (valid_pose_estimate)
     {
-        _allowed_missed_frames_before_no_fire = 5;
-
         vision_msgs::msg::PredictedArmor predicted_armor_msg;
         predicted_armor_msg.header = key_points_msg->header;
         predicted_armor_msg.x = tvec.at<float>(0);
@@ -121,13 +124,16 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha
         predicted_armor_msg.fire = (new_auto_aim_status == "FIRE");
         predicted_armor_publisher->publish(predicted_armor_msg);
     }
-    else
+
+    else if (new_auto_aim_status == "STOPPING")
     {
-        publishZeroPredictedArmor(key_points_msg->header);
+        publishZeroPredictedArmor(key_points_msg->header, new_auto_aim_status);
     }
+
+    RCLCPP_INFO(get_logger(), "status %s", new_auto_aim_status.c_str());
 }
 
-void PoseEstimatorNode::publishZeroPredictedArmor(std_msgs::msg::Header header)
+void PoseEstimatorNode::publishZeroPredictedArmor(std_msgs::msg::Header header, std::string new_auto_aim_status)
 {
     vision_msgs::msg::PredictedArmor predicted_armor_msg;
     predicted_armor_msg.header = header;
@@ -140,7 +146,7 @@ void PoseEstimatorNode::publishZeroPredictedArmor(std_msgs::msg::Header header)
     predicted_armor_msg.x_vel = 0;
     predicted_armor_msg.y_vel = 0;
     predicted_armor_msg.z_vel = 0;
-    predicted_armor_msg.fire = (_allowed_missed_frames_before_no_fire-- > 0);
+    predicted_armor_msg.fire = (new_auto_aim_status == "FIRE");
 
     predicted_armor_publisher->publish(predicted_armor_msg);
 }