[DRAFT] Add aruco pose estimate to gstreamer pipeline

src/Cameras/video_streaming/CMakeLists.txt

@@ -12,6 +12,11 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(interfaces REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(OpenCV REQUIRED)
 find_package(PkgConfig REQUIRED)
 
 pkg_check_modules(GSTREAMER REQUIRED IMPORTED_TARGET gstreamer-1.0)
@@ -27,6 +32,7 @@ add_library(${PROJECT_NAME} SHARED
   src/base_video_node.cpp
   src/input_node.cpp
   src/detect_node.cpp
+  src/aruco_pose_estimate.cpp
   src/webrtc_node.cpp
   src/srt_node.cpp
   src/srt_node_client.cpp
@@ -55,12 +61,17 @@ target_link_libraries(${PROJECT_NAME}
   PkgConfig::GSTREAMER
   PkgConfig::GST_APP
   PkgConfig::GST_VIDEO
+  ${OpenCV_LIBRARIES}
 )
 ament_target_dependencies(${PROJECT_NAME}
   rclcpp
   rclcpp_components
   interfaces
   std_msgs
+  sensor_msgs
+  geometry_msgs
+  cv_bridge
+  image_transport
 )
 
 # If pkg-config provided extra compile flags (e.g., -D, -pthread)

src/Cameras/video_streaming/include/video_streaming/aruco_pose_estimate.hpp

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <map>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+// --- OpenCV 4.8.0 Specific Headers ---
+#include <opencv2/calib3d.hpp>
+#include <opencv2/objdetect.hpp>
+#include <opencv2/objdetect/aruco_detector.hpp>
+#include <opencv2/objdetect/aruco_dictionary.hpp>
+#include <opencv2/opencv.hpp>
+// -------------------------------------
+
+#include <interfaces/msg/aruco_markers.hpp>
+
+struct CameraIntrinsics {
+  cv::Mat camera_matrix;
+  cv::Mat dist_coeffs;
+};
+
+class ArucoPoseEstimator {
+public:
+  ArucoPoseEstimator(rclcpp::Node *node);
+  void processImage(const cv::Mat &frame, const std::string &camera_name,
+                    const rclcpp::Time &stamp);
+
+private:
+  rclcpp::Node *parent_node_;
+  rclcpp::Publisher<interfaces::msg::ArucoMarkers>::SharedPtr drive_pub_;
+  rclcpp::Publisher<interfaces::msg::ArucoMarkers>::SharedPtr ee_pub_;
+
+  cv::aruco::DetectorParameters detector_params_;
+  cv::aruco::Dictionary dictionary_;
+
+  double marker_length_;
+  std::vector<cv::Point3f> marker_obj_points_;
+
+  // Camera routing and calibration mapping
+  std::unordered_map<std::string, std::string> camera_to_frame_id_;
+  std::unordered_map<std::string, CameraIntrinsics> calibrations_;
+};

src/Cameras/video_streaming/include/video_streaming/detect_node.hpp

@@ -1,7 +1,12 @@
 #pragma once
+#include "aruco_pose_estimate.hpp"
 #include "base_video_node.hpp"
+#include <gst/app/gstappsink.h>
 #include <interfaces/msg/object_detected.hpp>
+#include <memory>
+#include <sensor_msgs/msg/image.hpp>
 #include <std_msgs/msg/int32.hpp>
+#include <std_msgs/msg/string.hpp>
 
 class DetectNode : public BaseVideoNode {
 public:
@@ -18,12 +23,20 @@ class DetectNode : public BaseVideoNode {
 
 private:
   DetectionType detection_type_;
+  std::shared_ptr<ArucoPoseEstimator> aruco_estimator_;
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
   rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr marker_pub_;
   rclcpp::Publisher<interfaces::msg::ObjectDetected>::SharedPtr
       object_detected_pub_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr active_camera_sub_;
+
+  std::string active_camera_{"Drive"};
+
+  void active_camera_callback(const std_msgs::msg::String::SharedPtr msg);
   rcl_interfaces::msg::SetParametersResult
   on_parameter_change(const std::vector<rclcpp::Parameter> &parameters);
   DetectionType string_to_detection_type(const std::string &type_str);
   std::string detection_type_to_string() const;
-};
+
+  static GstFlowReturn on_new_sample(GstAppSink *sink, gpointer user_data);
+};

src/Cameras/video_streaming/include/video_streaming/input_node.hpp

@@ -2,6 +2,7 @@
 #include "base_video_node.hpp"
 #include <interfaces/srv/get_cameras.hpp>
 #include <interfaces/srv/video_out.hpp>
+#include <std_msgs/msg/string.hpp> // Added to fix the compiler error
 
 class InputNode : public BaseVideoNode {
 public:
@@ -30,7 +31,8 @@ class InputNode : public BaseVideoNode {
 
 private:
   std::map<std::string, size_t> source_map_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr active_camera_pub_;
   rclcpp::Service<interfaces::srv::VideoOut>::SharedPtr video_service_;
   rclcpp::Service<interfaces::srv::GetCameras>::SharedPtr get_cam_service_;
   std::shared_ptr<interfaces::srv::VideoOut::Request> current_video_request_;
-};
+};

src/Cameras/video_streaming/package.xml

@@ -13,6 +13,11 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>opencv</depend>
   <depend>gstreamer-1.0</depend>
   <depend>glib-2.0</depend>
   <depend>libssl-dev</depend>

src/Cameras/video_streaming/src/aruco_pose_estimate.cpp

@@ -0,0 +1,116 @@
+#include "aruco_pose_estimate.hpp"
+#include <geometry_msgs/msg/point.hpp>
+
+ArucoPoseEstimator::ArucoPoseEstimator(rclcpp::Node *node)
+    : parent_node_(node) {
+  drive_pub_ = parent_node_->create_publisher<interfaces::msg::ArucoMarkers>(
+      "/computer_vision/drive_nav_markers", 10);
+  ee_pub_ = parent_node_->create_publisher<interfaces::msg::ArucoMarkers>(
+      "/computer_vision/ee_nav_markers", 10);
+
+  dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
+  detector_params_ = cv::aruco::DetectorParameters();
+
+  marker_length_ =
+      parent_node_->declare_parameter("marker_size", 0.15); // 15cm default
+
+  float half_l = marker_length_ / 2.0f;
+  marker_obj_points_ = {
+      cv::Point3f(-half_l, half_l, 0), cv::Point3f(half_l, half_l, 0),
+      cv::Point3f(half_l, -half_l, 0), cv::Point3f(-half_l, -half_l, 0)};
+
+  // -------------------------------------------------------------
+  // Define frame IDs map based on active camera string
+  // -------------------------------------------------------------
+  camera_to_frame_id_["Drive"] = "DriveCamera";
+  camera_to_frame_id_["EndEffector"] = "EndEffector";
+
+  // -------------------------------------------------------------
+  // Define Intrinsic Calibrations
+  // -------------------------------------------------------------
+
+  // Drive Camera
+  CameraIntrinsics drive_calib;
+  drive_calib.camera_matrix = (cv::Mat_<double>(3, 3) << 1208.455865000, 0,
+                               953.045426800, 0, 1201.369845000,
+                               590.105458700, 0, 0, 1);
+  drive_calib.dist_coeffs =
+      (cv::Mat_<double>(1, 8)
+           << -1.041143259e-01, -8.638070390e-03, 1.363747596e-03,
+       -2.526459586e-05, 4.666582799e-02, 1.310149040e-04,
+       -5.928740585e-04, 4.767364821e-02);
+  calibrations_["Drive"] = drive_calib;
+
+  // End Effector Camera  (High Distortion)
+  CameraIntrinsics ee_calib;
+  ee_calib.camera_matrix = (cv::Mat_<double>(3, 3) << 234.2702, 0, 346.4237, 0,
+                            255.8694, 287.5284, 0, 0, 1);
+  ee_calib.dist_coeffs = (cv::Mat_<double>(1, 8) << 0.6030, 0.4734, -0.0011,
+                          -0.0004, -0.0708, 0.3530, 0.6553, -0.0759);
+  calibrations_["EndEffector"] = ee_calib;
+}
+
+void ArucoPoseEstimator::processImage(const cv::Mat &frame,
+                                      const std::string &camera_name,
+                                      const rclcpp::Time &stamp) {
+  try {
+    // Resolve Frame ID
+    std::string frame_id = camera_name; // Fallback
+    if (camera_to_frame_id_.find(camera_name) != camera_to_frame_id_.end()) {
+      frame_id = camera_to_frame_id_[camera_name];
+    }
+
+    // Fetch correct calibration
+    if (calibrations_.find(camera_name) == calibrations_.end()) {
+      RCLCPP_WARN_THROTTLE(
+          parent_node_->get_logger(), *parent_node_->get_clock(), 2000,
+          "No calibration found for camera: %s. Skipping Aruco.",
+          camera_name.c_str());
+      return;
+    }
+
+    cv::Mat cam_mat = calibrations_[camera_name].camera_matrix;
+    cv::Mat dist = calibrations_[camera_name].dist_coeffs;
+
+    cv::Mat undistorted;
+    cv::undistort(frame, undistorted, cam_mat, dist);
+
+    cv::aruco::ArucoDetector detector(dictionary_, detector_params_);
+    std::vector<int> markerIds;
+    std::vector<std::vector<cv::Point2f>> markerCorners, rejectedCandidates;
+    detector.detectMarkers(undistorted, markerCorners, markerIds,
+                           rejectedCandidates);
+
+    interfaces::msg::ArucoMarkers msg;
+    msg.header.stamp = stamp;
+    msg.header.frame_id = frame_id;
+
+    if (!markerIds.empty()) {
+      for (size_t i = 0; i < markerIds.size(); i++) {
+        msg.marker_ids.push_back(markerIds[i]);
+
+        cv::Vec3d rvec, tvec;
+        bool success =
+            cv::solvePnP(marker_obj_points_, markerCorners[i], cam_mat, dist,
+                         rvec, tvec, false, cv::SOLVEPNP_IPPE_SQUARE);
+
+        geometry_msgs::msg::Point pt;
+        if (success) {
+          pt.x = tvec[0];
+          pt.y = tvec[1];
+          pt.z = tvec[2];
+        }
+        msg.points.push_back(pt);
+        msg.is_moving.push_back(false);
+      }
+    }
+
+    if (camera_name == "EndEffector") {
+      ee_pub_->publish(msg);
+    } else {
+      drive_pub_->publish(msg);
+    }
+  } catch (const std::exception &e) {
+    RCLCPP_WARN(parent_node_->get_logger(), "Aruco logic failed: %s", e.what());
+  }
+}