various fixes

toid_vision/src/toid_vision.cpp

@@ -37,7 +37,7 @@ NutDetector::NutDetector(const rclcpp::NodeOptions & options)
   RCLCPP_INFO(this->get_logger(), "NutDetector activated... scanning for DEEZ NUTZ");
 
   pose_pub_ =
-    this->create_publisher<geometry_msgs::msg::PoseStamped>("/closest_acorn", rclcpp::QoS(5));
+    this->create_publisher<geometry_msgs::msg::PoseStamped>("/closest_acorn", rclcpp::QoS(1).keep_last(1));
   flip_pub_ = this->create_publisher<std_msgs::msg::String>("/to_flip", rclcpp::QoS(1));
 }
 
@@ -80,12 +80,16 @@ void NutDetector::process_image(
 
   size_t prev = 0;
   std::string colors = "";
+  size_t first_ok = -1;
 
   for (size_t i = 0; i < markerIds_.size(); i++) {
     const int idx = markers_[i].second;
     if (markerIds_[idx] != 47 && markerIds_[idx] != 36) {
       continue;
     }
+    if (first_ok == (size_t)-1) {
+      first_ok = idx;
+    }
 
     RCLCPP_DEBUG(
       this->get_logger(), "Marker %ld at %lf %lf", i, markerCorners_[idx][0].x,
@@ -131,6 +135,10 @@ void NutDetector::process_image(
     flip_pub_->publish(m);
   }
 
+  if (first_ok == (size_t)-1) {
+    return;
+  }
+
   geometry_msgs::msg::PoseStamped pose;
   get_pose(tvecs[markers_[0].second], rvecs[markers_[0].second], pose.pose);
   pose.header.frame_id = camera_frame_id_;