71 lines
2.2 KiB
C++
71 lines
2.2 KiB
C++
#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"
|
|
|
|
#include "tf2_ros/transform_listener.hpp"
|
|
#include "tf2_ros/buffer.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(const 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_;
|
|
|
|
tf2_ros::Buffer::SharedPtr tf_;
|
|
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
|
};
|
|
|
|
} // namespace toid
|