#include <rclcpp/rclcpp.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"
#include"dist_dev/ks104.hpp"
#include <vector>
#include<unordered_map>

using namespace std;
using namespace dist_dev;
static shared_ptr<DistSub> cur_node=nullptr;
DistCfg cfg0=DistCfg("ks104", "ultrasonic","front","/camera/color/image_raw","/camera/depth/image_raw");
DistCfg cfg1=DistCfg("mid360", "lidar","front","/livox/lidar","/livox/lidar");
int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    cur_node=make_shared<DistSub>();
    if(!cur_node){
        RCLCPP_ERROR(rclcpp::get_logger("main"),"dist_dev node create error!!!!");
        return -1;
    }
    //超声测距
    cur_node->dist_arr.resize(2);
    rclcpp::TimerBase::SharedPtr timer_=cur_node->create_wall_timer(std::chrono::milliseconds(100), [=]() { 

        //ls.header.frame_id="ultralsonic";
        //ls.header.stamp=cur_node->now();
        #if 1
        cur_node->dist_arr[0]=ks104_dist(0xd0); 
        cur_node->dist_arr[1]=ks104_dist(0xd2); 
       // cur_node->dist_arr[2]=ks104_dist(0xd4); 
       // cur_node->dist_arr[3]=ks104_dist(0xd6); 
        cur_node->pub_msg(cfg0);
        #endif
    });
    //雷达
    auto subscription_ = cur_node->create_subscription<sensor_msgs::msg::PointCloud2>(cfg1.getLidarTopic(), 10,
                            [=](const sensor_msgs::msg::PointCloud2& msg) { 
                                    std::cout<<"point cloud2 size:"<< msg.width<<"x"<< msg.height<<std::endl;
                                    cur_node->pub_msg(msg,cfg1);
                            });
    rclcpp::spin(cur_node); 
    rclcpp::shutdown(); 
    return 0;
}
