Merge pull request #1 from vortexntnu/tacc

Tacc changes + formatting
vortexntnu · Oct 12, 2024 · a3d07da · a3d07da
2 parents 895226e + 04c98f2
commit a3d07da
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 10 deletions.
diff --git a/aruco-detector/include/aruco_detector/aruco_detector_ros.hpp b/aruco-detector/include/aruco_detector/aruco_detector_ros.hpp
@@ -266,7 +266,9 @@ class ArucoDetectorNode : public rclcpp::Node{
     float marker_size_;
     float xDist_,yDist_;
     std::vector<int64_t> ids_;
-    std::vector<int> ids_detected_;
+    std::vector<int> ids_detected_once_;
+    std::vector<int> ids_detected_secure_;
+    std::unordered_map<int,int> id_detection_counter_;
     cv::Ptr<cv::aruco::Dictionary> dictionary_;
     std::string frame_;
     cv::Ptr<cv::aruco::DetectorParameters> detector_params_;

diff --git a/aruco-detector/src/aruco_detector_ros.cpp b/aruco-detector/src/aruco_detector_ros.cpp
@@ -388,24 +388,46 @@ void aruco_detector::ArucoDetectorNode::imageCallback(const sensor_msgs::msg::Im
     for (size_t i = 0; i < marker_ids.size(); i++)
     {
         int id = marker_ids[i];
+        if(id == 0){
+            continue;
+        }
+        id_detection_counter_[id]+=1;
+
         bool new_id = false;
         static std::string time = ros2TimeToString(msg->header.stamp);
-        if (std::find(ids_detected_.begin(), ids_detected_.end(), id) == ids_detected_.end()) {
-            ids_detected_.push_back(id);
+
+        // If the id is not already in ids_detected_, add it
+        if (std::find(ids_detected_once_.begin(), ids_detected_once_.end(), id) == ids_detected_once_.end()) {
+            ids_detected_once_.push_back(id);
             new_id = true;
-        }
-        if (new_id) {
-             // Define the directory path
-            std::string directory = "detected-aruco-markers/";
-
+            // Define the directory path
+            if(new_id){
+            std::string directory = "detected-aruco-markers-stream/";
+
             // Check if the directory exists, if not, create it
             if (!std::filesystem::exists(directory)) {
                 std::filesystem::create_directory(directory);
             }
-            
+
             // Write to the file in the specified directory
-            writeIntsToFile(directory + "detected_markers" + time + ".csv", ids_detected_);
+            writeIntsToFile(directory + "detected_markers" + time + ".csv", ids_detected_once_);
+            }
+        }
+        // Check if this id has been detected five times
+        if (id_detection_counter_[id] == 5) {
+            ids_detected_secure_.push_back(id);
+            // Define the directory path
+            std::string directory_secure = "detected-aruco-markers-unique/";
+
+            // Check if the directory_secure exists, if not, create it
+            if (!std::filesystem::exists(directory_secure)) {
+                std::filesystem::create_directory(directory_secure);
+            }
+
+            // Write to the file in the specified directory_secure
+            writeIntsToFile(directory_secure + "detected_markers" + time + ".csv", ids_detected_secure_);
         }
+
         cv::Vec3d rvec = rvecs[i];
         cv::Vec3d tvec = tvecs[i];
         tf2::Quaternion quat = rvec_to_quat(rvec);