add sick laser nnxx

GarageMeasurement.pro

@@ -24,8 +24,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 # add pcl
 INCLUDEPATH += /usr/include/eigen3
-INCLUDEPATH += /usr/include/vtk-6.2
-LIBS += /usr/lib/x86_64-linux-gnu/libvtk*.so
+#INCLUDEPATH += /usr/include/vtk-6.2
+#LIBS += /usr/lib/x86_64-linux-gnu/libvtk*.so
 INCLUDEPATH += /usr/include/boost
 LIBS += /usr/lib/x86_64-linux-gnu/libboost_*.so
 INCLUDEPATH += /usr/local/include/pcl-1.8
@@ -57,15 +57,10 @@ LIBS += /usr/local/cuda/lib64/libcudart.so
 LIBS += /usr/local/cuda/lib64/libcurand.so
 LIBS += /usr/local/cuda/lib64/libcufft.so
 
-
-# locate
-LIBS += /home/zx/zzw/GarageMeasurement/libs/libdark.so
-LIBS += /home/zx/zzw/GarageMeasurement/libs/libpointSIFT_API.so
-LIBS += /home/zx/zzw/GarageMeasurement/libs/libtf_3dcnn_api.so
-LIBS += /home/zx/zzw/GarageMeasurement/libs/libLocate_API.so
-LIBS += /home/zx/zzw/inv/tensorflow-1.8.0/bazel-bin/tensorflow/libtensorflow_cc.so
-LIBS += /home/zx/zzw/inv/tensorflow-1.8.0/bazel-bin/tensorflow/libtensorflow_framework.so
-
+# nnxx
+INCLUDEPATH += /usr/local/include
+#LIBS += /usr/local/lib/libnanomsg.so
+LIBS += /usr/local/lib/libnnxx.a
 
 SOURCES += \
     laser/Laser.cpp \
@@ -87,7 +82,10 @@ SOURCES += \
     src/measuretask.cpp \
     Process.cpp \
     src/pathcreator.cpp \
-    laser/LivoxHubLaser.cpp
+    laser/LivoxHubLaser.cpp \
+    laser/Sick511FileLaser.cpp \
+    laser/TcpLaser.cpp \
+    laser/UdpLaser.cpp
 
 HEADERS += \
     laser/Laser.h \
@@ -127,12 +125,16 @@ HEADERS += \
     src/pathcreator.h \
     laser/LivoxHubLaser.h \
     qtmessagedef.h \
-    src/locate/Locater.h \
-    src/locate/Locater3Dcnn.h \
-    src/locate/PointSiftSeg.h \
-    src/locate/yolo_v2_class.hpp \
-    src/locate/YoloDetector.h \
-    src/locate/pointSIFT_API.h
+    laser/Sick511FileLaser.h \
+    laser/TcpLaser.h \
+    laser/UdpLaser.h
 
 FORMS += \
         mainwindow.ui
+
+win32:CONFIG(release, debug|release): LIBS += -L$$PWD/../../../../../usr/local/lib/release/ -lnanomsg
+else:win32:CONFIG(debug, debug|release): LIBS += -L$$PWD/../../../../../usr/local/lib/debug/ -lnanomsg
+else:unix: LIBS += -L$$PWD/../../../../../usr/local/lib/ -lnanomsg
+
+INCLUDEPATH += $$PWD/''
+DEPENDPATH += $$PWD/''

GarageMeasurement.pro.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.4.1, 2019-10-29T19:23:33. -->
+<!-- Written by QtCreator 4.4.1, 2019-10-31T23:14:48. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
@@ -64,7 +64,7 @@
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop Qt 5.9.2 GCC 64bit</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop Qt 5.9.2 GCC 64bit</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">qt.592.gcc_64_kit</value>
-   <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">1</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
    <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
    <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
@@ -312,7 +312,7 @@
     <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">GarageMeasurement.pro</value>
     <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
     <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory"></value>
-    <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory.default">/home/zx/zzw/GarageMeasurement/build-GarageMeasurement-Desktop_Qt_5_9_2_GCC_64bit-Release</value>
+    <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory.default">/home/zx/zzw/GarageMeasurement/build-GarageMeasurement-Desktop_Qt_5_9_2_GCC_64bit-Debug</value>
     <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
     <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
     <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>

Process.cpp

@@ -10,7 +10,6 @@ CProcess::CProcess(Automatic::stCalibParam param, void* mainWnd)
 	m_laser = NULL;
 	m_main_wnd = mainWnd;
     m_thread_queue=0;
-    m_locater=0;
     connect(this,SIGNAL(MainWndSignal(QVariant)),
             (MainWindow*)m_main_wnd,SLOT(ProcessSlot(QVariant)));
 }
@@ -50,11 +49,7 @@ CProcess::~CProcess()
 		delete[] m_laser;
 		m_laser = NULL;
 	}
-	if (m_laser_calib_param.is_calib() == false)
-	{
-        if(m_locater)
-            delete m_locater;
-	}
+
 
 	
 }
@@ -62,6 +57,8 @@ CProcess::~CProcess()
 
 bool CProcess::Init()
 {
+
+
     m_thread_queue=tq::TQFactory::CreateDefaultQueue();
     m_thread_queue->Start(3);
 
@@ -137,10 +134,7 @@ bool CProcess::Init()
             m_monitor.set_callback(action_callback, monitor_callback,this);
 		}
 
-        if (m_locater == 0)
-		{
-			m_locater = new CLocater();
-        }
+
 	}
 
 	return true;
@@ -170,7 +164,7 @@ void CProcess::PushTask(uint16_t param, int action_type,bool test)
 		plc = &(m_monitor);
     tq::BaseTask* task = 0;
 	if(action_type==1)
-        task= new MeasureTask(lasers, /*m_locater*/0, int(plate),plc, m_laser_calib_param);
+        task= new MeasureTask(lasers,  int(plate),plc, m_laser_calib_param);
     /*else if(action_type==2)
         task = new CFenceTask(lasers, m_locater, int(plate), plc, m_laser_calib_param);*/
 

Process.h

@@ -4,6 +4,7 @@
 #include "./modbus/PLCMonitor.h"
 #include "./src/CalibParam.pb.h"
 #include "laser/LivoxLaser.h"
+#include "laser/Sick511FileLaser.h"
 #include "src/measuretask.h"
 #include "qtmessagedef.h"
 
@@ -30,7 +31,6 @@ protected:
     static void result_callback(void* data,void* pointer);
 
 public:
-    CLocater*                                   m_locater;
 
 	CLaser**  m_laser;
 	modbus::CPLCMonitor							m_monitor;

common.h

@@ -24,11 +24,11 @@
 #include <pcl/point_types.h>
 #include <pcl/common/io.h>
 #include <pcl/common/common.h>
-#include <pcl/visualization/pcl_visualizer.h>
+//#include <pcl/visualization/pcl_visualizer.h>
 
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
-#include <pcl/visualization/cloud_viewer.h>
+//#include <pcl/visualization/cloud_viewer.h>
 
 #include <pcl/filters/extract_indices.h>
 #include <pcl/features/normal_3d.h>
@@ -48,16 +48,16 @@
 
 
 //vtk
-#include <vtkRenderWindow.h>
-#include <vtkRenderWindowInteractor.h>
-#include "vtkRenderingCoreModule.h" // For export macro
-#include "vtkViewport.h"
-#include "vtkConeSource.h"
-#include "vtkPolyDataMapper.h"
-#include "vtkCamera.h"
-#include "vtkActor.h"
-#include "vtkRenderer.h"
-#include "vtkProperty.h"
+//#include <vtkRenderWindow.h>
+//#include <vtkRenderWindowInteractor.h>
+//#include "vtkRenderingCoreModule.h" // For export macro
+//#include "vtkViewport.h"
+//#include "vtkConeSource.h"
+//#include "vtkPolyDataMapper.h"
+//#include "vtkCamera.h"
+//#include "vtkActor.h"
+//#include "vtkRenderer.h"
+//#include "vtkProperty.h"
 
 typedef pcl::PointXYZ PointT;
 typedef pcl::PointCloud<PointT> PointCloudT;

laser/Laser.cpp

@@ -43,7 +43,7 @@ char* CBinaryData::Data()const {
 int CBinaryData::Length()const {
 	return m_length;
 }
-CBinaryData& CBinaryData::operator=(CBinaryData& data)
+CBinaryData& CBinaryData::operator=(CBinaryData data)
 {
 	if (m_buf)
 	{

laser/Laser.h

@@ -24,7 +24,7 @@ public:
 	CBinaryData(const CBinaryData& data);
 	~CBinaryData();
 	CBinaryData(const char* buf, int len, DATA_type type= eUnknow);
-	CBinaryData& operator=(CBinaryData& data);
+    CBinaryData& operator=(CBinaryData data);
 	bool operator==(const char* str);
 	const char* operator+(int n);
 	CBinaryData& operator+(CBinaryData& data);

laser/Sick511FileLaser.cpp

@@ -0,0 +1,297 @@
+
+#include "Sick511FileLaser.h"
+#include <unistd.h>
+
+RegisterLaser(Sick511File);
+CSick511FileLaser::CSick511FileLaser(int id, Automatic::stLaserCalibParam laser_param)
+	:CLaser(id, laser_param)
+	,m_start_read(false)
+{
+}
+
+CSick511FileLaser::~CSick511FileLaser()
+{
+	if (m_stream_read.is_open())
+		m_stream_read.close();
+}
+
+bool CSick511FileLaser::Connect()
+{
+	std::string ip = m_laser_param.laser_ip();
+	char file[255] = { 0 };
+	sprintf(file, "%s/laser%d.data", ip.c_str(), m_id + 1);
+	if (m_stream_read.is_open())
+	{
+		m_stream_read.close();
+	}
+	m_stream_read.open(file, ios::in | ios::binary);
+	if (!m_stream_read.good())
+		return false;
+	bool ret= CLaser::Connect();
+	m_statu = eLaser_ready;
+	m_file = file;
+	return ret;
+}
+void CSick511FileLaser::Disconnect()
+{
+	if(m_stream_read.is_open())
+		m_stream_read.close();
+	return CLaser::Disconnect();
+}
+bool CSick511FileLaser::Start()
+{
+	std::lock_guard<std::mutex> lk(m_mutex);
+	
+	if (!this->IsReady())
+		return false;
+	
+	m_start_read = true;
+	return CLaser::Start();
+	
+}
+
+bool CSick511FileLaser::Stop()
+{
+	std::lock_guard<std::mutex> lk(m_mutex);
+	m_start_read = false;
+	return true;
+}
+
+DATA_type CSick511FileLaser::Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)
+{
+	////拼接上次未打包数据
+	
+	DATA_type type = eUnknow;
+	CBinaryData frame;
+
+	if (m_last_data.Length() > 0)
+	{
+		if (pData)
+			frame = m_last_data + (*pData);
+		else
+			frame = m_last_data;
+		m_last_data = CBinaryData();
+	}
+	else if(pData)
+	{
+		frame = (*pData);
+	}
+	else
+	{
+		return type;
+	}
+
+	int head = FindHead(frame.Data(), frame.Length());
+	int tail = FindTail(frame.Data(), frame.Length());
+	
+	if (tail >= 0)
+	{
+		if (tail < frame.Length() - 1)		//包尾后还有数据
+		{
+			m_last_data = CBinaryData(frame + tail + 1, frame.Length() - tail - 1);
+		}
+		if (head >= 0 && head < tail)
+		{
+			////解包
+			CBinaryData data(frame + head, tail - head + 1);
+			if (data == "$SLSSTP")
+				type = eStop;
+			else if (data == "$SLSSTA")
+				type = eStart;
+			else if (data == "$SCLRDY")
+				type = eReady;
+			else if (data == "$SHWERR")
+				type = eHerror;
+			else if (data == "$N")
+				type = eData;
+
+			if (type == eData)
+			{
+				////解析数据
+				points.clear();
+				if (data.Length() <= 29)
+					return type;
+
+				////解析
+				unsigned char angle_1 = 0;
+				unsigned char angle_2 = 0;
+				memcpy(&angle_1, (data)+26, 1);
+				memcpy(&angle_2, (data)+27, 1);
+				float beta_a = float(angle_1 * 256 + angle_2)*0.01;
+				float start_angle = 0.0;
+				float freq = 0.0;
+
+				std::vector<float> distance;
+				if (GetData(&data, distance, freq, start_angle))
+				{
+					float beta = (beta_a)*DEGREES;
+					float sin_beta = sin(beta);
+					float cos_beta = cos(beta);
+					for (int i = 0; i < distance.size(); ++i)
+					{
+						if (distance[i] < 0.001)
+							continue;
+
+						float alpha = (start_angle + i*freq - 90.0) * DEGREES;
+						float sin_alpha = sin(alpha);
+						float cos_alpha = cos(alpha);
+
+						float x = distance[i] * sin_alpha;
+						float y = distance[i] * cos_alpha * sin_beta;
+						float z = distance[i] * cos_alpha * cos_beta;
+
+						points.push_back(CPoint3D(x, y, z));
+					}
+					//points.push_back(CPoint3D(double(distance.size()), start_angle, freq));
+				}
+
+			}
+			else if (type == eStop)
+			{
+				m_statu = eLaser_ready;
+			}
+		}
+	}
+	else if (head >= 0)
+	{
+		m_last_data = CBinaryData(frame + head, frame.Length() - head);
+	}
+
+	return type;
+}
+
+bool CSick511FileLaser::RecvData(CBinaryData& data)
+{
+	if (m_start_read == false)
+		return false;
+	if (!m_stream_read.is_open())
+		return false;
+	
+	if (m_stream_read.eof())
+	{
+		Stop();
+		m_stream_read.close();
+        usleep(100*1000);
+		m_stream_read.open(m_file.c_str(), ios::in | ios::binary);
+	}
+
+	char buf[1024] = { 0 };
+	m_stream_read.read(buf, 1024);
+	int count = m_stream_read.gcount();
+	if (count > 0)
+	{
+		CBinaryData bin_data(buf, count);
+		data = bin_data;
+		return true;
+	}
+	return false;
+}
+
+int CSick511FileLaser::FindHead(char* buf, int b_len)
+{
+	int i = 0;
+	if (b_len < 2)
+		return NULL;
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len > 10)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'N' && buf[i + 7] == 'S'
+				&&buf[i + 8] == '5'&&buf[i + 9] == '1'&&buf[i + 10] == '1')
+				return i;
+		}
+		if (b_len > 7)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'S'&&buf[i + 7] == '*')
+				return i;
+		}
+	}
+	return -1;
+}
+
+int CSick511FileLaser::FindTail(char* buf, int b_len)
+{
+	int i = 0;
+
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len >= i + 5)
+		{
+			if (buf[i] == '*' && buf[i + 3] == '\r'&&buf[i + 4] == '\n')
+				return i + 4;
+		}
+	}
+	return -9999999;
+}
+
+bool CSick511FileLaser::GetData(CBinaryData* pData, std::vector<float>& distance,
+	float& freq, float& start_angle)
+{
+	struct stData
+	{
+		const char* data;
+		int length;
+	};
+	std::vector<struct stData> strDatas;
+	int start = 0;
+	int end = 0;
+	int LMDscandata_index = -1;
+	for (int i = 0; i < pData->Length(); ++i)
+	{
+		if ((*pData)[i] == ' ')
+		{
+			end = i;
+			if (end > start)
+			{
+				struct stData strData;
+				strData.data = (*pData + start);
+				strData.length = end - start;
+
+				strDatas.push_back(strData);
+				if (strncmp(strData.data, "LMDscandata", 11) == 0)
+					LMDscandata_index = strDatas.size() - 1;
+			}
+			end = i + 1;
+			start = end;
+		}
+	}
+
+	if (strDatas.size() > 26 + LMDscandata_index - 1)
+	{
+		struct stData start_angle_str = strDatas[23 + LMDscandata_index - 1];
+		long start_angle_l = Str0x2Long(start_angle_str.data, start_angle_str.length);
+		start_angle = float(start_angle_l)*0.01;
+
+		struct stData freq_str = strDatas[24 + LMDscandata_index - 1];
+		long freq_l = Str0x2Long(freq_str.data, freq_str.length);
+		freq = float(freq_l)*0.0001;
+
+		struct stData count_str = strDatas[25 + LMDscandata_index - 1];
+		long count = Str0x2Long(count_str.data, count_str.length);
+		if (strDatas.size() >= 26 + LMDscandata_index - 1 + count)
+		{
+			for (int i = 26 + LMDscandata_index - 1; i < 26 + LMDscandata_index - 1 + count; ++i)
+			{
+				float dis = float(Str0x2Long(strDatas[i].data, strDatas[i].length));
+				distance.push_back(dis);
+			}
+			return true;
+		}
+	}
+	return false;
+}
+long CSick511FileLaser::Str0x2Long(const char* data, int len)
+{
+	long sum = 0;
+	for (int i = 0; i < len; ++i)
+	{
+		char c = data[i];
+		int n = 0;
+		if (c >= 48 && c <= 57)
+			n = c - 48;
+		else if (c >= 65 && c <= 70)
+			n = c - 65 + 10;
+		sum += n*pow(16, len - i - 1);
+	}
+	return sum;
+}

laser/Sick511FileLaser.h

@@ -0,0 +1,37 @@
+/*************************
+sick 511 À×´ï ÎļþÊý¾Ý½âÎö
+*************************/
+#ifndef __SICK_511_LASER_FILE__HH
+#define __SICK_511_LASER_FILE__HH
+#include "Laser.h"
+#include <fstream>
+class CSick511FileLaser :
+	public CLaser
+{
+public:
+	CSick511FileLaser(int id, Automatic::stLaserCalibParam laser_param);
+	virtual ~CSick511FileLaser();
+	virtual bool Connect();
+	virtual void Disconnect();
+	virtual bool Start();
+	virtual bool Stop();
+
+protected:
+	virtual bool RecvData(CBinaryData& data);
+	virtual DATA_type Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points);
+
+protected:
+	int FindHead(char* buf, int b_len);
+	int FindTail(char* buf, int b_len);
+protected:
+	virtual bool GetData(CBinaryData* pData, std::vector<float>& distance,
+		float& freq, float& start_angle);
+	long Str0x2Long(const char* data, int len);
+
+protected:
+	std::ifstream	m_stream_read;
+	std::string		m_file;
+	bool			m_start_read;
+	std::mutex		m_mutex;
+};
+#endif

laser/TcpLaser.cpp

@@ -0,0 +1,312 @@
+#include "TcpLaser.h"
+#include <stdlib.h>
+#include <unistd.h>
+
+RegisterLaser(Tcp);
+CTcpLaser::CTcpLaser(int id, Automatic::stLaserCalibParam laser_param)
+	:CLaser(id,laser_param)
+{
+}
+
+CTcpLaser::~CTcpLaser()
+{
+}
+
+bool CTcpLaser::Connect()
+{
+	std::string ip = m_laser_param.laser_ip();
+	int			port = m_laser_param.laser_port();
+	int			remoteport = m_laser_param.laser_port_remote();
+	if (ip == "" || port < 0)
+		return false;
+
+	//初始化Socket
+    //WSAStartup(MAKEWORD(1, 1), &m_wsd);
+	//创建Socket对象
+    m_socket = socket(AF_INET, SOCK_STREAM, 0);
+	if (m_socket <= 0)
+		return false;
+
+	m_send_addr.sin_family = AF_INET;
+	m_send_addr.sin_addr.s_addr = inet_addr(ip.c_str());
+	m_send_addr.sin_port = htons(remoteport);
+
+    if (0 != ::connect(m_socket, (struct sockaddr *)&m_send_addr, sizeof(struct sockaddr)))
+		return false;
+	m_last_data_time = clock();
+	return CLaser::Connect();
+}
+void CTcpLaser::Disconnect()
+{
+	if (m_socket > 0)
+	{
+        close(m_socket);
+    //	WSACleanup();
+		m_socket = -1;
+	}
+	return CLaser::Disconnect();
+}
+bool CTcpLaser::Start()
+{
+	std::lock_guard<std::mutex> lk(m_mutex);
+	if (!this->IsReady())
+		return false;
+
+	char* sendMsg = "$SLSSTA*0A\r\n";
+	if (Send(sendMsg, strlen(sendMsg)))
+	{
+		return CLaser::Start();
+	}
+	return false;
+}
+
+bool CTcpLaser::Stop()
+{
+	char sendMsg[] = "$SLSSTP*1B\r\n";
+	std::lock_guard<std::mutex> lk(m_mutex);
+	if (Send(sendMsg, strlen(sendMsg)))
+		return true;
+	return false;
+}
+
+DATA_type CTcpLaser::Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)
+{
+	////拼接上次未打包数据
+
+	DATA_type type = eUnknow;
+	CBinaryData frame;
+
+	if (m_last_data.Length() > 0)
+	{
+		if (pData)
+			frame = m_last_data + (*pData);
+		else
+			frame = m_last_data;
+		m_last_data = CBinaryData();
+	}
+	else if (pData)
+	{
+		frame = (*pData);
+	}
+	else
+	{
+		return type;
+	}
+
+	int head = FindHead(frame.Data(), frame.Length());
+	int tail = FindTail(frame.Data(), frame.Length());
+
+	if (tail >= 0)
+	{
+		if (tail < frame.Length() - 1)		//包尾后还有数据
+		{
+			m_last_data = CBinaryData(frame + tail + 1, frame.Length() - tail - 1);
+		}
+		if (head >= 0 && head < tail)
+		{
+			////解包
+			CBinaryData data(frame + head, tail - head + 1);
+			if (data == "$SLSSTP")
+				type = eStop;
+			else if (data == "$SLSSTA")
+				type = eStart;
+			else if (data == "$SCLRDY")
+				type = eReady;
+			else if (data == "$SHWERR")
+				type = eHerror;
+			else if (data == "$N")
+				type = eData;
+
+			if (type == eData)
+			{
+				////解析数据
+				points.clear();
+				if (data.Length() <= 29)
+					return type;
+
+				////解析
+				unsigned char angle_1 = 0;
+				unsigned char angle_2 = 0;
+				memcpy(&angle_1, (data)+26, 1);
+				memcpy(&angle_2, (data)+27, 1);
+				float beta_a = float(angle_1 * 256 + angle_2)*0.01;
+				float start_angle = 0.0;
+				float freq = 0.0;
+
+				std::vector<float> distance;
+				if (GetData(&data, distance, freq, start_angle))
+				{
+					float beta = (beta_a)*DEGREES;
+					float sin_beta = sin(beta);
+					float cos_beta = cos(beta);
+					for (int i = 0; i < distance.size(); ++i)
+					{
+						if (distance[i] < 0.001)
+							continue;
+
+						float alpha = (start_angle + i*freq - 90.0) * DEGREES;
+						float sin_alpha = sin(alpha);
+						float cos_alpha = cos(alpha);
+
+						float x = distance[i] * sin_alpha;
+						float y = distance[i] * cos_alpha * sin_beta;
+						float z = distance[i] * cos_alpha * cos_beta;
+
+						points.push_back(CPoint3D(x, y, z));
+					}
+					//points.push_back(CPoint3D(double(distance.size()), start_angle, freq));
+				}
+
+			}
+		}
+	}
+	else if (head >= 0)
+	{
+		m_last_data = CBinaryData(frame + head, frame.Length() - head);
+	}
+
+	return type;
+}
+
+bool CTcpLaser::RecvData(CBinaryData& data)
+{
+	char buf[4096] = { 0 };
+	int bytesRead = Recv(buf, 4096);
+	if (bytesRead > 0)
+	{
+		CBinaryData bin_data(buf, bytesRead);
+		data = bin_data;
+		m_last_data_time = clock();
+		return true;
+	}
+	return false;
+}
+
+int CTcpLaser::FindHead(char* buf, int b_len)
+{
+	int i = 0;
+	if (b_len < 2)
+		return NULL;
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len > 10)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'N' && buf[i + 7] == 'S'
+				&&buf[i + 8] == '5'&&buf[i + 9] == '1'&&buf[i + 10] == '1')
+				return i;
+		}
+		if (b_len > 7)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'S'&&buf[i + 7] == '*')
+				return i;
+		}
+	}
+	return -1;
+}
+
+int CTcpLaser::FindTail(char* buf, int b_len)
+{
+	int i = 0;
+
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len >= i + 5)
+		{
+			if (buf[i] == '*' && buf[i + 3] == '\r'&&buf[i + 4] == '\n')
+				return i + 4;
+		}
+	}
+	return -9999999;
+}
+
+bool CTcpLaser::Send(char* sendbuf, int len)
+{
+	int ret = 0;
+	do
+	{
+        ret = send(m_socket, sendbuf, strlen(sendbuf), 0);
+	} while (ret < 0);
+	return ret == len;
+}
+int CTcpLaser::Recv(char* recvbuf, int len)
+{
+	struct sockaddr_in sddr_from;
+	int ret = 0;
+	do
+	{
+        ret = recv(m_socket, recvbuf, 4096, 0);
+	} while (ret < 0);
+	return ret;
+}
+
+bool CTcpLaser::GetData(CBinaryData* pData, std::vector<float>& distance,
+	float& freq, float& start_angle)
+{
+	struct stData
+	{
+		const char* data;
+		int length;
+	};
+	std::vector<struct stData> strDatas;
+	int start = 0;
+	int end = 0;
+	int LMDscandata_index = -1;
+	for (int i = 0; i < pData->Length(); ++i)
+	{
+		if ((*pData)[i] == ' ')
+		{
+			end = i;
+			if (end > start)
+			{
+				struct stData strData;
+				strData.data = (*pData + start);
+				strData.length = end - start;
+
+				strDatas.push_back(strData);
+				if (strncmp(strData.data, "LMDscandata", 11) == 0)
+					LMDscandata_index = strDatas.size() - 1;
+			}
+			end = i + 1;
+			start = end;
+		}
+	}
+
+	if (strDatas.size() > 26 + LMDscandata_index - 1)
+	{
+		struct stData start_angle_str = strDatas[23 + LMDscandata_index - 1];
+		long start_angle_l = Str0x2Long(start_angle_str.data, start_angle_str.length);
+		start_angle = float(start_angle_l)*0.01;
+
+		struct stData freq_str = strDatas[24 + LMDscandata_index - 1];
+		long freq_l = Str0x2Long(freq_str.data, freq_str.length);
+		freq = float(freq_l)*0.0001;
+
+		struct stData count_str = strDatas[25 + LMDscandata_index - 1];
+		long count = Str0x2Long(count_str.data, count_str.length);
+		if (strDatas.size() >= 26 + LMDscandata_index - 1 + count)
+		{
+			for (int i = 26 + LMDscandata_index - 1; i < 26 + LMDscandata_index - 1 + count; ++i)
+			{
+				float dis = float(Str0x2Long(strDatas[i].data, strDatas[i].length));
+				distance.push_back(dis);
+			}
+			return true;
+		}
+	}
+	return false;
+}
+long CTcpLaser::Str0x2Long(const char* data, int len)
+{
+	long sum = 0;
+	for (int i = 0; i < len; ++i)
+	{
+		char c = data[i];
+		int n = 0;
+		if (c >= 48 && c <= 57)
+			n = c - 48;
+		else if (c >= 65 && c <= 70)
+			n = c - 65 + 10;
+		sum += n*pow(16, len - i - 1);
+	}
+	return sum;
+}

laser/TcpLaser.h

@@ -0,0 +1,52 @@
+/*************************
+sick 511 雷达 tcp协议连接控制板
+*************************/
+
+#pragma once
+#include "Laser.h"
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <mutex>
+
+#define LASER_DATA_TIME_OUT		10000
+
+class CTcpLaser :
+	public CLaser
+{
+public:
+	CTcpLaser(int id, Automatic::stLaserCalibParam laser_param);
+	~CTcpLaser();
+
+	virtual bool Connect();
+	virtual void Disconnect();
+	virtual bool Start();
+	virtual bool Stop();
+
+protected:
+	virtual bool RecvData(CBinaryData& data);
+	virtual DATA_type Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points);
+
+
+private:
+	bool	Send(char* sendbuf, int len);                          // 发送指令
+	int		Recv(char* recvbuf, int len);                           // 接收数据
+	static int FindHead(char* buf, int b_len);
+	static int FindTail(char* buf, int b_len);
+private:
+	bool GetData(CBinaryData* pData, std::vector<float>& distance,
+		float& freq, float& start_angle);
+	long Str0x2Long(const char* data, int len);
+
+private:
+#pragma region udp相关变量
+	int				m_socket;
+    //WSADATA			m_wsd;
+	struct sockaddr_in m_send_addr;	
+	std::mutex		m_mutex;
+	clock_t			m_last_data_time;
+#pragma endregion
+
+};
+

laser/UdpLaser.cpp

@@ -0,0 +1,322 @@
+
+#include "UdpLaser.h"
+#include <stdlib.h>
+#include <unistd.h>
+
+RegisterLaser(Udp);
+CUdpLaser::CUdpLaser(int id, Automatic::stLaserCalibParam laser_param)
+	:CLaser(id,laser_param)
+{
+}
+
+
+CUdpLaser::~CUdpLaser()
+{
+}
+
+bool CUdpLaser::Connect()
+{
+	std::string ip = m_laser_param.laser_ip();
+	int			port = m_laser_param.laser_port();
+	int			remoteport = m_laser_param.laser_port_remote();
+	if (ip == "" || port < 0)
+		return false;
+
+	//初始化Socket
+    //WSAStartup(MAKEWORD(1, 1), &m_wsd);
+	//创建Socket对象
+    m_socket = socket(AF_INET, SOCK_DGRAM, 0);
+	if (m_socket <= 0)
+		return false;
+
+	struct sockaddr_in sddr_udp;
+	sddr_udp.sin_family = AF_INET;
+    sddr_udp.sin_addr.s_addr = htons(INADDR_ANY);
+	sddr_udp.sin_port = htons(port);
+
+	m_send_addr.sin_family = AF_INET;
+	m_send_addr.sin_addr.s_addr = inet_addr(ip.c_str());
+	m_send_addr.sin_port = htons(remoteport);
+
+    if(-1==bind(m_socket, (struct sockaddr *)&sddr_udp, sizeof(struct sockaddr)))
+        return false;
+
+	return CLaser::Connect();
+}
+void CUdpLaser::Disconnect()
+{
+	if (m_socket > 0)
+	{
+
+        close(m_socket);
+        //WSACleanup();
+		m_socket = -1;
+	}
+	return CLaser::Disconnect();
+}
+bool CUdpLaser::Start()
+{
+	std::lock_guard<std::mutex> lk(m_mutex);
+	if (!this->IsReady())
+		return false;
+
+	char* sendMsg = "$SLSSTA*0A\r\n";
+	if (Send(sendMsg, strlen(sendMsg)))
+	{
+		m_statu = eLaser_busy;
+
+		return CLaser::Start();
+	}
+	return false;
+}
+
+bool CUdpLaser::Stop()
+{
+	char sendMsg[] = "$SLSSTP*1B\r\n";
+	std::lock_guard<std::mutex> lk(m_mutex);
+	if (Send(sendMsg, strlen(sendMsg)))
+		return true;
+	return false;
+}
+
+DATA_type CUdpLaser::Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)
+{
+	////拼接上次未打包数据
+
+	DATA_type type = eUnknow;
+	CBinaryData frame;
+
+	if (m_last_data.Length() > 0)
+	{
+		if (pData)
+			frame = m_last_data + (*pData);
+		else
+			frame = m_last_data;
+		m_last_data = CBinaryData();
+	}
+	else if (pData)
+	{
+		frame = (*pData);
+	}
+	else
+	{
+		return type;
+	}
+
+	int head = FindHead(frame.Data(), frame.Length());
+	int tail = FindTail(frame.Data(), frame.Length());
+	if (tail >= 0)
+	{
+		if (tail < frame.Length() - 1)		//包尾后还有数据
+		{
+			m_last_data = CBinaryData(frame + tail + 1, frame.Length() - tail - 1);
+		}
+		if (head >= 0 && head < tail)
+		{
+			////解包
+			CBinaryData data(frame + head, tail - head + 1);
+			if (data == "$SLSSTP")
+				type = eStop;
+			else if (data == "$SLSSTA")
+				type = eStart;
+			else if (data == "$SCLRDY")
+				type = eReady;
+			else if (data == "$SHWERR")
+				type = eHerror;
+			else if (data == "$N")
+				type = eData;
+
+			if (type == eData)
+			{
+				////解析数据
+				points.clear();
+				if (data.Length() <= 29)
+					return type;
+
+				////解析
+				unsigned char angle_1 = 0;
+				unsigned char angle_2 = 0;
+				memcpy(&angle_1, (data)+26, 1);
+				memcpy(&angle_2, (data)+27, 1);
+				float beta_a = float(angle_1 * 256 + angle_2)*0.01;
+				float start_angle = 0.0;
+				float freq = 0.0;
+
+				std::vector<float> distance;
+				if (GetData(&data, distance, freq, start_angle))
+				{
+					
+					float beta = (beta_a)*DEGREES;
+					float sin_beta = sin(beta);
+					float cos_beta = cos(beta);
+					for (int i = 0; i < distance.size(); ++i)
+					{
+						if (distance[i] < 0.001)
+							continue;
+
+						float alpha = (start_angle + i*freq - 90.0) * DEGREES;
+						float sin_alpha = sin(alpha);
+						float cos_alpha = cos(alpha);
+
+						float x = distance[i] * sin_alpha;
+						float y = distance[i] * cos_alpha * sin_beta;
+						float z = distance[i] * cos_alpha * cos_beta;
+
+						points.push_back(CPoint3D(x, y, z));
+					}
+					//points.push_back(CPoint3D(double(distance.size()), start_angle, freq));
+				}
+
+			}
+		}
+	}
+	else if (head >= 0)
+	{
+		m_last_data = CBinaryData(frame + head, frame.Length() - head);
+	}
+	
+	return type;
+}
+
+bool CUdpLaser::RecvData(CBinaryData& data)
+{
+	char buf[4096 * 10] = { 0 };
+	int bytesRead = Recv(buf, 4096 * 10);
+	if (bytesRead > 0)
+	{
+		CBinaryData bin_data(buf, bytesRead);
+		data = bin_data;
+		return true;
+	}
+	return false;
+}
+
+int CUdpLaser::FindHead(char* buf, int b_len)
+{
+	int i = 0;
+	if (b_len < 2)
+		return NULL;
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len > 10)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'N' && buf[i + 7] == 'S'
+				&&buf[i + 8] == '5'&&buf[i + 9] == '1'&&buf[i + 10] == '1')
+				return i;
+		}
+		if (b_len >7)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'S'&&buf[i + 7]=='*')
+				return i;
+		}
+	}
+	return -1;
+}
+
+int CUdpLaser::FindTail(char* buf, int b_len)
+{
+	int i = 0;
+	
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len >=i+ 5)
+		{
+			if (buf[i] == '*' && buf[i + 3] == '\r'&&buf[i + 4] == '\n')
+				return i+4;
+		}
+	}
+	return -9999999;
+}
+
+bool CUdpLaser::Send(char* sendbuf, int len)
+{
+	int ret = 0;
+	do 
+	{
+        ret = sendto(m_socket, sendbuf, strlen(sendbuf), 0, (struct sockaddr*)&m_send_addr, sizeof(struct sockaddr));
+	} while (ret < 0);
+	return ret == len;
+}
+int CUdpLaser::Recv(char* recvbuf, int len)
+{
+	struct sockaddr_in sddr_from;
+    socklen_t clientLen = sizeof(sddr_from);
+	int ret = 0;
+	do
+	{
+        ret = recvfrom(m_socket, recvbuf, 4096, 0, (struct sockaddr*)&sddr_from, &clientLen);
+	} while (ret < 0);
+	return ret;
+}
+
+bool CUdpLaser::GetData(CBinaryData* pData, std::vector<float>& distance,
+	float& freq, float& start_angle)
+{
+	struct stData
+	{
+		const char* data;
+		int length;
+	};
+	std::vector<struct stData> strDatas;
+	int start = 0;
+	int end = 0;
+	int LMDscandata_index = -1;
+	for (int i = 0; i < pData->Length(); ++i)
+	{
+		if ((*pData)[i] == ' ')
+		{
+			end = i;
+			if (end > start)
+			{
+				struct stData strData;
+				strData.data = (*pData + start);
+				strData.length = end - start;
+				
+				strDatas.push_back(strData);
+				if (strncmp(strData.data, "LMDscandata", 11) == 0)
+					LMDscandata_index = strDatas.size() - 1;
+			}
+			end = i + 1;
+			start = end;
+		}
+	}
+
+	if (strDatas.size() > 26 + LMDscandata_index - 1)
+	{
+		struct stData start_angle_str = strDatas[23 + LMDscandata_index - 1];
+		long start_angle_l = Str0x2Long(start_angle_str.data, start_angle_str.length);
+		start_angle = float(start_angle_l)*0.01;
+
+		struct stData freq_str = strDatas[24 + LMDscandata_index - 1];
+		long freq_l = Str0x2Long(freq_str.data, freq_str.length);
+		freq = float(freq_l)*0.0001;
+
+		struct stData count_str = strDatas[25 + LMDscandata_index - 1];
+		long count = Str0x2Long(count_str.data, count_str.length);
+		if (strDatas.size() >= 26 + LMDscandata_index - 1 + count)
+		{
+			for (int i = 26 + LMDscandata_index - 1; i < 26 + LMDscandata_index - 1 + count; ++i)
+			{
+				float dis = float(Str0x2Long(strDatas[i].data, strDatas[i].length));
+				distance.push_back(dis);
+			}
+			return true;
+		}
+	}
+	return false;
+}
+long CUdpLaser::Str0x2Long(const char* data, int len)
+{
+	long sum = 0;
+	for (int i = 0; i < len; ++i)
+	{
+		char c = data[i];
+		int n = 0;
+		if (c >= 48 && c <= 57)
+			n = c - 48;
+		else if (c >= 65 && c <= 70)
+			n = c - 65 + 10;
+		sum += n*pow(16, len - i - 1);
+	}
+	return sum;
+}

laser/UdpLaser.h

@@ -0,0 +1,44 @@
+/*************************
+sick 511 雷达 upd协议连接控制板
+*************************/
+#pragma once
+#include "Laser.h"
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <mutex>
+class CUdpLaser : public CLaser
+{
+public:
+	CUdpLaser(int id, Automatic::stLaserCalibParam laser_param);
+	virtual ~CUdpLaser();
+	virtual bool Connect();
+	virtual void Disconnect();
+	virtual bool Start();
+	virtual bool Stop();
+
+protected:
+	virtual bool RecvData(CBinaryData& data);
+	virtual DATA_type Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points);
+	
+
+private:
+	bool Send(char* sendbuf, int len);                          // 发送指令
+	int Recv(char* recvbuf, int len);                           // 接收数据
+	static int FindHead(char* buf, int b_len);
+	static int FindTail(char* buf, int b_len);
+private:
+	bool GetData(CBinaryData* pData, std::vector<float>& distance,
+		float& freq, float& start_angle);
+	long Str0x2Long(const char* data, int len);
+
+protected:
+#pragma region udp相关变量
+	int				m_socket;
+//	WSADATA			m_wsd;
+	struct sockaddr_in m_send_addr;
+	std::mutex		m_mutex;
+#pragma endregion
+};
+

main.cpp

@@ -24,7 +24,6 @@ void InitGlog()
     PathCreator creator;
     creator.Mkdir(strdir);
 
-
     char logPath[255] = { 0 };
     sprintf(logPath, "%s/", strdir);
     FLAGS_max_log_size = 100;
@@ -53,12 +52,12 @@ CProcess*					m_pProcess=0;
 
 int main(int argc, char *argv[])
 {
+    std::cout<<"   ---------  Start  ----------"<<std::endl;
     QApplication a(argc, argv);
     MainWindow w;
     w.show();
 
     InitGlog();
-
     char buf[255]={0};
     getcwd(buf,255);
     sprintf(buf,"%s/setting/calib.prototxt",buf);
@@ -66,12 +65,13 @@ int main(int argc, char *argv[])
     {
         QMessageBox::about(NULL, "Error", "Read proto failed...");
     }
-    m_pProcess=new CProcess(m_laser_calib_param,&w);
+
+    m_pProcess=new CProcess(m_laser_calib_param,0);
     if(m_pProcess->Init()==false)
     {
         QMessageBox::about(NULL, "Error", m_pProcess->LastError().c_str());
     }
 
-    return 0;
+
     return a.exec();
 }

modbus/PLCMonitor.h

@@ -6,7 +6,7 @@
 #include <mutex>
 #include <vector>
 #include <time.h>
-#include <QObject>
+
 #include "qtmessagedef.h"
 
 struct whiskboom_laser_value

src/locate/Locater.h

@@ -1,114 +0,0 @@
-#ifndef __LOCATER__HH__
-#define __LOCATER__HH__
-#include <mutex>
-#include "common.h"
-#include "Locater3Dcnn.h"
-#include "PointSiftSeg.h"
-#include "YoloDetector.h"
-
-
-#ifndef PI
-#define PI 3.14159265
-#endif
-
-
-typedef struct PLANE
-{
-	float a;
-	float b;
-	float c;
-}Plane;
-
-enum ERROR_CODE
-{
-	eSucc=0
-	,eArea				//车位区域错误
-	,eLidar				//雷达点云不重合
-	,eCloud				//点云为空
-	,eNoCar				//未检测到车
-	,eMulCar			//多辆车
-	,eDistance			//车间距过小
-
-	,eLimitL=101
-	,eLimitT
-	,eLimitR
-	,eLimitB
-};
-
-typedef struct stCarPosition
-{
-	float x;
-	float y;
-	float a;
-	float l;
-	float w;
-	float h;
-	ERROR_CODE error_code;
-	cv::RotatedRect rrect;
-	int		pos_id;				//车位ID
-	stCarPosition()
-	{
-		error_code = eSucc;
-		pos_id = -1;
-	}
-}CarPosition;
-
-
-class  CLocater
-{
-public:
-	CLocater();
-	~CLocater();
-	bool Locate_test(PtrCloud cloud0, PtrCloud cloud1, std::vector<CarPosition>& results, std::string work_dir,
-		ERROR_CODE& sta,std::string dir_name);
-
-	bool Locate(PtrCloud cloud0, PtrCloud cloud1, std::vector<CarPosition>& results, std::string work_dir,ERROR_CODE& sta);
-	bool Locate(PtrCloud cloud_in, std::vector<CarPosition>& results, std::string work_dir, ERROR_CODE& sta);
-	
-protected:
-	//pointSIFT
-	bool Locate_SIFT(PtrCloud cloud_in, std::vector<bbox_t> boxes ,PtrCloud& cloud_targe, std::string work_dir);
-	///yolo
-	bool Locate_yolo(PtrCloud cloud_in, std::vector<bbox_t>& boxes, std::string work_dir);
-	
-	////
-	bool TestPointInbbox(PointT point, bbox_t box);
-	//////对点云根据yolo结果重新聚类
-	std::vector<PtrCloud> ReCluster(PtrCloud cloud_target, PtrCloud cloud_in, std::vector<bbox_t> boxes, std::string save_dir, std::string dir_name="");
-
-	PtrCloud FilterXY(PtrCloud cloud);
-	//6-25 找直线方法过滤后视镜点
-	bool bound_box(PtrCloud cloud, cv::RotatedRect& box);
-	//原始投影滤波方法
-	bool bound_box_median(PtrCloud cloud, cv::RotatedRect& box);
-	//验证在 z<200 以下的两片点云是否重合(各个区域高度差)
-	bool verify_cloud2(PtrCloud cloud1, PtrCloud cloud2);
-
-
-public:
-	void cut_RGB_cloud_By_yolo(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string save_path, std::string name_dir);
-
-private:
-	//切割找box
-	cv::RotatedRect getRotateRect1(PtrCloud cloud);
-	//投影滤波找box
-	cv::RotatedRect  getRotateRect0(PtrCloud cloud);
-	
-	cv::RotatedRect  GetMinRotateRect(PtrCloud cloud);
-	bool check_box(cv::RotatedRect& box,PtrCloud convex_hull);
-	PtrCloud Filter_obj(PtrCloud cloud);
-	bool is_need_cluser(PtrCloud cloud);
-	std::vector<PtrCloud> find_clusters(PtrCloud cloud);
-protected:
-	bool getRotatedRectFromImageXY(PtrCloud cloud, double dense, cv::RotatedRect& rect);
-	int getRotatedRectFromImageXY(pcl::PointCloud<PointT>::Ptr cloud, cv::RotatedRect &dstRect);
-protected:
-public:
-	int								m_nfunction;
-	CLocater3Dcnn*					m_p3dcnn_locater;
-	CPointSiftSeg*					m_pPointSiftSeg;
-	YoloDetector*					m_pYoloDetector;
-	std::mutex						m_mutex;
-};
-
-#endif

src/locate/Locater3Dcnn.h

@@ -1,33 +0,0 @@
-#pragma once
-#include "common.h"
-
-
-class  CLocater3Dcnn
-{
-public:
-	CLocater3Dcnn(int l,int w,int h,int freq,int nClass);
-	virtual ~CLocater3Dcnn();
-	virtual bool Init(std::string weights);
-	
-	virtual bool Predict(PtrCloud cloud, cv::RotatedRect& rect, std::string save_dir);
-
-protected:
-	float* generate_tensor(PtrCloud cloud, float min_x, float max_x,
-		float min_y, float max_y, float min_z, float max_z);
-	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> decodeCloud(pcl::PointCloud<PointT>& cloud,
-		float* data, float min_x, float max_x, float min_y, float max_y, float min_z, float max_z);
-protected:
-	//判断四点是否是矩形
-	bool isRect(std::vector<cv::Point2f> points);
-	std::vector<cv::Point2f> kmeans(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string cluster_file_path);
-	bool check_box(cv::RotatedRect& box, PtrCloud cloud);
-	////计算长轴延长后的 IOU 防止车门打开
-	bool check_IOU(cv::RotatedRect& box, PtrCloud cloud);
-protected:
-	int m_lenth;
-	int m_width;
-	int m_height;
-	int m_freq;
-	int m_nClass;
-};
-

src/locate/PointSiftSeg.h

@@ -1,25 +0,0 @@
-#pragma once
-#include "common.h"
-#include <string>
-#include "pointSIFT_API.h"
-class CPointSiftSeg : public PointSifter
-{
-public:
-	CPointSiftSeg(int point_size,int cls,float freq,PointT minp,PointT maxp);
-
-	virtual bool Init(std::string graph,std::string cpkt);
-	virtual bool Seg(PtrCloud cloud, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& clouds, std::string save_dir);
-
-	virtual ~CPointSiftSeg();
-protected:
-	bool	Create_data(PtrCloud cloud, float* output);
-	bool    RecoveryCloud(float* output, float* cloud, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg);
-	////  对目标类过滤， 过滤掉障碍物附近的目标点
-	bool	FilterObs(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg, std::string save_dir);
-protected:
-	float		m_freq;
-	PointT		m_minp;
-	PointT		m_maxp;
-
-};
-

src/locate/YoloDetector.h

@@ -1,82 +0,0 @@
-#pragma once
-#include "common.h"
-#define YOLO_API_USED	1
-#include "yolo_v2_class.hpp"
-
-
-
-class YoloDetector
-{
-public:
-	YoloDetector(std::string cfg,std::string weights,float minx,float maxx,float miny,float maxy,float freq);
-	virtual ~YoloDetector();
-
-	bool detect(cv::Mat& l, cv::Mat& r, std::vector<bbox_t>& boxes,std::string save_dir);
-	bool detect(PtrCloud cloud_in, std::vector<bbox_t>& boxes, std::string save_dir);
-protected:
-    void free_img(image_t img);
-    std::shared_ptr<image_t> mat_to_image_resize(cv::Mat mat) const
-    {
-        if (mat.data == NULL) return std::shared_ptr<image_t>(NULL);
-        cv::Mat det_mat;
-        cv::resize(mat, det_mat, cv::Size(m_pDetector->get_net_width(), m_pDetector->get_net_height()));
-        return mat_to_image(det_mat);
-    }
-
-    static std::shared_ptr<image_t> mat_to_image(cv::Mat img_src)
-    {
-        cv::Mat img;
-        cv::cvtColor(img_src, img, cv::COLOR_RGB2BGR);
-        std::shared_ptr<image_t> image_ptr(new image_t, [](image_t *img) { if(img->data) free(img->data); delete img; });
-        std::shared_ptr<IplImage> ipl_small = std::make_shared<IplImage>(img);
-        *image_ptr = ipl_to_image(ipl_small.get());
-        return image_ptr;
-    }
-    static image_t ipl_to_image(IplImage* src)
-    {
-        unsigned char *data = (unsigned char *)src->imageData;
-        int h = src->height;
-        int w = src->width;
-        int c = src->nChannels;
-        int step = src->widthStep;
-        image_t out = make_image_custom(w, h, c);
-        int count = 0;
-
-        for (int k = 0; k < c; ++k) {
-            for (int i = 0; i < h; ++i) {
-                int i_step = i*step;
-                for (int j = 0; j < w; ++j) {
-                    out.data[count++] = data[i_step + j*c + k] / 255.;
-                }
-            }
-        }
-
-        return out;
-    }
-
-    static image_t make_empty_image(int w, int h, int c)
-    {
-        image_t out;
-        out.data = 0;
-        out.h = h;
-        out.w = w;
-        out.c = c;
-        return out;
-    }
-
-    static image_t make_image_custom(int w, int h, int c)
-    {
-        image_t out = make_empty_image(w, h, c);
-        out.data = (float *)calloc(h*w*c, sizeof(float));
-        return out;
-    }
-
-protected:
-	Detector*	m_pDetector;
-	float m_minx;
-	float m_maxx;
-	float m_miny;
-	float m_maxy;
-	float m_freq;
-};
-

src/locate/pointSIFT_API.h

@@ -1,22 +0,0 @@
-#pragma once
-#include <string>
-#include <mutex>
-
-class PointSifter
-{
-public:
-	PointSifter(int point_num, int cls_num);
-	~PointSifter();
-	bool Load(std::string meta, std::string cpkt);
-	bool Predict(float* data, float* output);
-	std::string LastError();
-private:
-	PointSifter();
-protected:
-	std::mutex		m_mutex;
-	std::string		m_error;
-	bool			m_bInit;
-	int				m_point_num;
-	int				m_cls_num;
-	void*			m_sess;
-};

src/measuretask.cpp

@@ -70,11 +70,10 @@ double distance_polys(std::vector<cv::Point2f> poly1, std::vector<cv::Point2f> p
 
 std::mutex* MeasureTask::g_mutex = new std::mutex();
 
-MeasureTask::MeasureTask(std::vector<CLaser*> lasers, void* locater,int positon_id,
+MeasureTask::MeasureTask(std::vector<CLaser*> lasers,int positon_id,
     modbus::CPLCMonitor* plc,Automatic::stCalibParam param)
 {
     m_lasers = lasers;
-    m_locater = locater;
     m_plc = plc;
     m_exit = false;
     m_calib_param = param;
@@ -300,6 +299,31 @@ void MeasureTask::SetResultCallback(void *ptr, ResultCallback func)
     m_ptr=ptr;
 }
 
+
+#include <nnxx/message.h>
+#include <nnxx/message_control.h>
+#include <nnxx/socket.h>
+#include <nnxx/reqrep.h>
+#include <nnxx/unittest.h>
+#include <nnxx/timeout.h>
+#include <nnxx/error.h>
+#include <string>
+#include <iostream>
+#include <string.h>
+
+bool msg2result(nnxx::message& msg,std::vector<CarPosition>& positions)
+{
+    int size=sizeof(CarPosition);
+    if(msg.size()%size!=0)
+        return false;
+
+    int num=msg.size()/size;
+    if(num<=0) return false;
+    positions.resize(num);
+    memcpy(positions.data(),msg.data(),msg.size());
+    return true;
+}
+
 void  MeasureTask::Main()
 {
     ////等待雷达空闲资源
@@ -328,17 +352,26 @@ void  MeasureTask::Main()
 
     while (!m_exit && !IsLaserReady()) { usleep(1000); }	//等待摆扫结束
 
+
     std::vector<CarPosition> results;
-    ERROR_CODE code = eSucc;
-    if (m_locater)
+    ERROR_CODE code = eCloud;
+    nnxx::socket client { nnxx::SP, nnxx::REQ };
+    client.connect("ipc:///tmp/reqrep.ipc");
+
+    if (client.send((void*)(m_cloud.points.data()),sizeof(PointT)*m_cloud.size()) ==14)
     {
-        //m_locater->Locate(m_cloud.makeShared(), results, strPath.GetBuffer(), code)
-        if (m_cloud.size() == 0)
-        {
-            code = eCloud;
-        }
-        else if (0)
-        {
+        nnxx::message ret;
+        try { nnxx::with_recv_timeout _ { client, std::chrono::seconds(3) };
+            ret=client.recv();
+          }
+          catch (const nnxx::timeout_error &) {
+          }
+          catch (const std::exception &) {
+
+          }
+
+       if(msg2result(ret,results))
+       {
             code = Dispatch_posID(results);
             if (code == eSucc)
             {
@@ -367,7 +400,7 @@ void  MeasureTask::Main()
         }
         else
         {
-            LOG(ERROR) << "\t测量失败 : " << Error_string(code);
+            LOG(ERROR) << "\tmsg 2 results failed : ";
         }
     }
     ///写入测量完成信号

src/measuretask.h

@@ -5,13 +5,43 @@
 #include "TaskQueue/BaseTask.h"
 #include "laser/Laser.h"
 #include "modbus/PLCMonitor.h"
-#include "locate/Locater.h"
 
 #ifndef PI
 #define PI 3.14159265
 #endif
 
-
+enum ERROR_CODE
+{
+    eSucc=0
+    ,eArea				//车位区域错误
+    ,eLidar				//雷达点云不重合
+    ,eCloud				//点云为空
+    ,eNoCar				//未检测到车
+    ,eMulCar			//多辆车
+    ,eDistance			//车间距过小
+
+    ,eLimitL=101
+    ,eLimitT
+    ,eLimitR
+    ,eLimitB
+};
+typedef struct stCarPosition
+{
+    float x;
+    float y;
+    float a;
+    float l;
+    float w;
+    float h;
+    ERROR_CODE error_code;
+    cv::RotatedRect rrect;
+    int		pos_id;				//车位ID
+    stCarPosition()
+    {
+        error_code = eSucc;
+        pos_id = -1;
+    }
+}CarPosition;
 
 std::string Error_string(ERROR_CODE code);
 bool RegionInRegion(std::vector<cv::Point2f>& mini_poly, std::vector<cv::Point2f>& large_poly);
@@ -22,7 +52,7 @@ typedef void(*ResultCallback)(QtMessageData msg, void*);
 class MeasureTask : public tq::BaseTask
 {
 public:
-    MeasureTask(std::vector<CLaser*> lasers,void* locater,int position_id,
+    MeasureTask(std::vector<CLaser*> lasers,int position_id,
         modbus::CPLCMonitor* pcl, Automatic::stCalibParam	param);
     virtual void   Main();
     virtual void   Cancel();
@@ -38,7 +68,7 @@ protected:
 
 protected:
     std::vector<CLaser*>		m_lasers;
-    void*					m_locater;
+
     modbus::CPLCMonitor*		m_plc;
 
     int							m_position_id;		//³µÎ»id