增加配置

CMakeLists.txt

@@ -43,13 +43,15 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/laser LASER )
 #aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/verify/match3d MATCH3D )
 
 
-add_executable(shutter_verify ./main.cpp ${ERROR_SRC} ${LASER}
+add_executable(shutter_verify ./main.cpp setting.pb.cc
+		safety_excutor.cpp
+		${ERROR_SRC} ${LASER}
 		${TOOL_SRC} )
 target_link_libraries(shutter_verify ${OpenCV_LIBS}
 		${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ${CERES_LIBRARIES}
 		/usr/local/lib/libglog.a /usr/local/lib/libgflags.a
 		/usr/local/lib/liblivox_sdk_static.a
-		/usr/lib/x86_64-linux-gnu/libmodbus.so  ipopt)
+		)
 
 
 

laser/LivoxMid70.cpp

@@ -91,13 +91,14 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr LivoxMid70::GetCloud()
     m_frames[idc].is_timeout();*/
     TimedLivoxExtendRawPoint     frames[MAX_FRAME];
     m_mutex.lock();
+    int id=m_idx-1;
     for(int i=0;i<MAX_FRAME;++i)
     {
         frames[i]=m_frames[i];
     }
     m_mutex.unlock();
 
-    int id=m_idx-1;
+
     for(int i=0;i<MAX_FRAME;++i)
     {
         int idc=id-i;

main.cpp

@@ -19,6 +19,8 @@ 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;
@@ -79,8 +81,21 @@ void init_glog()
 std::mutex   mut;
 pcl::visualization::PCLVisualizer viewer("Cloud");
 
-livox::LivoxMid70 read_lidar;
-livox::LivoxMid70 read_lidar1;
+
+
+//读取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, &parameter);
+    delete input;
+    close(fd);
+    return success;
+}
 
 int Working()
 {
@@ -269,10 +284,26 @@ int main(int argc,char* argv[])
     init_glog();
     viewer.addCoordinateSystem(2.0,0,0,0,"car_center");
 
-
     int ret = livox::LdsLidar::InitLdsLidar();
 
-    const float DEGREE=M_PI/180.0;
+
+    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};
 
@@ -285,9 +316,9 @@ int main(int argc,char* argv[])
     rpy2<<176.45*DEGREE,28.05*DEGREE,71.85*DEGREE;
 
     Eigen::Vector3f t1;
-    t1<<2.512,0,0;
+    t1<<1.256,0,0;
     Eigen::Vector3f t2;
-    t2<<0,0,0;
+    t2<<-1.256,0,0;
 
 
     read_lidar.SetTransformParam(rpy1,t1);
@@ -300,8 +331,8 @@ int main(int argc,char* argv[])
         printf("%s  connect failed\n",sn2.c_str());
 
     }
-    read_lidar.SetFPS(2.0);
-    read_lidar1.SetFPS(2.0);
+    read_lidar.SetFPS(10.0);
+    read_lidar1.SetFPS(10.0);*/
     usleep(3000*1000);
 
 
@@ -314,23 +345,8 @@ int main(int argc,char* argv[])
 
     while(1)
     {
-
-        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1=read_lidar.GetCloud();
-        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2=read_lidar1.GetCloud();
-        viewer.removeAllPointClouds();
-        int color[3]={0,255,0};
-        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud1, color[0],color[1],color[2]); // green
-        viewer.addPointCloud(cloud1, single_color, "cloud1");
-        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud1");
-
-
-        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud2, 255,0,0); // green
-        viewer.addPointCloud(cloud2, single_color2, "cloud2");
-        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud2");
-
-        printf("  lidar 1 count %d     lidar2 count %d  \n",cloud1->size(),cloud2->size());
-
-        viewer.spinOnce();
+        excutor->verify();
+        /**/
         usleep(33*1000);
     }
 

proto.sh

@@ -0,0 +1,3 @@
+protoc -I=./ setting.proto --cpp_out=./
+
+

safety_excutor.cpp

@@ -0,0 +1,73 @@
+//
+// Created by zx on 2021/12/16.
+//
+
+#include "safety_excutor.h"
+
+
+safety_excutor::safety_excutor()
+{
+    mp_lidars=nullptr;
+}
+safety_excutor::~safety_excutor()
+{
+    if(mp_lidars!= nullptr)
+    {
+        delete[] mp_lidars;
+        mp_lidars= nullptr;
+    }
+}
+
+
+bool safety_excutor::init(const shutter::shutter_param& parameter)
+{
+    if(parameter.lidars_size()==0)
+        return false;
+    mp_lidars=new livox::LivoxMid70[parameter.lidars_size()];
+    for(int i=0;i<parameter.lidars_size();++i)
+    {
+        shutter::lidar_parameter ld_p=parameter.lidars(i);
+        mp_lidars[i].AddBroadcastCode(parameter.lidars(i).sncode());
+        mp_lidars[i].SetFPS(parameter.lidars(i).fps());
+        mp_lidars[i].SetTransformParam(Eigen::Vector3f(ld_p.r(),ld_p.p(),ld_p.y()),
+                Eigen::Vector3f(ld_p.tx(),ld_p.ty(),ld_p.tz()));
+    }
+    m_parameter=parameter;
+    return true;
+}
+
+safety_excutor* safety_excutor::CreateExcutor(const shutter::shutter_param& parameter)
+{
+    safety_excutor* excutor=new safety_excutor();
+    if(excutor->init(parameter)==false)
+        return nullptr;
+    return excutor;
+}
+
+#include <pcl/visualization/pcl_visualizer.h>
+extern pcl::visualization::PCLVisualizer viewer;
+bool safety_excutor::verify()
+{
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    for(int i=0;i<m_parameter.lidars_size();++i)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_t=mp_lidars[i].GetCloud();
+        (*cloud)+=(*cloud_t);
+
+
+    }
+
+    printf(" shutter  recieve points %d\n",cloud->size());
+
+
+    viewer.removeAllPointClouds();
+    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud, 0,255,0); // green
+    viewer.addPointCloud(cloud, single_color2, "cloud");
+    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
+
+
+    viewer.spinOnce();
+
+}

safety_excutor.h

@@ -0,0 +1,28 @@
+//
+// Created by zx on 2021/12/16.
+//
+
+#ifndef SHUTTER_VERIFY__SAFETY_EXCUTOR_H_
+#define SHUTTER_VERIFY__SAFETY_EXCUTOR_H_
+
+#include "LivoxMid70.h"
+#include "setting.pb.h"
+class safety_excutor
+{
+ public:
+    static safety_excutor* CreateExcutor(const shutter::shutter_param& parameter);
+    ~safety_excutor();
+    bool verify();
+ private:
+    safety_excutor();
+    bool init(const shutter::shutter_param& parameter);
+
+
+ private:
+    shutter::shutter_param          m_parameter;
+    livox::LivoxMid70*          mp_lidars;
+
+};
+
+
+#endif //SHUTTER_VERIFY__SAFETY_EXCUTOR_H_

setting.proto

@@ -0,0 +1,29 @@
+syntax = "proto2";
+package shutter;
+message lidar_parameter {
+    optional string sncode = 1;
+    optional double r = 2 [default = 0.0];
+    optional double p = 3 [default = 0.0];
+    optional double y = 4 [default = 0.0];
+    optional double tx = 5 [default = 1.0];
+    optional double ty = 6 [default = 1.0];
+    optional double tz = 7 [default = 1.0];
+    optional double fps=8 [default=10.0];
+
+}
+
+message box_param
+{
+    optional double minx=1;
+    optional double maxx=2;
+    optional double miny=3;
+    optional double maxy=4;
+    optional double minz=5;
+    optional double maxz=6;
+}
+
+message shutter_param
+{
+   repeated lidar_parameter   lidars=1;
+   optional box_param           box=2;
+}