#include <iostream>
#include <ros/ros.h>
#include <thread>
#include "data.h"



int main(int argc, char** argv)
{
    ros::init(argc, argv, "upbot_init"); // Initialize the ROS node
    std::shared_ptr<uwb_slam::Uwb> uwb = std::make_shared<uwb_slam::Uwb>();
    std::shared_ptr<uwb_slam::RunL> runL = std::make_shared<uwb_slam::RunL>();
    std::shared_ptr<uwb_slam::Data> data = std::make_shared<uwb_slam::Data>();



    // mp->uwb_ = uwb;
    // mp->align_ = align;
    // align->uwb_ = uwb;
    // sender->align_ = align;
    data->uwb_ = uwb;
    data->runL_ = runL;

    // uwb serried read
    std::thread uwb_trd([&uwb]() {
        uwb->Run();
    });

    std::thread runL_trd([&runL]() {
        runL->Run();
    });

    std::thread data_trd([&data]() {
        data->Run();
    });


    ros::spin(); 
}
