LidarMeasure/test/locate_sample.cpp

//
// Created by zx on 2019/12/30.
//
/*
 * locater 模块测试例子
 */
#include <fcntl.h>
#include "../locate/locate_task.h"
#include "../locate/locater.h"
#include <glog/logging.h>
#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;
#include <sys/stat.h>
#include <dirent.h>

void init_glog()
{
    FLAGS_max_log_size = 100;
    FLAGS_logbufsecs = 0;
    google::InitGoogleLogging("LidarMeasurement");
    google::SetStderrLogging(google::INFO);
    google::InstallFailureSignalHandler();
    FLAGS_colorlogtostderr = true;        // Set log color
    FLAGS_logbufsecs = 0;                // Set log output speed(s)
    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
    FLAGS_stop_logging_if_full_disk = true;
}
void list_dir( const char * dir_name,std::vector<std::string>& files)
{
    if( 0 == dir_name )
    {
        std::cout<<" dir_name is null ! "<<std::endl;
        return;
    }

    struct stat s;
    lstat( dir_name , &s );
    if( ! S_ISDIR( s.st_mode ) )
    {
        return;
    }
    struct dirent * filename;
    DIR * dir;
    dir = opendir( dir_name );
    if( NULL == dir )
    {
        return;
    }

    int iName=0;
    while( ( filename = readdir(dir) ) != NULL )
    {
        if( strcmp( filename->d_name , "." ) == 0 ||
            strcmp( filename->d_name , "..") == 0)
            continue;

        char wholePath[128] = {0};
        sprintf(wholePath, "%s", filename->d_name);
        files.push_back(wholePath);
    }
}

bool read_proto_param(std::string path, ::google::protobuf::Message& param)
{
    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, &param);
    delete input;
    close(fd);
    return success;
}
bool string2point(std::string str,pcl::PointXYZ& point)
{
    std::istringstream iss;
    iss.str(str);
    float value;
    float data[3]={0};
    for(int i=0;i<3;++i)
    {
        if(iss>>value)
        {
            data[i]=value;
        }
        else
        {
            return false;
        }
    }
    point.x=data[0];
    point.y=data[1];
    point.z=data[2];
    return true;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
{
    std::ifstream fin(file.c_str());
    const int line_length=64;
    char str[line_length]={0};
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    while(fin.getline(str,line_length))
    {
        pcl::PointXYZ point;
        if(string2point(str,point))
        {
            cloud->push_back(point);
        }
    }
    return cloud;
}

int main(int argc,char* argv[])
{
    init_glog();
    std::string input_file="balck_suv";
    std::string out_path="./test";
    if(argc>=2)
    {
        input_file=argv[1];
    }
    if(argc>=3)
    {
        out_path=argv[2];
    }
    Error_manager  code;
    Locater* locater=new Locater();
    Measure::LocateParameter parameter;
    if(false==read_proto_param("./setting/locate.prototxt",parameter))
    {
        std::cout<<" read parameter failed..."<<std::endl;
    }
    code=locater->init(parameter);
    if(code!=SUCCESS)
    {
        LOG(ERROR)<<code.to_string();
    }
    Locate_task* task=new Locate_task();

    std::string cloud_path="/home/zx/data/samples/src_txt/";
    cloud_path+=input_file;
    std::vector<std::string> files;
    list_dir(cloud_path.c_str(),files);
    int count=files.size();
    int correct_size=0;
    for(int i=0;i<files.size();++i) {

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
        std::string t_file=cloud_path+"/"+files[i];
        cloud = ReadTxtCloud(t_file);
        std::cout << "  input file: " << t_file << std::endl;
        code = task->set_locate_cloud(cloud);
        if (code != SUCCESS) {
            LOG(ERROR) << code.to_string();
            return 0;
        }
        task->set_save_path(out_path);
        code = locater->execute_task(task, 5);
        if (code == SUCCESS) {
            LOG(INFO) << " LOCATE SUCCESS";
            correct_size++;
        }
        else {
            LOG(ERROR) << code.to_string();
            //移动文件
            usleep(300);
            std::string src="./test/cnn3d.txt";
            std::string dst="./error/";
            dst+=files[i];
            std::string name="mv "+src+" "+dst;
            const char *des_name=name.c_str();

            system(des_name); //调用系统命令
            usleep(200);

        }
    }
    if(count>0)
    {
        std::cout<<std::endl<<"  acc : "<<correct_size<<"/"<<count<<
        "    percent:"<<float(correct_size)/float(count)<<std::endl;
    }
    return 0;
}