Formating + debug output

toid_vision/include/toid_vision/toid_vision.hpp

@@ -18,6 +18,9 @@
 #include "tf2/LinearMath/Transform.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
+#include "tf2_ros/transform_listener.hpp"
+#include "tf2_ros/buffer.hpp"
+
 namespace toid
 {
 
@@ -27,7 +30,7 @@ class NutDetector : public rclcpp::Node
     sensor_msgs::msg::CompressedImage, sensor_msgs::msg::CameraInfo>;
 
 public:
-  NutDetector(const rclcpp::NodeOptions &options);
+  NutDetector(const rclcpp::NodeOptions & options);
 
 public:
   void compressed_topic_callback(
@@ -46,7 +49,6 @@ public:
   message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cam_info_sub_;
   std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;
 
-
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr flip_pub_;
 
@@ -60,8 +62,10 @@ public:
   cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
   cv::Ptr<cv::aruco::Dictionary> dictionary_;
 
-
   message_filters::Subscriber<sensor_msgs::msg::Image> img_sub_;
+
+  tf2_ros::Buffer::SharedPtr tf_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 };
 
 }  // namespace toid

toid_vision/src/toid_vision.cpp

@@ -2,6 +2,7 @@
 
 #include <string>
 #include <vector>
+#include "tf2/utils.hpp"
 
 namespace toid
 {
@@ -12,6 +13,9 @@ NutDetector::NutDetector(const rclcpp::NodeOptions & options)
   detectorParams_ = cv::aruco::DetectorParameters::create();
   dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_100);
 
+  tf_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+
   bool use_compressed = true;
   this->declare_parameter("use_compressed", rclcpp::ParameterValue(true));
   this->get_parameter("use_compressed", use_compressed);
@@ -34,8 +38,7 @@ NutDetector::NutDetector(const rclcpp::NodeOptions & options)
 
   pose_pub_ =
     this->create_publisher<geometry_msgs::msg::PoseStamped>("/closest_acorn", rclcpp::QoS(1));
-flip_pub_ = 
-    this->create_publisher<std_msgs::msg::String>("/to_flip", rclcpp::QoS(1));
+  flip_pub_ = this->create_publisher<std_msgs::msg::String>("/to_flip", rclcpp::QoS(1));
 }
 
 void NutDetector::compressed_topic_callback(
@@ -114,11 +117,11 @@ void NutDetector::process_image(
 
   if (colors.size() == 4) {
     RCLCPP_DEBUG(this->get_logger(), "Last seen orientation: %s", colors.c_str());
-    for(int i = 0; i<4; i++) {
-        if(colors[i] == 'b') {
-            colors[i] = (is_blue_)? '0' : '1';
+    for (int i = 0; i < 4; i++) {
+      if (colors[i] == 'b') {
+        colors[i] = (is_blue_) ? '0' : '1';
       } else {
-            colors[i] = (is_blue_)? '1' : '0';
+        colors[i] = (is_blue_) ? '1' : '0';
       }
     }
 
@@ -133,6 +136,18 @@ void NutDetector::process_image(
   pose.header.frame_id = camera_frame_id_;
   pose.header.stamp = header.stamp;
   pose_pub_->publish(pose);
+
+  try {
+    geometry_msgs::msg::PoseStamped out_pose;
+    out_pose = tf_->transform(pose, "base_footprint");
+    RCLCPP_DEBUG_THROTTLE(
+      this->get_logger(), *this->get_clock(), 2000, "Closest guy is at: %lf %lf %lf",
+      out_pose.pose.position.x, out_pose.pose.position.y, tf2::getYaw(out_pose.pose.orientation)
+    );
+  } catch (const tf2::TransformException & e) {
+    RCLCPP_ERROR_THROTTLE(
+      this->get_logger(), *this->get_clock(), 2000, "Failed to transfrom point");
+  }
 }
 
 void NutDetector::extract_markers(cv::Mat & image)