Created vision node

toid_vision/include/toid_vision/toid_vision.hpp

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "message_filters/subscriber.hpp"
+#include "message_filters/sync_policies/approximate_time.hpp"
+#include "message_filters/time_synchronizer.hpp"
+#include "opencv2/aruco.hpp"
+#include "opencv2/objdetect.hpp"
+#include "opencv2/opencv.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/camera_info.hpp"
+#include "sensor_msgs/msg/compressed_image.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "tf2/LinearMath/Quaternion.hpp"
+#include "tf2/LinearMath/Transform.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+class NutDetector : public rclcpp::Node
+{
+  using SyncPolicy = message_filters::sync_policies::ApproximateTime<
+    sensor_msgs::msg::CompressedImage, sensor_msgs::msg::CameraInfo>;
+
+public:
+  NutDetector(rclcpp::NodeOptions &options);
+
+public:
+  void compressed_topic_callback(
+    const sensor_msgs::msg::CompressedImage::ConstSharedPtr & msg,
+    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info);
+
+  void process_image(
+    cv::Mat & decodedImage, const std_msgs::msg::Header & header,
+    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & cam_info);
+
+  void extract_markers(cv::Mat & image);
+
+  void get_pose(cv::Vec3d & tvec, cv::Vec3d & rvec, geometry_msgs::msg::Pose & out_pose);
+
+  message_filters::Subscriber<sensor_msgs::msg::CompressedImage> compressed_img_sub_;
+  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_;
+
+  std::vector<std::vector<cv::Point2f>> markerCorners_;
+  std::vector<int> markerIds_;
+  std::vector<std::pair<std::vector<cv::Point2f>, int>> markers_;
+
+  std::string camera_frame_id_ = "camera";
+  bool is_blue_;
+
+  cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
+  cv::Ptr<cv::aruco::Dictionary> dictionary_;
+  
+
+  message_filters::Subscriber<sensor_msgs::msg::Image> img_sub_;
+};
+
+}  // namespace toid