#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include "interfaces/msg/img_msg.hpp"// 自定义消息头文件
#include "img_dev/img_cfg.hpp"
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.hpp>
#include <message_filters/synchronizer.h>
#include"img_dev/img_dev_node.hpp"
namespace img_dev{
ImageSubscriber::ImageSubscriber() : Node("image_subscriber") {
    img_msg= std::make_shared<interfaces::msg::ImgMsg>();
    img_pub = this->create_publisher<interfaces::msg::ImgMsg>("/img_msg", 10);
}
static long long pub_cnt=0;
void ImageSubscriber::pub_msg(const sensor_msgs::msg::Image& c_img,const sensor_msgs::msg::Image& d_img,const ImgCfg& cfg){
    img_msg->source = cfg.getSource();
    img_msg->type = cfg.getType();
    img_msg->position=cfg.getPosition();
    img_msg->image_color = std::move(c_img);
    img_msg->image_depth = std::move(d_img);
    img_msg->karr=cfg.k_arr;
    img_msg->darr=cfg.d_arr;
    img_pub->publish(*img_msg);
    // pub_cnt+=1;
    
    std::cout<<"pub msg["<<pub_cnt<<"]:"<<img_msg->image_color.width<<"x"<<img_msg->image_color.height<<";"<<img_msg->darr[0]<<","<<img_msg->darr[1]<<",k:"<<img_msg->karr[0]<<","<<img_msg->karr[1]<<std::endl;
}

}
