增加laser模块,待测试.

error_code/error_code.h

@@ -75,6 +75,9 @@ enum Error_code
 //    laser.cpp文件
 
     LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达任务输入参数错误
+    LASER_TASK_TYPE_ERROR,
+    LASER_CONNECT_FAILED,
+    LASER_LIVOX_SKD_INIT_FAILED,
 
 
 //    laser_livox.cpp的错误码

laser/Laser.cpp

@@ -94,7 +94,7 @@ char& CBinaryData::operator[](int n)
 }
 //////////////
 
-CLaser::CLaser(int id, Automatic::stLaserCalibParam laser_param)
+Laser_base::Laser_base(int id, Laser_proto::laser_parameter laser_param)
 	:m_ThreadRcv(0)
 	,m_ThreadPro(0)
 	,m_ThreadPub(0)
@@ -112,7 +112,7 @@ CLaser::CLaser(int id, Automatic::stLaserCalibParam laser_param)
 }
 
 
-CLaser::~CLaser()
+Laser_base::~Laser_base()
 {
 	if (m_dMatrix)
 	{
@@ -121,7 +121,12 @@ CLaser::~CLaser()
 	}
 }
 
-bool CLaser::Start()
+Error_manager Laser_base::check_error()
+{
+    return SUCCESS;
+}
+
+bool Laser_base::Start()
 {
     m_bscan_start=true;
 	m_statu = eLaser_busy;
@@ -130,7 +135,7 @@ bool CLaser::Start()
 	return true;
 }
 
-bool CLaser::Stop()
+bool Laser_base::Stop()
 {
     m_bscan_start=false;
 	m_bStart_capture.Notify(false);
@@ -150,7 +155,7 @@ bool CLaser::Stop()
 	return true;
 }
 
-void CLaser::SetMetrix(double* data)
+void Laser_base::SetMetrix(double* data)
 {
 	if (m_dMatrix == 0)
 	{
@@ -158,22 +163,22 @@ void CLaser::SetMetrix(double* data)
 	}
 	memcpy(m_dMatrix, data, 12 * sizeof(double));
 }
-void CLaser::SetPointCallBack(PointCallBack fnc, void* pointer)
+void Laser_base::SetPointCallBack(PointCallBack fnc, void* pointer)
 {
 	m_point_callback_fnc = fnc;
 	m_point_callback_pointer = pointer;
 }
 
-bool CLaser::Connect()
+bool Laser_base::Connect()
 {
 	m_bThreadRcvRun.Notify(true);
 	if(m_ThreadRcv==0)
-		m_ThreadRcv = new std::thread(&CLaser::thread_recv, this);
+		m_ThreadRcv = new std::thread(&Laser_base::thread_recv, this);
 	m_ThreadRcv->detach();
 
 	m_bThreadProRun.Notify(true);
 	if(m_ThreadPro==0)
-		m_ThreadPro = new std::thread(&CLaser::thread_toXYZ, this);
+		m_ThreadPro = new std::thread(&Laser_base::thread_toXYZ, this);
 	m_ThreadPro->detach();
 
     if(m_ThreadPub==0)
@@ -182,7 +187,7 @@ bool CLaser::Connect()
 
 	return true;
 }
-void CLaser::Disconnect()
+void Laser_base::Disconnect()
 {
 	if (m_ThreadRcv)
 	{
@@ -207,46 +212,44 @@ void CLaser::Disconnect()
 
 //对外的接口函数，负责接受并处理任务单，
 //input：laser_task 雷达任务单，必须是子类，并且任务正确。（传引用）
-Error_manager CLaser::porform_task(Laser_task* p_laser_task)
+Error_manager Laser_base::execute_task(Task_Base* p_laser_task)
 {
     Error_manager t_error_manager;
     //检查指针
     if(p_laser_task == NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "CLaser::porform_task failed, POINTER_IS_NULL");
+                             "Laser_base::porform_task failed, POINTER_IS_NULL");
     }
-    else
+    if(p_laser_task->get_task_type()!=LASER_TASK)
     {
-        //接受任务，并将任务的状态改为TASK_SIGNED已签收
-        mp_laser_task = p_laser_task;
-        mp_laser_task->set_task_statu(TASK_SIGNED);
+        return Error_manager(Error_code::LASER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+                             "laser task type error");
     }
 
+
+    //接受任务，并将任务的状态改为TASK_SIGNED已签收
+    mp_laser_task = (Laser_task*)p_laser_task;
+    mp_laser_task->set_task_statu(TASK_SIGNED);
+
+
     //检查消息内容是否正确，
     //检查点云指针
-    if(p_laser_task->get_task_point3d_cloud_vector() == NULL)
+    if(mp_laser_task->get_task_point3d_cloud_vector().get() == NULL)
     {
         t_error_manager.error_manager_reset(Error_code::POINTER_IS_NULL,
                                             Error_level::MINOR_ERROR,
-                                            "CLaser::porform_task failed, POINTER_IS_NULL");
-    }
-        //检查任务类型
-    else if(p_laser_task->get_task_type() != LASER_TASK)
-    {
-        t_error_manager.error_manager_reset(Error_code::LASER_TASK_PARAMETER_ERROR,
-                                            Error_level::MINOR_ERROR,
-                                            "CLaser::porform_task failed, task_type is error");
+                                            "Laser_base::porform_task failed, POINTER_IS_NULL");
     }
     else
     {
         //将任务的状态改为 TASK_WORKING 处理中
         mp_laser_task->set_task_statu(TASK_WORKING);
-
+        std::string save_path=mp_laser_task->get_save_path();
+        SetSaveDir(save_path);
         //启动雷达扫描
         Start();
     }
-
     //返回错误码
     if(t_error_manager != Error_code::SUCCESS)
     {
@@ -256,7 +259,7 @@ Error_manager CLaser::porform_task(Laser_task* p_laser_task)
 }
 
 
-void CLaser::SetSaveDir(std::string strDir,bool bSaveFile)
+void Laser_base::SetSaveDir(std::string strDir,bool bSaveFile)
 {
 	m_bSave_file = bSaveFile;
 	m_pts_log_tool.close();
@@ -275,7 +278,7 @@ void CLaser::SetSaveDir(std::string strDir,bool bSaveFile)
 	m_binary_log_tool.open(bin_file,true);
 }
 
-void CLaser::thread_recv()
+void Laser_base::thread_recv()
 {
 	while (m_bThreadRcvRun.WaitFor(1))
 	{
@@ -295,7 +298,7 @@ void CLaser::thread_recv()
 	}
 }
 
-CPoint3D CLaser::transfor(CPoint3D point)
+CPoint3D Laser_base::transfor(CPoint3D point)
 {
 	if (m_dMatrix)
 	{
@@ -314,7 +317,7 @@ CPoint3D CLaser::transfor(CPoint3D point)
 	}
 }
 
-void CLaser::thread_toXYZ()
+void Laser_base::thread_toXYZ()
 {
 	while (m_bThreadProRun.WaitFor(1))
 	{
@@ -351,8 +354,6 @@ void CLaser::thread_toXYZ()
                 for (int i = 0; i < cloud.size(); ++i)
                 {
                     CPoint3D point = transfor(cloud[i]);
-                    if (m_point_callback_fnc)
-                        m_point_callback_fnc(point, m_point_callback_pointer);
                     if(m_bSave_file) {
                         char buf[64] = {0};
                         sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
@@ -361,7 +362,10 @@ void CLaser::thread_toXYZ()
 
                     //此时cloud为雷达采集数据的结果，转换后的三维点云。
                     //将每个点存入任务单里面的点云容器。
-                    mp_laser_task->get_task_point3d_cloud_vector()->push_back(point);
+                    if(mp_laser_task!=NULL)
+                    {
+                        mp_laser_task->get_task_point3d_cloud_vector()->push_back(pcl::PointXYZ(point.x,point.y,point.z));
+                    }
                 }
                 m_points_count+=cloud.size();
                 if (type == eStop)
@@ -401,7 +405,7 @@ void CLaser::thread_toXYZ()
 
 
 #include "unistd.h"
-void CLaser::threadPublish(CLaser *laser)
+void Laser_base::threadPublish(Laser_base *laser)
 {
     if(laser==0) return ;
     while(laser->m_bThreadRcvRun.WaitFor(1))
@@ -411,10 +415,10 @@ void CLaser::threadPublish(CLaser *laser)
     }
 }
 
-#include "../common.h"
-void CLaser::PublishMsg()
+
+void Laser_base::PublishMsg()
 {
-    globalmsg::msg msg;
+    /*globalmsg::msg msg;
     msg.set_msg_type(globalmsg::eLaser);
 
     globalmsg::laserStatus status;
@@ -427,5 +431,5 @@ void CLaser::PublishMsg()
     msg.mutable_laser_msg()->set_laser_status(status);
     msg.mutable_laser_msg()->set_queue_data_count(m_queue_laser_data.size());
     msg.mutable_laser_msg()->set_cloud_count(m_points_count);
-    MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());
+    MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());*/
 }

laser/Laser.h

@@ -3,9 +3,9 @@
 #include "Point2D.h"
 #include "Point3D.h"
 #include "LogFiles.h"
-#include "../src/CalibParam.pb.h"
-#include "../src/StdCondition.h"
-
+#include <glog/logging.h>
+#include "../tool/StdCondition.h"
+#include "laser_parameter.pb.h"
 #include "../error_code/error_code.h"
 #include "laser_task_command.h"
 
@@ -158,22 +158,24 @@ enum eLaserStatu
 typedef void (*PointCallBack)(CPoint3D ,  void* );
 
 //雷达的基类，不能直接使用，必须子类继承
-class CLaser
+class Laser_base
 {
 public:
     //唯一的构造函数，按照设备名称和雷达参数来创建实例。
     //input：id： 雷达设备的id，（唯一索引）
     //input：laser_param：雷达的参数，
     //注：利用protobuf创建stLaserCalibParam类，然后从文件读取参数
-	CLaser(int id,Automatic::stLaserCalibParam laser_param);
-	virtual ~CLaser();
+	Laser_base(int id,Laser_proto::laser_parameter laser_param);
+	virtual ~Laser_base();
 	///��������Դ
 	virtual bool Connect();
 	virtual void Disconnect();
 
 	//对外的接口函数，负责接受并处理任务单，
 	//input：p_laser_task 雷达任务单，必须是子类，并且任务正确。
-    virtual Error_manager porform_task(Laser_task* p_laser_task);
+    virtual Error_manager execute_task(Task_Base* p_laser_task);
+    //检查雷达状态,是否正常运行
+    Error_manager check_error();
 
 	///��ʼ��ֹͣ�ɼ�
 	virtual bool Start();
@@ -201,7 +203,7 @@ protected:
 	////���Ʊ任
 	virtual CPoint3D transfor(CPoint3D point);
 protected:
-    static void threadPublish(CLaser* laser);
+    static void threadPublish(Laser_base* laser);
     void PublishMsg();
 protected:
 	std::thread*		m_ThreadRcv;
@@ -217,7 +219,7 @@ protected:
 	bool                m_bscan_start;
 	eLaserStatu			m_statu;
 	bool				m_bSave_file;
-	Automatic::stLaserCalibParam  m_laser_param;////����
+    Laser_proto::laser_parameter  m_laser_param;////����
 
     //���ݴ������
 	threadsafe_queue<CBinaryData*>		m_queue_laser_data;     // ���ݶ���
@@ -241,12 +243,12 @@ protected:
 
 class LaserRegistory
 {
-	typedef CLaser*  (*CreateLaserFunc)(int id, Automatic::stLaserCalibParam laser_param);
+	typedef Laser_base*  (*CreateLaserFunc)(int id, Laser_proto::laser_parameter laser_param);
 public:
 	LaserRegistory(std::string name, CreateLaserFunc pFun) {
 		AddCreator(name, pFun);
 	}
-	static CLaser* CreateLaser(std::string name, int id,Automatic::stLaserCalibParam laser_param) {
+	static Laser_base* CreateLaser(std::string name, int id,Laser_proto::laser_parameter laser_param) {
 		if (GetFuncMap().count(name) == 0)
             return 0;
 		return GetFuncMap()[name](id,laser_param);
@@ -264,7 +266,7 @@ private:
 
 
 #define RegisterLaser(NAME) \
-	static CLaser* Create_##NAME##_Laser(int id, Automatic::stLaserCalibParam param)	\
+	static Laser_base* Create_##NAME##_Laser(int id, Laser_proto::laser_parameter param)	\
 	{																\
 		return new C##NAME##Laser(id,param);							\
 	}																\

laser/LivoxLaser.cpp

@@ -1,6 +1,5 @@
 
 #include "LivoxLaser.h"
-#include "../common.h"
 
 RegisterLaser(Livox)
 
@@ -25,7 +24,10 @@ void CLivoxLaser::UpdataHandle()
 
 bool CLivoxLaser::IsScanComplete()
 {
-	return g_count[m_handle] >= m_frame_maxnum;
+    if(mp_laser_task!=NULL)
+	    return g_count[m_handle] >= mp_laser_task->get_task_points_number();
+    else
+        return false;
 }
 
 void CLivoxLaser::InitLivox()
@@ -50,8 +52,8 @@ void CLivoxLaser::InitLivox()
 	}
 }
 
-CLivoxLaser::CLivoxLaser(int id, Automatic::stLaserCalibParam laser_param)
-	:CLaser(id, laser_param)
+CLivoxLaser::CLivoxLaser(int id, Laser_proto::laser_parameter laser_param)
+	:Laser_base(id, laser_param)
 {
 	
 	m_frame_maxnum = laser_param.frame_num();
@@ -223,11 +225,11 @@ eLaserStatu CLivoxLaser::GetStatu()
 
 bool CLivoxLaser::Connect()
 {
-	return CLaser::Connect();
+	return Laser_base::Connect();
 }
 void CLivoxLaser::Disconnect() 
 {
-	CLaser::Disconnect();
+	Laser_base::Disconnect();
 }
 bool CLivoxLaser::Start()
 {
@@ -235,11 +237,11 @@ bool CLivoxLaser::Start()
 	//???????????
 	m_queue_livox_data.clear();
 	g_count[m_handle] = 0;
-	return CLaser::Start();
+	return Laser_base::Start();
 }
 bool CLivoxLaser::Stop()
 {
-	return CLaser::Stop();
+	return Laser_base::Stop();
 }
 
 bool CLivoxLaser::RecvData(CBinaryData& data)

laser/LivoxLaser.h

@@ -6,7 +6,7 @@
 #include <map>
 
 
-class CLivoxLaser :public CLaser
+class CLivoxLaser :public Laser_base
 {
 protected:
 	typedef enum {
@@ -22,7 +22,7 @@ protected:
 
 
 public:
-	CLivoxLaser(int id, Automatic::stLaserCalibParam laser_param);
+	CLivoxLaser(int id, Laser_proto::laser_parameter laser_param);
 	~CLivoxLaser();
 
 	virtual bool Connect();

laser/LivoxMid100Laser.cpp

@@ -1,11 +1,10 @@
 
 #include "LivoxMid100Laser.h"
-#include "../common.h"
-
+#include <glog/logging.h>
 RegisterLaser(LivoxMid100)
 
 
-CLivoxMid100Laser::CLivoxMid100Laser(int id, Automatic::stLaserCalibParam laser_param)
+CLivoxMid100Laser::CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_param)
 	:CLivoxLaser(id, laser_param)
 {
 	m_frame_maxnum = laser_param.frame_num();
@@ -44,8 +43,15 @@ void CLivoxMid100Laser::UpdataHandle()
 }
 bool CLivoxMid100Laser::IsScanComplete()
 {
-	return g_count[m_handle1] >= m_frame_maxnum && g_count[m_handle2] >= m_frame_maxnum
-		&& g_count[m_handle2] >= m_frame_maxnum;
+    if(mp_laser_task==NULL)
+    {
+        return false;
+    }
+    else {
+        int frame_count=mp_laser_task->get_task_points_number();
+        return g_count[m_handle1] >= frame_count && g_count[m_handle2] >= frame_count
+            && g_count[m_handle2] >= frame_count;
+    }
 }
 
 eLaserStatu CLivoxMid100Laser::GetStatu()
@@ -92,7 +98,7 @@ bool CLivoxMid100Laser::Start()
 		g_count[m_handle1] = 0;
 		g_count[m_handle2] = 0;
 		g_count[m_handle3] = 0;
-		return CLaser::Start();
+		return Laser_base::Start();
 }
 
 CLivoxMid100Laser::~CLivoxMid100Laser()

laser/LivoxMid100Laser.h

@@ -7,7 +7,7 @@ class CLivoxMid100Laser : public CLivoxLaser
 {
 
 public:
-	CLivoxMid100Laser(int id, Automatic::stLaserCalibParam laser_param);
+	CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_param);
 	~CLivoxMid100Laser();
 
     virtual bool Connect();

laser/laser.cpp

@@ -1,19 +0,0 @@
-//
-// Created by zx on 2020/1/2.
-//
-
-#include "laser.h"
-
-Laser_base::Laser_base()
-{
-
-}
-
-Laser_base::~Laser_base()
-{
-
-}
-Error_manager Laser_base::get_error()
-{
-    return SUCCESS;
-}

laser/laser_task_command.cpp

@@ -10,7 +10,7 @@
 //构造函数
 Laser_task::Laser_task()
 {
-    m_task_type = UNKNOW_TASK;
+    m_task_type = LASER_TASK;
     m_task_statu = TASK_CREATED;
     //m_task_statu_information默认为空
 
@@ -29,16 +29,14 @@ Laser_task::~Laser_task()
 // input:task_statu任务状态
 // output:p_task_point3d_cloud_vector三维点云容器指针
 //注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
-Error_manager Laser_task::task_init(Task_type task_type,
-                                    Task_statu task_statu,
-                                    std::vector<CPoint3D> * p_task_point3d_cloud_vector)
+Error_manager Laser_task::task_init( Task_statu task_statu,
+                                     pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector)
 {
-    if(p_task_point3d_cloud_vector == NULL)
+    if(p_task_point3d_cloud_vector.get() == NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "Laser_task::task_init failed");
     }
-    m_task_type = task_type;
     m_task_statu = task_statu;
     m_task_statu_information.clear();
 
@@ -56,18 +54,16 @@ Error_manager Laser_task::task_init(Task_type task_type,
 //    input:task_points_number点云的采集数量
 //    output:p_task_point3d_cloud_vector三维点云容器指针
 //注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
-Error_manager Laser_task::task_init(Task_type task_type,
-                                    Task_statu task_statu,
+Error_manager Laser_task::task_init(Task_statu task_statu,
                                     std::string & task_statu_information,
                                     int task_points_number,
-                                    std::vector<CPoint3D> * p_task_point3d_cloud_vector)
+                                    pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector)
 {
-    if(p_task_point3d_cloud_vector == NULL)
+    if(p_task_point3d_cloud_vector.get() == NULL)
     {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "Laser_task::task_init failed");
     }
-    m_task_type = task_type;
     m_task_statu = task_statu;
     m_task_statu_information = task_statu_information;
 
@@ -84,7 +80,7 @@ int Laser_task::get_task_points_number()
     return m_task_points_number;
 }
 //获取 三维点云容器
-std::vector<CPoint3D> * Laser_task::get_task_point3d_cloud_vector()
+pcl::PointCloud<pcl::PointXYZ>::Ptr Laser_task::get_task_point3d_cloud_vector()
 {
     return pm_task_point3d_cloud_vector;
 }
@@ -110,7 +106,7 @@ void Laser_task::set_task_points_number(int task_points_number)
     m_task_points_number = task_points_number;
 }
 //设置 三维点云容器
-void Laser_task::set_task_point3d_cloud_vector(std::vector<CPoint3D> * p_task_point3d_cloud_vector)
+void Laser_task::set_task_point3d_cloud_vector(pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector)
 {
     pm_task_point3d_cloud_vector = p_task_point3d_cloud_vector;
 }
@@ -120,6 +116,16 @@ void Laser_task::set_task_error_manager(Error_manager & error_manager)
     m_task_error_manager = error_manager;
 }
 
+//获取采集的点云保存路径
+std::string Laser_task::get_save_path()
+{
+    return m_save_path;
+}
+//设置采集的点云保存路径
+void Laser_task::set_save_path(std::string save_path)
+{
+    m_save_path=save_path;
+}
 
 
 

laser/laser_task_command.h

@@ -11,6 +11,8 @@
 #define __LASER_TASK_COMMAND__HH__
 #include "Point2D.h"
 #include "Point3D.h"
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
 #include "../error_code/error_code.h"
 
 #include <vector>
@@ -33,9 +35,8 @@ public:
     // input:task_statu任务状态
     // output:p_task_point3d_cloud_vector三维点云容器指针
     //注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
-    Error_manager task_init(Task_type task_type,
-                            Task_statu task_statu,
-                            std::vector<CPoint3D> * p_task_point3d_cloud_vector);
+    Error_manager task_init(Task_statu task_statu,
+                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector);
 
     //初始化任务单，必须初始化之后才可以使用，（可选参数）
     //    input:task_type 任务类型
@@ -44,16 +45,15 @@ public:
     //    input:task_points_number点云的采集数量
     //    output:p_task_point3d_cloud_vector三维点云容器指针
     //注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
-    Error_manager task_init(Task_type task_type,
-                            Task_statu task_statu,
+    Error_manager task_init(Task_statu task_statu,
                             std::string & task_statu_information,
                             int task_points_number,
-                            std::vector<CPoint3D> * p_task_point3d_cloud_vector);
+                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector);
 
     //获取 点云的采集数量
     int get_task_points_number();
     //获取 三维点云容器
-    std::vector<CPoint3D> * get_task_point3d_cloud_vector();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point3d_cloud_vector();
     //获取 错误码
     Error_manager get_task_error_manager();
 
@@ -64,17 +64,21 @@ public:
     //设置 点云的采集数量
     void set_task_points_number(int task_points_number);
     //设置 三维点云容器
-    void set_task_point3d_cloud_vector(std::vector<CPoint3D> * p_task_point3d_cloud_vector);
+    void set_task_point3d_cloud_vector(pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point3d_cloud_vector);
     //设置 错误码
     void set_task_error_manager(Error_manager & error_manager);
+    //获取采集的点云保存路径
+    std::string get_save_path();
+    //设置采集的点云保存路径
+    void set_save_path(std::string save_path);
 
 protected:
     //点云的采集数量，任务输入
     int                             m_task_points_number;
-
+    std::string                     m_save_path;
     //采集结果，三维点云容器，任务输出
     //这里只是指针，实际内存由应用层管理，初始化时必须保证内存有效。
-    std::vector<CPoint3D> *         pm_task_point3d_cloud_vector;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr        pm_task_point3d_cloud_vector;
 
     //错误码，任务故障信息，任务输出
     Error_manager                   m_task_error_manager;

main.cpp

@@ -4,7 +4,7 @@
 #include <fcntl.h>
 #include <iostream>
 #include "plc/plc_communicator.h"
-#include "laser/laser.h"
+#include "laser/Laser.h"
 #include "plc/plc_communicator.h"
 #include "locate/locater.h"
 #include "terminor/terminal_command_executor.h"

plc/plc_task.cpp

@@ -9,7 +9,7 @@ Plc_Task::~Plc_Task(){
 
 }
 
-int Plc_Task::init(){
+Error_manager Plc_Task::init(){
     m_measure_result.terminal_id = -1;
     m_measure_result.correctness = false;
     m_measure_result.x = 0;
@@ -20,7 +20,7 @@ int Plc_Task::init(){
     m_measure_result.height = 0;
     m_measure_result.wheel_base = 0;
     m_task_statu = Task_statu::TASK_CREATED;
-    return 1;
+    return SUCCESS;
 }
 
 Error_manager Plc_Task::get_result(struct measure_result &result){

plc/plc_task.h

@@ -22,7 +22,7 @@ struct measure_result
 class Plc_Task : public Task_Base
 {
 public:
-    virtual int init();
+    virtual Error_manager init();
     
     Plc_Task();
     ~Plc_Task();

system_manager/System_Manager.cpp

@@ -7,6 +7,7 @@
 #include <google/protobuf/io/zero_copy_stream_impl.h>
 #include <google/protobuf/text_format.h>
 #include <fcntl.h>
+#include <livox_sdk.h>
 using google::protobuf::io::FileInputStream;
 using google::protobuf::io::FileOutputStream;
 using google::protobuf::io::ZeroCopyInputStream;
@@ -75,6 +76,12 @@ System_Manager::~System_Manager()
 Error_manager System_Manager::init()
 {
     Error_manager code;
+    //读laser配置
+    Laser_proto::Laser_parameter_all laser_parameters;
+    if(!read_proto_param("./setting/laser.prototxt",laser_parameters))
+    {
+        return Error_manager(SYSTEM_READ_PARAMETER_ERROR,NORMAL,"read laser parameter failed");
+    }
     //读取plc配置
     plc_module::plc_connection_params plc_parameter;
     if(!read_proto_param("./setting/plc.prototxt",plc_parameter))
@@ -95,7 +102,36 @@ Error_manager System_Manager::init()
     }
     //第二步,根据配置,创建各个模块
     //创建雷达
-    //
+    int laser_cout=laser_parameters.laser_parameters_size();
+    m_laser_vector.resize(laser_cout);
+    for(int i=0;i<laser_parameters.laser_parameters_size();++i)
+    {
+        m_laser_vector[i]=LaserRegistory::CreateLaser(laser_parameters.laser_parameters(i).type(),i,
+                                                      laser_parameters.laser_parameters(i));
+        if(m_laser_vector[i]!=NULL)
+        {
+            if(!m_laser_vector[i]->Connect())
+            {
+                char description[255]={0};
+                sprintf(description,"Laser %d connect failed...",i);
+                return Error_manager(LASER_CONNECT_FAILED,NORMAL,description);
+            }
+        }
+    }
+    //查询是否有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)
+        {
+            if (Start() == false)
+            {
+                Uninit();
+                return Error_manager(LASER_LIVOX_SKD_INIT_FAILED,NORMAL,"Livox laser init failed...");
+            }
+            break;
+        }
+    }
     //创建测量模块
     mp_locater=new Locater();
     code=mp_locater->init(locater_parameter);

system_manager/System_Manager.h

@@ -4,7 +4,7 @@
 
 #ifndef SYSTEM_MANAGER_H
 #define SYSTEM_MANAGER_H
-#include "../laser/laser.h"
+#include "../laser/Laser.h"
 #include "../plc/plc_communicator.h"
 #include "../locate/locater.h"
 #include "../terminor/terminal_command_executor.h"

terminor/terminal_command_executor.cpp

@@ -1,6 +1,7 @@
 #include "terminal_command_executor.h"
 #include <glog/logging.h>
 #include <chrono>
+#include <pcl/filters/passthrough.h>
 
 //////以下两个函数用于测试,读取指定点云
 bool string2point(std::string str,pcl::PointXYZ& point)
@@ -115,7 +116,7 @@ Error_manager Terminor_Command_Executor::execute_command(std::vector<Laser_base*
     //第二步,雷达,plc以及定位模块是否能正常接收指令;
     for(int i=0;i<lasers.size();++i)
     {
-        code=lasers[i]->get_error();
+        code=lasers[i]->check_error();
         if(code!=SUCCESS)
         {
             LOG(ERROR)<<" terminor check laser error:"<<code.to_string()<<"   terminor id :"<<m_terminor_parameter.id();
@@ -189,7 +190,10 @@ Error_manager Terminor_Command_Executor::working()
     std::vector<Task_Base*> laser_task_vector;
     for(int i=0;i<mp_laser_vector.size();++i)
     {
-        Task_Base* laser_task=0;//new Task_Base();
+        Laser_task* laser_task=new Laser_task();
+        //
+        laser_task->task_init(TASK_CREATED,scan_cloud);
+        laser_task->set_task_points_number(1000);
         laser_task_vector.push_back(laser_task);
         //发送任务单给雷达
     }
@@ -243,8 +247,25 @@ Error_manager Terminor_Command_Executor::working()
     }
     //第二步,根据区域筛选点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr locate_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-    ///测试读取指令点云
-    locate_cloud=ReadTxtCloud("/home/zx/zzw/code/Detection_src/LidarCppAD/extract/black_car_sample/20191217-221612.txt");
+    pcl::PassThrough<pcl::PointXYZ> passX;
+    pcl::PassThrough<pcl::PointXYZ> passY;
+    pcl::PassThrough<pcl::PointXYZ> passZ;
+    passX.setInputCloud(scan_cloud);
+    passX.setFilterFieldName("x");
+    passX.setFilterLimits(m_terminor_parameter.area_3d().min_x(),m_terminor_parameter.area_3d().max_x());
+    passX.filter(*locate_cloud);
+
+    passY.setInputCloud(locate_cloud);
+    passY.setFilterFieldName("y");
+    passY.setFilterLimits(m_terminor_parameter.area_3d().min_y(),m_terminor_parameter.area_3d().max_y());
+    passY.filter(*locate_cloud);
+
+    passY.setInputCloud(locate_cloud);
+    passY.setFilterFieldName("z");
+    passY.setFilterLimits(m_terminor_parameter.area_3d().min_z(),m_terminor_parameter.area_3d().max_z());
+    passY.filter(*locate_cloud);
+
+
     //第三步,创建测量任务, 进入测量中
     Locate_task* locate_task=new Locate_task();
     locate_task->set_locate_cloud(locate_cloud);

terminor/terminal_command_executor.h

@@ -7,7 +7,7 @@
 
 #include "../plc/plc_communicator.h"
 #include "../plc/plc_task.h"
-#include "../laser/laser.h"
+#include "../laser/Laser.h"
 #include "../locate/locater.h"
 #include "Terminor_parameter.pb.h"
 #include <thread>