// // Created by zx on 2019/12/28. // #include #include #include "plc/plc_communicator.h" //#include "laser/Laser.h" #include "plc/plc_communicator.h" #include "tool/pathcreator.h" #include #include "verifier.h" #include "proto_tool.h" #include #include "LivoxMid70.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 "setting.pb.h" #include "safety_excutor.h" 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/log/%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("shutter_verify"); 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; } std::mutex mut; pcl::visualization::PCLVisualizer viewer("Cloud"); //读取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; } int Working() { while(1) { break; } /*Error_manager code; Eigen::Vector4f plane,plane1; plane[0]=0; plane[1]=1; plane[2]=0.2; plane[3]=2; plane1[0]=0; plane1[1]=1; plane1[2]=-0.2; plane1[3]=-1.3; float maxd=0.02,mind=-0.08,maxd1=-0.02,mind1=-0.12; shuttler_verifier left_shutter; shuttler_verifier right_shutter; left_shutter.set_condition(plane1,maxd1,mind1); right_shutter.set_condition(plane,maxd,mind); verifier shutter(left_shutter,right_shutter); mut.lock(); left_shutter.create_plane(viewer); right_shutter.create_plane(viewer); mut.unlock(); //创建雷达 循环采集 //读laser配置 Laser_proto::Laser_parameter_all laser_parameters; if(!proto_tool::read_proto_param("./setting/laser.prototxt",laser_parameters)) { LOG(ERROR)<<"read laser parameter failed"; return -1; } int laser_cout=laser_parameters.laser_parameters_size(); if(laser_cout==0) { LOG(ERROR)<<" no lidars "; return 0; } std::vector plasers; plasers.resize(laser_cout); for(int i=0;iconnect_laser() != SUCCESS) { char description[255] = {0}; sprintf(description, "Laser %d connect failed...", i); LOG(ERROR)< scan_cloud; std::mutex cloud_lock; std::vector laser_task_vector; for (int i = 0; i < plasers.size(); ++i) { int frame_count = laser_parameters.laser_parameters(i).frame_num(); if (plasers[i] != nullptr) { //创建扫描任务, Laser_task *laser_task = new Laser_task(); // laser_task->task_init(TASK_CREATED, &scan_cloud, &cloud_lock); laser_task->set_task_frame_maxnum(frame_count); laser_task_vector.push_back(laser_task); //发送任务单给雷达 code = plasers[i]->execute_task(laser_task); if (code != SUCCESS) { LOG(ERROR) << " capture failed :" << code.get_error_description(); return -3; } } } //等待雷达完成任务单 double second=0.0; 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(t_end_point - t_start_point); second = double(duration.count()) * std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den; if (second > 0.5) { LOG(WARNING)<<"capture time out"; break; } usleep(1000*10); } //检查雷达任务完成情况,是否是正常完成 //释放扫描任务单 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; } } LOG(INFO) << " frame :"<<++frames<<",laser scanning over cloud size:" << scan_cloud.size() <<" , time:"< single_color(scan_cloud.makeShared(), color[0],color[1],color[2]); // green viewer.addPointCloud(scan_cloud.makeShared(), single_color, "livox"); viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "livox"); mut.unlock(); usleep(10*1000); } */ } int main(int argc,char* argv[]) { //初始化日志 init_glog(); viewer.addCoordinateSystem(2.0,0,0,0,"car_center"); int ret = livox::LdsLidar::InitLdsLidar(); shutter::shutter_param shutter_param; if(read_proto_param("./setting/setting.prototxt",shutter_param)==false) { printf(" read proto failed \n"); return -1; } safety_excutor* excutor=safety_excutor::CreateExcutor(shutter_param); if(excutor== nullptr) { printf(" create safety_excutor failed\n"); return -1; } /*const float DEGREE=M_PI/180.0; float transform1[]={-2.31,161.58,-64,2.512,0,0}; float transform2[]={176.45,28.05,71.85,0,0,0}; std::string sn1="3GGDJ8S00100921"; std::string sn2="3GGDJ8X00100541"; Eigen::Vector3f rpy1; rpy1<<-2.31*DEGREE,161.58*DEGREE,-64*DEGREE; Eigen::Vector3f rpy2; rpy2<<176.45*DEGREE,28.05*DEGREE,71.85*DEGREE; Eigen::Vector3f t1; t1<<1.256,0,0; Eigen::Vector3f t2; t2<<-1.256,0,0; read_lidar.SetTransformParam(rpy1,t1); read_lidar1.SetTransformParam(rpy2,t2); if(read_lidar.AddBroadcastCode(sn1)!=0) printf("%s connect failed\n",sn1.c_str()); if(read_lidar1.AddBroadcastCode(sn2)!=0) { printf("%s connect failed\n",sn2.c_str()); } read_lidar.SetFPS(10.0); read_lidar1.SetFPS(10.0);*/ usleep(3000*1000); //lidar1.AddSNCode(sn1); //lidar2.AddSNCode(sn2); std::thread* thread=new std::thread(Working); while(1) { excutor->verify(); /**/ usleep(33*1000); } return 0; }