#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include "interfaces/msg/dist_msg.hpp"// 自定义消息头文件
#include "dist_dev/dist_cfg.hpp"
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.hpp>
#include <message_filters/synchronizer.h>
#include"dist_dev/dist_dev_node.hpp"
namespace dist_dev{
DistSub::DistSub() : Node("dist_dev_node") {
    dist_msg= std::make_shared<interfaces::msg::DistMsg>();
    dist_pub = this->create_publisher<interfaces::msg::DistMsg>("/dist_msg", 10);
}
static long long pub_cnt=0;
void DistSub::pub_msg(const sensor_msgs::msg::PointCloud2& pc,const DistCfg& cfg){
    dist_msg->source = cfg.getSource();
    dist_msg->type = cfg.getType();
    dist_msg->position=cfg.getPosition();
    dist_msg->points=pc;
    dist_pub->publish(*dist_msg);
    pub_cnt+=1;
    std::cout<<"pub msg["<<pub_cnt<<"]:"<<std::endl;
}
void DistSub::pub_msg(const DistCfg& cfg){
    dist_msg->source = cfg.getSource();
    dist_msg->type = cfg.getType();
    dist_msg->position=cfg.getPosition();
    dist_msg->ranges=dist_arr;
    dist_msg->points= sensor_msgs::msg::PointCloud2();
    dist_pub->publish(*dist_msg);
    pub_cnt+=1;
    std::cout<<"pub msg["<<pub_cnt<<"]:"<<dist_arr[0]<<","<<dist_arr[1]<<std::endl;
}

}
