LidarMeasure/wj_lidar/fence_controller.cpp

//
// Created by zx on 2019/12/6.
//
#include "fence_controller.h"

int Fence_controller::S_RETRY_TIMES=3;
/**
 * 配置读取函数，暂未使用
 * */
bool Fence_controller::read_proto_param(std::string path)
{
    int fd = open(path.c_str(), O_RDONLY);
    if (fd == -1)
        return false;
    FileInputStream *input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &m_wj_manager_param);
    // std::cout<<m_global_param.data_path()<<std::endl;
    delete input;
    close(fd);
    return success;
}

/**
 * 保存点云为txt文件
 * */
void Fence_controller::save_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud)
{
    std::ofstream os;
    os.open(txt, std::ios::out);
    for (int i = 0; i < pCloud->points.size(); i++)
    {
        pcl::PointXYZ point = pCloud->points[i];
        char buf[255];
        memset(buf, 0, 255);
        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
        os.write(buf, strlen(buf));
    }
    os.close();
}

/**
 * 初始化函数，根据配置生成日志与数据保存路径，创建雷达，区域与plc句柄以及更新，消息接收与处理线程
 * */
Error_manager Fence_controller::initialize(wj::wjManagerParams params)
{
    mp_merged_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

    // get current working directory
    char curr_dir[100];
    getcwd(curr_dir, sizeof(curr_dir));

    m_wj_manager_param.CopyFrom(params);

    // create data path
    if (m_wj_manager_param.has_fence_data_path() && m_wj_manager_param.fence_data_path() != "")
    {
        PathCreator path_creator;
        path_creator.Mkdir(m_wj_manager_param.fence_data_path());
    }
    mp_plc_data = Plc_data::get_instance(m_wj_manager_param.plc_ip_address());
    if (mp_plc_data != 0 && mp_plc_data->get_plc_status())
    {
        LOG(INFO) << "created plc data handler" << mp_plc_data;
    }
    else
    {
//        LOG(ERROR) << "create plc data handler error";
        return Error_manager(Error_code::WJ_MANAGER_UNINITIALIZED,NORMAL,"万集plc连接失败");
    }

    // init lidar instances
    for (int i = 0; i < m_wj_manager_param.wj_lidar_size(); ++i)
    {
        m_lidars_vector.push_back(new Wj_lidar_encapsulation());
        Error_manager code = m_lidars_vector[i]->initialize(m_wj_manager_param.wj_lidar(i));
        // 错误码信息已在内部打印
        if (code != SUCCESS)
        {
            return Error_manager(Error_code::WJ_MANAGER_UNINITIALIZED,NORMAL,"万集雷达连接失败");
        }
    }
    // init region_detector instances
    for (int j = 0; j < m_wj_manager_param.regions_size(); ++j)
    {
        m_region_workers_vector.push_back(new Region_worker(j, m_wj_manager_param.regions(j), mp_verify_result_tool));
    }
    LOG(INFO) << "created wj lidars and region workers";

    // init wheel handling thread
    m_cond_wheel_exit.Notify(false);
    if (m_wheel_soc)
    {
        m_wheel_soc.bind(CONNECTSTRING);
    }
    m_wheel_send_thread = new std::thread(wheel_msg_handling_thread, this);
    m_wheel_send_thread->detach();
    m_wheel_recv_thread = new std::thread(wheel_msg_recv_thread, this);
    m_wheel_recv_thread->detach();
    LOG(INFO) << "created wheel msg receiving and sending threads";

    m_cond_exit.Notify(false);
    m_update_thread = new std::thread(cloud_merge_update, this);
    m_update_thread->detach();
    LOG(INFO) << "created cloud merge thread";

    LOG(INFO) << "fence this: "<< this;
    mb_initialized = true;
    return Error_manager(Error_code::SUCCESS);
}

/**
 * 无参构造
 * */
Fence_controller::Fence_controller(Verify_result* verify_handle) : mb_initialized(0),
                                       m_update_thread(0),
                                       m_wheel_recv_thread(0),
                                       m_wheel_send_thread(0),
                                       mp_verify_result_tool(0)
{
    m_lidars_vector.clear();
    m_region_workers_vector.clear();
    if(verify_handle!=0)
        mp_verify_result_tool = verify_handle;
}

/**
 * 析构函数
 * */
Fence_controller::~Fence_controller()
{
    m_cond_exit.Notify(true);
    m_cond_wheel_exit.Notify(true);
    if (m_update_thread)
    {
        if (m_update_thread->joinable())
        {
            m_update_thread->join();
        }
        delete m_update_thread;
        m_update_thread = 0;
    }
    LOG(INFO) << "exit update cloud thread";
    if (m_wheel_recv_thread)
    {
        if (m_wheel_recv_thread->joinable())
        {
            m_wheel_recv_thread->join();
        }
        delete m_wheel_recv_thread;
        m_wheel_recv_thread = 0;
    }
    LOG(INFO) << "exit receive wheel msg thread";
    if (m_wheel_send_thread)
    {
        if (m_wheel_send_thread->joinable())
        {
            m_wheel_send_thread->join();
        }
        delete m_wheel_send_thread;
        m_wheel_send_thread = 0;
    }
    LOG(INFO) << "exit send wheel result thread";
    // delete lidar instances
    for (int i = 0; i < m_lidars_vector.size(); ++i)
    {
        if (m_lidars_vector[i])
        {
            delete m_lidars_vector[i];
            m_lidars_vector[i] = 0;
        }
    }
    LOG(INFO) << "exit lidar handlers";
    // delete region_detector instances
    for (int j = 0; j < m_region_workers_vector.size(); ++j)
    {
        if (m_region_workers_vector[j])
        {
            delete m_region_workers_vector[j];
            m_region_workers_vector[j] = 0;
        }
    }
    LOG(INFO) << "exit region worker handles";
    //PlcData::Release();
    LOG(INFO) << "exit plc communicator";
}

/**
 * 外部获取点云
 * */
Error_manager Fence_controller::get_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out)
{
    // LOG(INFO) << "try to get cloud";
    if (!mb_initialized)
        return Error_manager(Error_code::WJ_MANAGER_UNINITIALIZED);

    std::lock_guard<std::mutex> t_lck(m_cloud_mutex);
    // LOG(INFO) << "get cloud check cloud size";
    if (mp_merged_cloud->size() <= 0){
        return Error_manager(Error_code::WJ_MANAGER_EMPTY_CLOUD);
    }
    
    // LOG(INFO) << "get cloud push new cloud ";
    cloud_out->clear();
    cloud_out->operator+=(*mp_merged_cloud);
    LOG(INFO) << "cloud size: " << mp_merged_cloud->size() << ", " << cloud_out->size();
    return Error_manager(Error_code::SUCCESS);
}

/**
 * 获取当前软件整体状态，包括雷达、与plc连接状态
 * */
Error_manager Fence_controller::get_current_status()
{
    Error_manager code;
    // 1.检查初始化状态
    if (!mb_initialized)
        return Error_manager(Error_code::WJ_MANAGER_UNINITIALIZED);
    bool disconnected_lidar_exist = false;
    char buf[50];
    memset(buf, 0, 50);
    sprintf(buf, "lidar disconnected: ");
    // 2.检查雷达初始化与连接状态
    for (size_t i = 0; i < m_lidars_vector.size(); i++)
    {
        if (m_lidars_vector[i] == 0)
        {
            LOG(ERROR) << i << "号雷达检测到空雷达指针";
            return Error_manager(Error_code::POINTER_IS_NULL);
        }
        else if (!m_lidars_vector[i]->get_initialize_status())
        {
            LOG(WARNING) << i << "号雷达初始化未完成";
            return Error_manager(Error_code::WJ_MANAGER_UNINITIALIZED);
        }
        else if (!m_lidars_vector[i]->get_connection_status())
        {
            LOG(ERROR) << i << "号雷达检测到连接断开";
            sprintf(buf+strlen(buf), "%d ", i);
            disconnected_lidar_exist = true;
        }
    }
    if (disconnected_lidar_exist)
    {
        LOG(WARNING) << buf;
        return Error_manager(Error_code::WJ_MANAGER_LIDAR_DISCONNECTED);
    }
    // 3.检查plc状态
    if (mp_plc_data == 0)
    {
        LOG(ERROR) << "检测plc为空指针";
        return Error_manager(Error_code::POINTER_IS_NULL);
    }
    else if (!mp_plc_data->get_plc_status())
    {
        LOG(WARNING) << "检测到plc掉线";
        return Error_manager(Error_code::WJ_MANAGER_PLC_DISCONNECTED);
    }
    else
    {
        return Error_manager(Error_code::SUCCESS);
    }
}

/**
 * 获取controller初始化状态
 * */
bool Fence_controller::get_initialize_status()
{
    return mb_initialized;
}

/**
 * 执行任务单
 * */
Error_manager Fence_controller::execute_task(Task_Base* task)
{
    // 参数合法性判断
    if(task == 0)
        return Error_manager(Error_code::WJ_LIDAR_TASK_EMPTY_TASK);

    if (task->get_task_type() != Task_type::WJ_TASK)
    {
        return Error_manager(Error_code::WJ_LIDAR_TASK_WRONG_TYPE,NORMAL,"电子围栏接收到非万集任务");
    }

    Wj_lidar_Task* task_temp = (Wj_lidar_Task*)task;
    wj_command command_temp;
    Error_manager code = task_temp->get_command(command_temp);

    if(code != SUCCESS)
        return code;
    int terminal_id = command_temp.terminal_id;
    if(terminal_id < 0)
    {
        return Error_manager(Error_code::WJ_LIDAR_TASK_INVALID_TASK,NORMAL,"电子围栏接收到错误万集任务，id异常");
    }

    // 初始化重试次数
    int count = S_RETRY_TIMES > 0 ? S_RETRY_TIMES : 1;
    // 创建保存结果容器
    wj_measure_result wj_measure_temp;
    wj_measure_temp.terminal_id = terminal_id;
    wj_measure_temp.correctness = false;
    std::lock_guard<std::mutex> t_lck(m_cloud_mutex);
    while(count-->0 && !task_temp->get_result_flag()){
        for (int i = 0; i < m_region_workers_vector.size(); ++i)
        {
            if (m_region_workers_vector[i]->get_id() == terminal_id)
            {
                char cloud_txt_filename[255];
                memset(cloud_txt_filename, 0, 255);
                char whole_filename[255];
                memset(whole_filename, 0, 255);
                bool correctness = false;

                LOG(INFO) << "--------callback find terminal------" << terminal_id;
                double x = 0, y = 0, c = 0, wheelbase = 0, width = 0,front_theta=0;
                // 获取最新点云并保存到total_cloud中

                pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
                bool has_few_cloud = false;
                std::string t_cloud_info_str=" ";
                for (int i = 0; i < m_lidars_vector.size(); ++i)
                {
                    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
                    if (m_lidars_vector[i]->get_cloud(cloud, command_temp.command_time, command_temp.timeout_in_milliseconds) == SUCCESS)
                    {
                        total_cloud->operator+=(*cloud);

                        // added yc yct, 区域四角雷达是否有点融合判断
                        std::string t_laser_ip = m_lidars_vector[i]->get_ip();
                        int t_laser_id = std::stoi(t_laser_ip.substr(t_laser_ip.size()-2));
                        if(t_laser_id == terminal_id+1 || t_laser_id == terminal_id+2||t_laser_id == terminal_id+8||t_laser_id == terminal_id+9)
                        {
                            t_cloud_info_str.append(" ");
                            t_cloud_info_str.append(std::to_string(t_laser_id)+"->"+std::to_string(cloud->size()));
                            has_few_cloud |= cloud->size() <= 5;
                        }
                    }
                }

                LOG(INFO) << "--------callback get cloud, size: " << total_cloud->size();

                // 获取检测结果
                Error_manager code = m_region_workers_vector[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width,front_theta);
                time_t tt = time(0);
                struct tm *tc = localtime(&tt);
                char buf[255] = {0};
                memset(buf, 0, 255);
                sprintf(buf, "%d-%02d-%02d_%02d:%02d:%02d", tc->tm_year + 1900,
                        tc->tm_mon + 1, tc->tm_mday, tc->tm_hour, tc->tm_min, tc->tm_sec);

                // 根据是否成功生成对应文件名
                if (code == SUCCESS)
                {
                    correctness = true;
                    sprintf(cloud_txt_filename, "/%s.txt", buf);
                }
                else
                {
                    LOG(WARNING) << "get wheel result failed: " << code.to_string();
                    correctness = false;
                    sprintf(cloud_txt_filename, "/%s%s.txt", buf, "_failed");
                }

                // save cloud txt
                if (m_wj_manager_param.has_fence_data_path() && m_wj_manager_param.fence_data_path() != "")
                {
                    PathCreator path_creator;
                    path_creator.CreateDatePath(m_wj_manager_param.fence_data_path(), false);
                    sprintf(whole_filename, "%s%s", path_creator.GetCurPath().c_str(), cloud_txt_filename);

                    save_cloud_txt(whole_filename, total_cloud);
                }
                wj_measure_temp.x = x * 1000.0;
                wj_measure_temp.y = y * 1000.0;
                wj_measure_temp.angle = c;
                wj_measure_temp.wheel_base = wheelbase*1000.0;
                wj_measure_temp.width = width * 1000.0;
                wj_measure_temp.correctness = correctness;
                // 20201109 added by yct. 6号车位机械累计误差，测量17407与机械手17370同位置，软件修正-15mm
                if(terminal_id == 5)
                {
                    wj_measure_temp.x -= 15;
                }
                if (wj_measure_temp.correctness){
                    // 20210527 added by yct. 一旦存在雷达没有点而结果正确，此时将结果改为错误
                    Error_code ec=Error_code::SUCCESS;
                    if(has_few_cloud)
                    {
                        wj_measure_temp.correctness = false;
                        ec = Error_code::WJ_MANAGER_EMPTY_CLOUD;
                    }
                    task_temp->set_result(wj_measure_temp);
                    task_temp->set_cloud(*total_cloud);
                    char description[512] = {0};
                    sprintf(description, "WJ center(%.1f, %.1f) size:(%.1f, %.1f)  angle:%.2f %s%s",
                            x * 1000.0, y * 1000.0, wheelbase * 1000.0, width * 1000.0, c, has_few_cloud?"存在空点云 ":"雷达正常 ", t_cloud_info_str.c_str());
                    LOG(INFO) << description;
                    return Error_manager(ec);
                }
            }
        }
    }
    task_temp->set_result(wj_measure_temp);
    return Error_manager(Error_code::WJ_LIDAR_TASK_MEASURE_FAILED);
}

/**
 * 点云拼接与区域状态更新线程函数
 * 默认点云获取超时时间为1秒
 * */
void Fence_controller::cloud_merge_update(Fence_controller *fc)
{
    // 句柄判空
    if (fc == 0)
        return;
    while (!fc->m_cond_exit.WaitFor(1))
    {
        // LOG(INFO) << "------- trying to update cloud ";
        // 加锁，依次获取最新点云并依次更新各区域状态
//        fc->m_cloud_mutex.lock();
        std::lock_guard<std::mutex> t_lck(fc->m_cloud_mutex);
        fc->mp_merged_cloud->clear();
        for (int i = 0; i < fc->m_lidars_vector.size(); ++i)
        {
            if(fc->m_lidars_vector[i] == 0)
            {
                LOG(WARNING) << "No " << i << " wj lidar, null pointer";
                continue;
            }
            if(!fc->m_lidars_vector[i]->get_initialize_status())
            {
                // LOG(WARNING) << "No " << i << " wj lidar, uninitialized";
                continue;
            }
            if(!fc->m_lidars_vector[i]->get_connection_status())
            {
                // LOG(WARNING) << "No " << i << " wj lidar, disconnected";
                continue;
            }
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
            Error_manager code = fc->m_lidars_vector[i]->get_cloud(cloud, std::chrono::steady_clock::now(), 1000);
            if (code == SUCCESS)
            {
                fc->mp_merged_cloud->operator+=(*cloud);
                // LOG(INFO) << "No " << i << " wj lidar, get cloud size: " << fc->mp_merged_cloud->size();
            }
            else
            {
                LOG(WARNING) << "No " << i << " wj lidar, get cloud failed. " << code.to_string();
            }
        }
        if (fc->mp_merged_cloud->size() > 0)
        {
            // LOG(INFO) << "trying to update cloud in region";
            for (int i = 0; i < fc->m_region_workers_vector.size(); ++i)
            {
                fc->m_region_workers_vector[i]->update_cloud(fc->mp_merged_cloud);
            }
        }
//        fc->m_cloud_mutex.unlock();
    }
}

/**
 * 轮距计算任务消息接收线程函数
 * */
void Fence_controller::wheel_msg_recv_thread(Fence_controller *fc)
{
    // 句柄判空
    if (fc == 0)
        return;
    while (!fc->m_cond_wheel_exit.WaitFor(1))
    {
        if (fc->m_wheel_soc)
        {
            // LOG(INFO) << "------- trying to recv msg ";
            nnxx::message_control *c1 = new nnxx::message_control();
            fc->m_mutex_wheel_handling.lock();
            auto m1 = fc->m_wheel_soc.recv(1, *c1);
            fc->m_mutex_wheel_handling.unlock();
            // 接收到消息则放入任务队列
            if (!m1.empty())
            {
                MsgTask task;
                task.cmd = nnxx::to_string(m1);
                task.sock_controller = c1;
                fc->m_msg_queue.push(task);
                LOG(INFO) << "-------recv msg " << task.cmd.c_str() << "-------";
            }
		if(c1 != nullptr)
		{
			delete c1;
		}
        }
    }
    LOG(INFO) << "任务消息接收线程退出";
}

// /**
//  * 电子围栏主动测量函数
//  * */
// Error_manager Fence_controller::wj_fence_measure()
// {
    
// }

/**
 * 轮距计算任务处理线程
 * */
#include "globalmsg.pb.h"
void Fence_controller::wheel_msg_handling_thread(Fence_controller *fc)
{
    // 句柄判空
    if (fc == 0)
        return;

    while (!fc->m_cond_wheel_exit.WaitFor(1))
    {
        // LOG(INFO) << "------- trying to handle msg ";
        // 初始化当前允许重测次数
        int count = fc->S_RETRY_TIMES > 0 ? fc->S_RETRY_TIMES : 1;
        MsgTask task;
        // 从队列中获取任务
        if (fc->m_msg_queue.try_pop(task))
        {
            // 初始化测量结果消息
            globalmsg::resultInfo result;
            time_t tt = time(0);
            struct tm *tc = localtime(&tt);
            char buf[255] = {0};
            memset(buf, 0, 255);
            sprintf(buf, "%d-%02d-%02d_%02d:%02d:%02d", tc->tm_year + 1900,
                    tc->tm_mon + 1, tc->tm_mday, tc->tm_hour, tc->tm_min, tc->tm_sec);
            result.set_time(buf);
            result.set_correctness(false);

            // 解析任务指令
            if (task.cmd.size() > 8 && task.cmd.substr(0, 8) == "Terminal")
            {
                int terminal_id = -1;
                try
                {
                    terminal_id = std::stoi(task.cmd.substr(8, task.cmd.length() - 8).c_str());
                }
                catch (std::exception const &e)
                {
                    LOG(WARNING) << e.what() << ", 消息解析失败，无法找到对应id号";
                    continue;
                }
                LOG(INFO) << "handling msg from terminal id: " << terminal_id;
                std::lock_guard<std::mutex> t_lck(fc->m_cloud_mutex);
                while (count-- >= 0 && !result.correctness())
                {
                    LOG(INFO) << "times :" << count;
                    // 寻找到对应区域
                    for (int i = 0; i < fc->m_region_workers_vector.size(); ++i)
                    {
                        if (terminal_id >= 0 && fc->m_region_workers_vector[i]->get_id() == terminal_id)
                        {
                            LOG(INFO) << "--------callback find terminal------" << terminal_id;
                            double x = 0, y = 0, c = 0, wheelbase = 0, width = 0,front_theta=0;
                            // 获取最新点云并保存到total_cloud中
//                            fc->m_cloud_mutex.lock();
                            pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
                            for (int i = 0; i < fc->m_lidars_vector.size(); ++i)
                            {
                                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
                                if (fc->m_lidars_vector[i]->get_cloud(cloud, std::chrono::steady_clock::now()) == SUCCESS)
                                {
                                    total_cloud->operator+=(*cloud);
                                }
                            }
//                            fc->m_cloud_mutex.unlock();
                            LOG(INFO) << "--------callback get cloud, size: " << total_cloud->size();

                            char cloud_txt_filename[255];
                            memset(cloud_txt_filename, 0, 255);
                            char whole_filename[255];
                            memset(whole_filename, 0, 255);
                            // 获取检测结果
                            Error_manager code = fc->m_region_workers_vector[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width,front_theta);
                            // 根据是否成功生成对应文件名
                            if (code == SUCCESS)
                            {
                                result.set_correctness(true);
                                sprintf(cloud_txt_filename, "/%s.txt", result.time().c_str());
                            }
                            else
                            {
                                LOG(WARNING) << "get wheel result failed: " << code.to_string();
                                result.set_correctness(false);
                                sprintf(cloud_txt_filename, "/%s%s.txt", result.time().c_str(), "_failed");
                            }

                            // save cloud txt
                            if (fc->m_wj_manager_param.has_fence_data_path() && fc->m_wj_manager_param.fence_data_path() != "")
                            {
                                PathCreator path_creator;
                                path_creator.CreateDatePath(fc->m_wj_manager_param.fence_data_path(), false);
                                sprintf(whole_filename, "%s%s", path_creator.GetCurPath().c_str(), cloud_txt_filename);

                                fc->save_cloud_txt(whole_filename, total_cloud);
                                LOG(INFO) << " save cloud " << whole_filename;
                            }
                            LOG(INFO) << "--------callback get result------";
                            result.set_x(x * 1000.0);
                            result.set_y(y * 1000.0);
                            result.set_c(c);
                            result.set_wheel_base(wheelbase * 1000.0);
                            result.set_width(width * 1000.0);
                            result.set_front_theta(front_theta*180.0/M_PI);
                            if (result.correctness())
                                break;
                        }
                    }
                }
            }
            std::string result_str = result.SerializeAsString();

            LOG(INFO) << "--------callback serialized: \n"
                      << result.DebugString() << "------";
            fc->m_mutex_wheel_handling.lock();
            if (fc->m_wheel_soc)
            {
                fc->m_wheel_soc.send(result_str.data(), result_str.length(), 0,
                                     std::move(*task.sock_controller));
                LOG(INFO) << "------- callback msg sent -------";
            }
            fc->m_mutex_wheel_handling.unlock();
            delete task.sock_controller;
        }
    }
}