|
|
@@ -0,0 +1,251 @@
|
|
|
+//
|
|
|
+// Created by zx on 2021/5/10.
|
|
|
+//
|
|
|
+
|
|
|
+#include "LivoxLaser.h"
|
|
|
+
|
|
|
+#include <google/protobuf/io/zero_copy_stream_impl.h>
|
|
|
+#include <google/protobuf/text_format.h>
|
|
|
+#include <fcntl.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 "pathcreator.h"
|
|
|
+//读取protobuf 配置文件
|
|
|
+//file:文件
|
|
|
+//parameter:要读取的配置
|
|
|
+bool read_proto_param(std::string file, ::google::protobuf::Message& parameter)
|
|
|
+{
|
|
|
+ int fd = open(file.c_str(), O_RDONLY);
|
|
|
+ if (fd == -1) return false;
|
|
|
+ FileInputStream* input = new FileInputStream(fd);
|
|
|
+ bool success = google::protobuf::TextFormat::Parse(input, ¶meter);
|
|
|
+ delete input;
|
|
|
+ close(fd);
|
|
|
+ return success;
|
|
|
+}
|
|
|
+
|
|
|
+GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
|
|
|
+{
|
|
|
+ time_t tt;
|
|
|
+ time( &tt );
|
|
|
+ tt = tt + 8*3600; // transform the time zone
|
|
|
+ tm* t= gmtime( &tt );
|
|
|
+ char buf[255]={0};
|
|
|
+ sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
|
|
|
+ t->tm_year + 1900,
|
|
|
+ t->tm_mon + 1,
|
|
|
+ t->tm_mday,
|
|
|
+ t->tm_hour,
|
|
|
+ t->tm_min,
|
|
|
+ t->tm_sec);
|
|
|
+
|
|
|
+ FILE* tp_file=fopen(buf,"w");
|
|
|
+ fprintf(tp_file,data,strlen(data));
|
|
|
+ fclose(tp_file);
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void init_glog()
|
|
|
+{
|
|
|
+ time_t tt = time(0);//时间cuo
|
|
|
+ struct tm* t = localtime(&tt);
|
|
|
+
|
|
|
+ char strYear[255]={0};
|
|
|
+ char strMonth[255]={0};
|
|
|
+ char strDay[255]={0};
|
|
|
+
|
|
|
+ sprintf(strYear,"%04d", t->tm_year+1900);
|
|
|
+ sprintf(strMonth,"%02d", t->tm_mon+1);
|
|
|
+ sprintf(strDay,"%02d", t->tm_mday);
|
|
|
+
|
|
|
+ char buf[255]={0};
|
|
|
+ getcwd(buf,255);
|
|
|
+ char strdir[255]={0};
|
|
|
+ sprintf(strdir,"%s/testlog/%s/%s/%s", buf,strYear,strMonth,strDay);
|
|
|
+ PathCreator creator;
|
|
|
+ creator.Mkdir(strdir);
|
|
|
+
|
|
|
+ char logPath[255] = { 0 };
|
|
|
+ sprintf(logPath, "%s/", strdir);
|
|
|
+ FLAGS_max_log_size = 100;
|
|
|
+ FLAGS_logbufsecs = 0;
|
|
|
+ google::InitGoogleLogging("test_lidar");
|
|
|
+ google::SetStderrLogging(google::INFO);
|
|
|
+ google::SetLogDestination(0, logPath);
|
|
|
+ google::SetLogFilenameExtension("zxlog");
|
|
|
+ google::InstallFailureSignalHandler();
|
|
|
+ google::InstallFailureWriter(&shut_down_logging);
|
|
|
+ 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;
|
|
|
+}
|
|
|
+
|
|
|
+Error_manager scan(std::vector<Laser_base*> tp_lasers)
|
|
|
+{
|
|
|
+
|
|
|
+ //准备目录
|
|
|
+ time_t tt;
|
|
|
+ time(&tt);
|
|
|
+ tt = tt + 8 * 3600; // transform the time zone
|
|
|
+ tm *t = gmtime(&tt);
|
|
|
+ char path[255] = {0};
|
|
|
+ sprintf(path, "/home/zx/data/livoxhorizon_test/%4d/%02d/%02d",
|
|
|
+ t->tm_year + 1900,
|
|
|
+ t->tm_mon + 1,
|
|
|
+ t->tm_mday);
|
|
|
+ PathCreator path_creator;
|
|
|
+ path_creator.CreateDatePath(path);
|
|
|
+ std::string project_path = path_creator.GetCurPath();
|
|
|
+
|
|
|
+
|
|
|
+ //第一步,启动雷达扫描点云,切换当前状态为扫描中
|
|
|
+ //根据配置筛选雷达
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> scan_cloud;
|
|
|
+ std::mutex cloud_lock;
|
|
|
+ std::vector<Laser_task *> laser_task_vector;
|
|
|
+ Error_manager code;
|
|
|
+
|
|
|
+ for (int i = 0; i < tp_lasers.size(); ++i) {
|
|
|
+
|
|
|
+ //创建扫描任务,
|
|
|
+ Laser_task *laser_task = new Laser_task();
|
|
|
+ //
|
|
|
+ laser_task->task_init(TASK_CREATED, &scan_cloud, &cloud_lock);
|
|
|
+ laser_task->set_task_frame_maxnum(1000);
|
|
|
+ laser_task->set_save_path(project_path);
|
|
|
+ laser_task_vector.push_back(laser_task);
|
|
|
+ //发送任务单给雷达
|
|
|
+ code = tp_lasers[i]->execute_task(laser_task);
|
|
|
+
|
|
|
+ if (code != SUCCESS) {
|
|
|
+ return code;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ if (tp_lasers.size() == 0) {
|
|
|
+ return TERMINOR_NOT_CONTAINS_LASER;
|
|
|
+ }
|
|
|
+ double timeout=3.0;
|
|
|
+ auto t_start_point = std::chrono::system_clock::now();
|
|
|
+ //等待雷达完成任务单
|
|
|
+ while (1) {
|
|
|
+ //判断是否强制退出
|
|
|
+
|
|
|
+ //判断雷达任务单是否全部完成
|
|
|
+ bool tb_laser_complete = true;
|
|
|
+ for (int i = 0; i < laser_task_vector.size(); ++i) {
|
|
|
+ tb_laser_complete &= (TASK_OVER == laser_task_vector[i]->get_statu());
|
|
|
+ }
|
|
|
+ if (tb_laser_complete) {
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ //计算扫描时间,若超时,并且没有点,则返回错误.
|
|
|
+ auto t_end_point = std::chrono::system_clock::now();
|
|
|
+ auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_point - t_start_point);
|
|
|
+ double second = double(duration.count()) *
|
|
|
+ std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den;
|
|
|
+ if (second > timeout) {
|
|
|
+ //扫描超时,点云中没有点,则返回错误
|
|
|
+ if (scan_cloud.size() == 0) {
|
|
|
+ return TERMINOR_LASER_TIMEOUT;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ }
|
|
|
+ usleep(1000 * 100);
|
|
|
+ }
|
|
|
+ //检查雷达任务完成情况,是否是正常完成
|
|
|
+ LOG(INFO) << " laser scanning over cloud size:" << scan_cloud.size();
|
|
|
+ //释放扫描任务单
|
|
|
+ for (int i = 0; i < laser_task_vector.size(); ++i) {
|
|
|
+ if (laser_task_vector[i] != 0) {
|
|
|
+ delete laser_task_vector[i];
|
|
|
+ laser_task_vector[i] = NULL;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return SUCCESS;
|
|
|
+}
|
|
|
+
|
|
|
+int main(int argc,char* argv[])
|
|
|
+{
|
|
|
+ init_glog();
|
|
|
+ Error_manager code;
|
|
|
+ //读laser配置
|
|
|
+ Laser_proto::Laser_parameter_all laser_parameters;
|
|
|
+ if(!read_proto_param("./setting/laser.prototxt",laser_parameters))
|
|
|
+ {
|
|
|
+ code= Error_manager(SYSTEM_READ_PARAMETER_ERROR,NORMAL,"read laser parameter failed");
|
|
|
+ }
|
|
|
+ if(code!=SUCCESS)
|
|
|
+ {
|
|
|
+ std::cout<<code.get_error_description()<<std::endl;
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ //创建大疆雷达
|
|
|
+ int laser_cout=laser_parameters.laser_parameters_size();
|
|
|
+ std::vector<Laser_base*> laser_vector;
|
|
|
+ laser_vector.resize(laser_cout);
|
|
|
+ for(int i=0;i<laser_parameters.laser_parameters_size();++i)
|
|
|
+ {
|
|
|
+ LOG(INFO)<<"创建雷达:"<<laser_parameters.laser_parameters(i).sn();
|
|
|
+ laser_vector[i]=LaserRegistory::CreateLaser(laser_parameters.laser_parameters(i).type(),i,
|
|
|
+ laser_parameters.laser_parameters(i));
|
|
|
+ if(laser_vector[i]!=NULL)
|
|
|
+ {
|
|
|
+ if(laser_vector[i]->connect_laser()!=SUCCESS)
|
|
|
+ {
|
|
|
+ char description[255]={0};
|
|
|
+ sprintf(description,"Laser %d connect failed...",i);
|
|
|
+ code= Error_manager(LASER_CONNECT_FAILED,NORMAL,description);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(code!=SUCCESS)
|
|
|
+ {
|
|
|
+ std::cout<<code.get_error_description()<<std::endl;
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+ //查询是否有livox雷达,初始化库
|
|
|
+ for (int i = 0; i < laser_parameters.laser_parameters_size(); ++i)
|
|
|
+ {
|
|
|
+ std::string type = laser_parameters.laser_parameters(i).type();
|
|
|
+ if ( (type.find("Livox") != type.npos) || (type.find("Horizon") != type.npos) )
|
|
|
+ {
|
|
|
+ if (Start() == false)
|
|
|
+ {
|
|
|
+ Uninit();
|
|
|
+ code= Error_manager(LASER_LIVOX_SKD_INIT_FAILED,NORMAL,"Livox laser init failed...");
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(code!=SUCCESS)
|
|
|
+ {
|
|
|
+ std::cout<<code.get_error_description()<<std::endl;
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ //扫描
|
|
|
+
|
|
|
+ char c=getchar();
|
|
|
+ code =scan(laser_vector);
|
|
|
+ if(code!=SUCCESS)
|
|
|
+ {
|
|
|
+ std::cout<<code.get_error_description()<<std::endl;
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+ std::cout<<" scan over"<<std::endl;
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
