position(car) measure Qt

GarageMeasurement.pro

@@ -0,0 +1,122 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2019-10-17T11:19:58
+#
+#-------------------------------------------------
+
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+TARGET = GarageMeasurement
+TEMPLATE = app
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+# add pcl
+INCLUDEPATH += /usr/include/eigen3
+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/include/pcl-1.7
+LIBS += /usr/lib/x86_64-linux-gnu/libpcl_*.so
+
+# add opencv 34
+INCLUDEPATH += /usr/local/include
+LIBS += /usr/local/lib/libopencv_*.so
+
+#livox sdk  apr lib
+LIBS += /usr/local/lib/liblivox_sdk_static.a
+LIBS += /usr/local/apr/lib/libapr-1.a
+
+# glog protobuf gflags
+LIBS += /usr/local/lib/libglog.a
+LIBS += /usr/local/lib/libgflags.a
+LIBS += /usr/local/lib/libprotobuf.a
+
+
+# modubus
+INCLUDEPATH += /usr/local/include/modbus
+LIBS += /usr/local/lib/libmodbus.so
+
+
+SOURCES += \
+    laser/Laser.cpp \
+    laser/LivoxLaser.cpp \
+    laser/LivoxMid100Laser.cpp \
+    laser/LogFiles.cpp \
+    laser/Point2D.cpp \
+    laser/Point3D.cpp \
+    src/CalibParam.pb.cc \
+    src/StdCondition.cpp \
+    main.cpp \
+    mainwindow.cpp \
+    modbus/LibmodbusWrapper.cpp \
+    modbus/PLCMonitor.cpp \
+    modbus/Runnable.cpp \
+    TaskQueue/BaseTask.cpp \
+    TaskQueue/ThreadTaskQueue.cpp \
+    TaskQueue/TQFactory.cpp \
+    src/measuretask.cpp \
+    Process.cpp \
+    src/pathcreator.cpp \
+    laser/LivoxHubLaser.cpp
+
+HEADERS += \
+    laser/Laser.h \
+    laser/LivoxLaser.h \
+    laser/LivoxMid100Laser.h \
+    laser/LogFiles.h \
+    laser/Point2D.h \
+    laser/Point3D.h \
+    src/StdCondition.h \
+    mainwindow.h \
+    laser/Laser.h \
+    laser/LivoxLaser.h \
+    laser/LivoxMid100Laser.h \
+    laser/LogFiles.h \
+    laser/Point2D.h \
+    laser/Point3D.h \
+    src/CalibParam.pb.h \
+    src/StdCondition.h \
+    mainwindow.h \
+    modbus/LibmodbusWrapper.h \
+    modbus/PLCMonitor.h \
+    modbus/Runnable.h \
+    TaskQueue/threadpp/impl/pthread_lock.h \
+    TaskQueue/threadpp/impl/pthread_lock.hpp \
+    TaskQueue/threadpp/impl/pthread_thread.h \
+    TaskQueue/threadpp/impl/pthread_thread.hpp \
+    TaskQueue/threadpp/impl/std_lock.h \
+    TaskQueue/threadpp/impl/std_lock.hpp \
+    TaskQueue/threadpp/impl/std_thread.h \
+    TaskQueue/threadpp/impl/std_thread.hpp \
+    TaskQueue/threadpp/impl/win_lock.h \
+    TaskQueue/threadpp/impl/win_lock.hpp \
+    TaskQueue/threadpp/impl/win_thread.h \
+    TaskQueue/threadpp/impl/win_thread.hpp \
+    TaskQueue/threadpp/recursive_lock.h \
+    TaskQueue/threadpp/threadpp.h \
+    TaskQueue/threadpp/threadpp_assert.h \
+    TaskQueue/BaseTask.h \
+    TaskQueue/TaskPool.h \
+    TaskQueue/ThreadTaskQueue.h \
+    TaskQueue/TQFactory.h \
+    TaskQueue/TQInterface.h \
+    src/measuretask.h \
+    Process.h \
+    src/pathcreator.h \
+    laser/LivoxHubLaser.h
+
+FORMS += \
+        mainwindow.ui

GarageMeasurement.pro.user

@@ -0,0 +1,338 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE QtCreatorProject>
+<!-- Written by QtCreator 4.4.1, 2019-10-22T22:46:28. -->
+<qtcreator>
+ <data>
+  <variable>EnvironmentId</variable>
+  <value type="QByteArray">{572766cd-d6c0-4a3f-8c4c-978d47719327}</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.ActiveTarget</variable>
+  <value type="int">0</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.EditorSettings</variable>
+  <valuemap type="QVariantMap">
+   <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
+   <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
+   <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
+   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
+    <value type="QString" key="language">Cpp</value>
+    <valuemap type="QVariantMap" key="value">
+     <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
+    </valuemap>
+   </valuemap>
+   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
+    <value type="QString" key="language">QmlJS</value>
+    <valuemap type="QVariantMap" key="value">
+     <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
+    </valuemap>
+   </valuemap>
+   <value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
+   <value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
+   <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
+   <value type="int" key="EditorConfiguration.IndentSize">4</value>
+   <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
+   <value type="int" key="EditorConfiguration.MarginColumn">80</value>
+   <value type="bool" key="EditorConfiguration.MouseHiding">true</value>
+   <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
+   <value type="int" key="EditorConfiguration.PaddingMode">1</value>
+   <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
+   <value type="bool" key="EditorConfiguration.ShowMargin">false</value>
+   <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
+   <value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
+   <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
+   <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
+   <value type="int" key="EditorConfiguration.TabSize">8</value>
+   <value type="bool" key="EditorConfiguration.UseGlobal">true</value>
+   <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
+   <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
+   <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
+   <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
+   <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.PluginSettings</variable>
+  <valuemap type="QVariantMap">
+   <valuelist type="QVariantList" key="ClangStaticAnalyzer.SuppressedDiagnostics"/>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.Target.0</variable>
+  <valuemap type="QVariantMap">
+   <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.ActiveDeployConfiguration">0</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/zx/zzw/GarageMeasurement/build-GarageMeasurement-Desktop_Qt_5_9_2_GCC_64bit-Debug</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
+      <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
+     </valuemap>
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+    </valuemap>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+    <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+    <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Debug</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
+    <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">2</value>
+    <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
+   </valuemap>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/zx/zzw/GarageMeasurement/build-GarageMeasurement-Desktop_Qt_5_9_2_GCC_64bit-Release</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
+      <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
+     </valuemap>
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+    </valuemap>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">-j8</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+    <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+    <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
+    <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
+    <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
+   </valuemap>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.2">
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/zx/zzw/build-GarageMeasurement-Desktop_Qt_5_9_2_GCC_64bit-Profile</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
+      <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">true</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
+     </valuemap>
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+    </valuemap>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+    <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+    <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Profile</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
+    <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
+    <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
+   </valuemap>
+   <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">3</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
+   </valuemap>
+   <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
+    <value type="bool" key="Analyzer.QmlProfiler.AggregateTraces">false</value>
+    <value type="bool" key="Analyzer.QmlProfiler.FlushEnabled">false</value>
+    <value type="uint" key="Analyzer.QmlProfiler.FlushInterval">1000</value>
+    <value type="QString" key="Analyzer.QmlProfiler.LastTraceFile"></value>
+    <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
+    <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
+    <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
+    <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
+    <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
+    <value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
+    <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
+    <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
+    <value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
+    <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+    <value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
+    <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
+    <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
+    <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
+     <value type="int">0</value>
+     <value type="int">1</value>
+     <value type="int">2</value>
+     <value type="int">3</value>
+     <value type="int">4</value>
+     <value type="int">5</value>
+     <value type="int">6</value>
+     <value type="int">7</value>
+     <value type="int">8</value>
+     <value type="int">9</value>
+     <value type="int">10</value>
+     <value type="int">11</value>
+     <value type="int">12</value>
+     <value type="int">13</value>
+     <value type="int">14</value>
+    </valuelist>
+    <value type="int" key="PE.EnvironmentAspect.Base">2</value>
+    <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">GarageMeasurement</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">GarageMeasurement2</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/zx/zzw/GarageMeasurement/GarageMeasurement/GarageMeasurement.pro</value>
+    <value type="bool" key="QmakeProjectManager.QmakeRunConfiguration.UseLibrarySearchPath">true</value>
+    <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
+    <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="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
+    <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
+    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
+    <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
+    <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
+    <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
+   </valuemap>
+   <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.TargetCount</variable>
+  <value type="int">1</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
+  <value type="int">18</value>
+ </data>
+ <data>
+  <variable>Version</variable>
+  <value type="int">18</value>
+ </data>
+</qtcreator>

Process.cpp

@@ -0,0 +1,257 @@
+
+#include "Process.h"
+#include "qmessagebox.h"
+#include "TaskQueue/TQFactory.h"
+#include "mainwindow.h"
+
+CProcess::CProcess(Automatic::stCalibParam param, void* mainWnd)
+{
+	m_laser_calib_param = param;
+	m_laser = NULL;
+	m_main_wnd = mainWnd;
+    m_thread_queue=0;
+    connect(this,SIGNAL(plc_signal(uint16_t*,int)),
+            (MainWindow*)m_main_wnd,SLOT(PLCSlot(uint16_t*,int)));
+}
+
+
+CProcess::~CProcess()
+{
+	const int n = m_laser_calib_param.laser_size();
+	for (int i = 0; i < n; ++i)
+	{
+		std::string type = m_laser_calib_param.laser(i).type();
+		if (type.find("Livox") != type.npos)
+		{
+			Uninit();
+			break;
+		}
+	}
+
+    if(m_thread_queue)
+    {
+        m_thread_queue->Stop();
+        delete m_thread_queue;
+        m_thread_queue=0;
+    }
+	if (m_laser)
+	{
+		for (int i = 0; i < n; ++i)
+		{
+			if (m_laser[i])
+			{
+				m_laser[i]->Disconnect();
+				delete m_laser[i];
+				m_laser[i] = NULL;
+			}
+
+		}
+		delete[] m_laser;
+		m_laser = NULL;
+	}
+	if (m_laser_calib_param.is_calib() == false)
+	{
+
+	}
+
+	
+}
+
+
+bool CProcess::Init()
+{
+    m_thread_queue=tq::TQFactory::CreateDefaultQueue();
+    m_thread_queue->Start(3);
+
+	if (NULL == m_laser)
+	{
+		const int n = m_laser_calib_param.laser_size();
+		m_laser = new CLaser*[n];
+
+		for (int i = 0; i < n; ++i)
+		{
+			std::string type = m_laser_calib_param.laser(i).type();
+			////判断是否是从文件来
+			if (type.find("File") != type.npos)
+			{
+				LOG(INFO) << "create fileLaser";
+				std::string dir = m_laser_calib_param.debug_file();
+				m_laser_calib_param.mutable_laser(i)->set_laser_ip(dir);
+
+			}
+			//创建laser
+			m_laser[i] = LaserRegistory::CreateLaser(type, i, m_laser_calib_param.laser(i));
+			if (m_laser[i] == NULL)
+			{
+				char buf[255] = { 0 };
+				sprintf(buf, " Laser type : %s  is not exits...", type.c_str());
+                m_last_error=buf;
+				return false;
+			}
+			
+			if (!m_laser[i]->Connect())
+			{
+				char error[255] = { 0 };
+				sprintf(error, "laser %d connect failed...", i);
+				m_last_error = error;
+				return false;
+			}
+			double mat[12] = { 0 };
+			if (m_laser_calib_param.is_calib()==false)
+			{
+				create_mat_param(m_laser_calib_param.laser(i), mat);
+				m_laser[i]->SetMetrix(mat);
+			}
+		}
+
+		for (int i = 0; i < n; ++i)
+		{
+			std::string type = m_laser_calib_param.laser(i).type();
+			if (type.find("Livox") != type.npos)
+			{
+				if (Start() == false)
+				{
+					Uninit();
+                    m_last_error=("Livox laser init failed...");
+					return false;
+				}
+				break;
+			}
+		}
+
+	}
+
+
+	// 连接到PLC,创建locate
+	if (m_laser_calib_param.is_calib() == false)
+	{
+		if (0 != m_monitor.connect(m_laser_calib_param.plc().plc_ip().c_str(), m_laser_calib_param.plc().plc_port(), 1))
+		{
+            m_last_error=(" connect PLC failed...");
+			return false;
+		}
+		else
+		{
+            m_monitor.set_callback(action_callback, monitor_callback,this);
+            /*m_monitor.SetLogWnd(((CMainFrame*)m_main_wnd)->m_wndSplitter.GetPane(0, 1),
+                &((CMainFrame*)m_main_wnd)->m_dlg_laser_data);*/
+		}
+
+        /*if (m_locater == 0)
+		{
+			m_locater = new CLocater();
+        }*/
+	}
+
+	return true;
+}
+
+void CProcess::PushTask(uint16_t param, int action_type,bool test)
+{
+	char plate = 0;
+	uint16_t laserID = param;
+	memcpy(&plate, &param, 1);
+	m_plate_index = plate;
+	std::vector<CLaser*> lasers;
+	for (int i = 0; i < 8; ++i)
+	{
+		if (((0x01 << i)&laserID) > 0)
+		{
+			const int n = m_laser_calib_param.laser_size();
+			if (i >= 0 && i < n)
+				lasers.push_back(m_laser[i]);
+		}
+	}
+
+	//创建摆扫测量任务
+
+	modbus::CPLCMonitor* plc = 0;
+	if (test == false)
+		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);
+    /*else if(action_type==2)
+        task = new CFenceTask(lasers, m_locater, int(plate), plc, m_laser_calib_param);*/
+
+    //task->SetResultCallback(this, result_callback);
+    m_thread_queue->AddTask(task);
+}
+
+void CProcess::action_callback(bool bOn, uint16_t param, void* pointer, int action_type)
+{
+
+	CProcess* process = reinterpret_cast<CProcess*>(pointer);
+	if(bOn)
+		process->PushTask(param, action_type);
+}
+
+void CProcess::monitor_callback(uint16_t* data,int size, void* pointer)
+{
+
+    CProcess* process = reinterpret_cast<CProcess*>(pointer);
+    emit process->plc_signal(data,size);
+}
+
+void CProcess::create_mat_param(Automatic::stLaserCalibParam laser, double* buf)
+{
+	std::string type = laser.type();
+	if (type.find("Linear") != type.npos)
+	{
+		double x_angle = laser.axis_x_theta() / 180.0*PI;
+		double y_angle = laser.axis_y_theta() / 180.0*PI;
+		double z_angle = laser.axis_z_theta() / 180.0*PI;
+		double t_x = laser.translation_x();
+		double t_y = laser.translation_y();
+		double t_z = laser.translation_z();
+		Eigen::AngleAxisd rollAngle(x_angle, Eigen::Matrix<double, 3, 1>::UnitX());
+		Eigen::AngleAxisd pitchAngle(y_angle, Eigen::Matrix<double, 3, 1>::UnitY());
+		Eigen::AngleAxisd yawAngle(z_angle, Eigen::Matrix<double, 3, 1>::UnitZ());
+		Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
+		Eigen::Matrix<double, 3, 3> rotationMatrix = q.toRotationMatrix();
+
+		Eigen::Matrix<double, 3, 4> transform;
+		Eigen::Matrix<double, 3, 1> translation;
+		translation << t_x, t_y, t_z;
+		transform << rotationMatrix, translation;
+
+		buf[0] = transform(0, 0);
+		buf[1] = transform(0, 1);
+		buf[2] = transform(0, 2);
+		buf[3] = transform(0, 3);
+		buf[4] = transform(1, 0);
+		buf[5] = transform(1, 1);
+		buf[6] = transform(1, 2);
+		buf[7] = transform(1, 3);
+		buf[8] = transform(2, 0);
+		buf[9] = transform(2, 1);
+		buf[10] = transform(2, 2);
+		buf[11] = transform(2, 3);
+	}
+	else
+	{
+		buf[0] = laser.mat_r00();
+		buf[1] = laser.mat_r01();
+		buf[2] = laser.mat_r02();
+		buf[3] = laser.mat_r03();
+		buf[4] = laser.mat_r10();
+		buf[5] = laser.mat_r11();
+		buf[6] = laser.mat_r12();
+		buf[7] = laser.mat_r13();
+		buf[8] = laser.mat_r20();
+		buf[9] = laser.mat_r21();
+		buf[10] = laser.mat_r22();
+		buf[11] = laser.mat_r23();
+	}
+}
+
+/*#include "MsgDef.h"
+void CProcess::result_callback(Locate_Message msg, void* ptr)
+{
+	
+	CProcess* process = reinterpret_cast<CProcess*>(ptr);
+	std::lock_guard<std::mutex> lk(process->m_mutex);
+	process->m_locate_msg = msg;
+	if(process->m_main_wnd)
+		::PostMessage(process->m_main_wnd->m_hWnd, WM_MSG_MainWnd, WPARAM(WM_Draw_cloud), 0);
+}*/

Process.h

@@ -0,0 +1,48 @@
+#pragma once
+#include <QObject>
+#include "common.h"
+#include "./modbus/PLCMonitor.h"
+#include "./src/CalibParam.pb.h"
+#include "laser/LivoxLaser.h"
+#include "src/measuretask.h"
+
+
+class CProcess :public QObject
+{
+    Q_OBJECT
+public:
+    CProcess(Automatic::stCalibParam param,void* mainWnd=NULL);
+	bool			Init();
+	std::string		LastError() { return m_last_error; }
+	void			PushTask(uint16_t param , int action_type,bool test=false);
+    size_t			TaskQueueSize() { return 0;}//m_thread_queue.QueueSize(); }
+    size_t			TaskProcessSize() { return 0;}//m_thread_queue.ProcessSize(); }
+    virtual ~CProcess();
+protected:
+    static void action_callback(bool bOn, uint16_t param, void* pointer, int action_type);
+    static void monitor_callback(uint16_t* data, int size,void* pointer);
+	static void create_mat_param(Automatic::stLaserCalibParam laser, double*);
+
+protected:
+signals:
+    void plc_signal(uint16_t* data,int size);
+
+public:
+
+	CLaser**  m_laser;
+	modbus::CPLCMonitor							m_monitor;
+protected:
+    tq::IQueue*                                 m_thread_queue;
+	// 激光标定参数和分割算法参数
+	Automatic::stCalibParam		m_laser_calib_param;
+	std::string					m_last_error;
+
+public:
+	////获取最近结果
+	mutable std::mutex							m_mutex;
+    void*										m_main_wnd;
+	int											m_plate_index;
+
+};
+
+std::string Error_string(ERROR_CODE code);

TaskQueue/BaseTask.cpp

@@ -0,0 +1,33 @@
+//
+//  BaseTaskQueue.cpp
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#include "BaseTask.h"
+
+namespace tq {
+    BaseTask::BaseTask():ITask(),_cancelled(false)
+    {
+    }
+    
+    void BaseTask::Cancel()
+    {
+        _cancelled = true;
+    }
+    
+    bool BaseTask::IsCancelled() const
+    {
+        return _cancelled;
+    }
+    
+    void BaseTask::Run()
+    {
+        if (_cancelled) {
+            return;
+        }
+        Main();
+    }
+}

TaskQueue/BaseTask.h

@@ -0,0 +1,29 @@
+//
+//  BaseTaskQueue.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__BaseTaskQueue__
+#define __LibDriveRating_CXX__BaseTaskQueue__
+
+#include "TQInterface.h"
+namespace tq
+{
+    class BaseTask : public ITask
+    {
+    private:
+        volatile bool _cancelled;
+    public:
+        BaseTask();
+        void Run();
+        void Cancel();
+        bool IsCancelled() const;
+        
+        virtual void Main() = 0;
+        virtual TaskCategory GetCategory() const {return NoCategory;}
+    };
+}
+#endif /* defined(__LibDriveRating_CXX__BaseTaskQueue__) */

TaskQueue/TQFactory.cpp

@@ -0,0 +1,23 @@
+//
+//  TQFactory.cpp
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/10/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#include "TQFactory.h"
+#include "ThreadTaskQueue.h"
+namespace tq {
+    IQueue* TQFactory::CreateDefaultQueue()
+    {
+        return new ThreadTaskQueue();
+    }
+    
+    void TQFactory::ReleaseQueue(IQueue* queue)
+    {
+        delete queue;
+    }
+    
+    
+}

TaskQueue/TQFactory.h

@@ -0,0 +1,24 @@
+//
+//  TQFactory.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/10/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__TQFactory__
+#define __LibDriveRating_CXX__TQFactory__
+
+#include "TQInterface.h"
+
+namespace tq {
+    class TQFactory
+    {
+    public:
+        static IQueue* CreateDefaultQueue();
+
+        static void ReleaseQueue(IQueue* queue);
+    };
+}
+
+#endif /* defined(__LibDriveRating_CXX__TQFactory__) */

TaskQueue/TQInterface.h

@@ -0,0 +1,58 @@
+//
+//  TQInterface.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__TQInterface__
+#define __LibDriveRating_CXX__TQInterface__
+
+namespace tq {
+    typedef unsigned long TaskCategory;
+
+    const TaskCategory NoCategory = 0;
+
+    class ITask
+    {
+    public:
+        virtual void Run() = 0;
+        virtual void Cancel() = 0;
+        virtual bool IsCancelled() const = 0;
+        virtual TaskCategory GetCategory() const = 0;
+        virtual ~ITask(){}
+    };
+    
+    typedef void (*TaskRecycler)(ITask* task,void* context);
+
+    class IQueue
+    {
+    public:
+        virtual void Start(unsigned int nThreads = 1) = 0;
+
+        virtual void Stop() = 0;
+
+        virtual void AddTask(ITask* task) = 0;
+        
+        virtual void GetTasks(ITask** tasksBuf, unsigned int taskBufSize) const= 0;
+        
+        virtual unsigned int TaskCount() const = 0;
+        
+        virtual void CancelAll() = 0;
+        
+        virtual void WaitForFinish() = 0;
+
+        virtual void Suspend() = 0;
+        
+        virtual void Resume() = 0;
+        
+        virtual void SetTaskRecycler(TaskCategory cat, TaskRecycler recycler,void *context){}
+
+        virtual void ClearTaskRecycler(TaskCategory cat){}
+
+        virtual ~IQueue() {}
+    };
+}
+
+#endif /* defined(__LibDriveRating_CXX__TQInterface__) */

TaskQueue/TaskPool.h

@@ -0,0 +1,89 @@
+#ifndef H_TASK_POOL_H
+#define H_TASK_POOL_H
+#include <list>
+#include "threadpp/threadpp.h"
+#include <cstdlib>
+#include <cstdio>
+namespace tq
+{
+    template <typename TaskType>
+    class TaskPool
+    {
+    public:
+        TaskPool(unsigned capacity = 10);
+        ~TaskPool();
+        TaskType* GetTask(TaskType const& taskPrototype);
+        void RecycleTask(TaskType* task);
+        void Purge();
+    private:
+        threadpp::lock _mutex;
+        std::list<TaskType*> _tasks;
+        unsigned _capacity;
+    };
+
+    template <typename TaskType>
+    TaskType* TaskPool<TaskType>::GetTask(TaskType const& taskPrototype)
+    {
+        _mutex.lock();
+        if(!_tasks.empty())
+        {
+
+            TaskType* taskptr = _tasks.front();
+            _tasks.pop_front();
+            _mutex.unlock();
+            new (taskptr)TaskType(taskPrototype);
+            return taskptr;
+        }
+        else
+        {
+            _mutex.unlock();
+            static int newcount = 0;
+            TaskType* taskptr = new TaskType(taskPrototype);
+            return taskptr;
+        }
+    }
+
+    template <typename TaskType>
+    void TaskPool<TaskType>::RecycleTask(TaskType* task)
+    {
+        (*task).~TaskType();
+        _mutex.lock();
+        if(_tasks.size()<_capacity)
+        {
+            _tasks.push_back(task);
+        }
+        else
+        {
+            free(task);
+        }
+        _mutex.unlock();
+    }
+    
+    template <typename TaskType>
+    void TaskPool<TaskType>::Purge()
+    {
+        _mutex.lock();
+        if(!_tasks.empty())
+        {
+            for(typename std::list<TaskType*>::const_iterator it = _tasks.begin();it!=_tasks.end();++it)
+            {
+                free(*it);
+            }
+            _tasks.clear();
+        }
+        _mutex.unlock();
+    }
+
+    template <typename TaskType>
+    TaskPool<TaskType>::TaskPool(unsigned capacity):
+    _capacity(capacity)
+    {
+    }
+
+    template <typename TaskType>
+    TaskPool<TaskType>::~TaskPool()
+    {
+        this->Purge();
+    }
+}
+#endif

TaskQueue/ThreadTaskQueue.cpp

@@ -0,0 +1,286 @@
+//
+//  ThreadTaskQueue.cpp
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#include "ThreadTaskQueue.h"
+#include <algorithm>
+#define WAIT_TIMEOUT 5000
+
+using namespace threadpp;
+
+namespace tq{
+    
+    class QueueRunnable
+    {
+        ThreadTaskQueue* queue;
+        ITask* currentTask;
+    protected:
+        static void run_callback(void*);
+        thread* th;
+        
+        QueueRunnable(ThreadTaskQueue* q):queue(q),currentTask(NULL){}
+        void run();
+        void CancelCurrentTask();
+        bool TaskIsRunning() const;
+        friend class ThreadTaskQueue;
+    };
+    
+    void QueueRunnable::run_callback(void *ctx)
+    {
+        ((QueueRunnable*) ctx)->run();
+    }
+    
+    void QueueRunnable::run()
+    {
+        while (queue->IsStarted()) {
+            queue->LockQueue();
+            ITask* task = queue->NextTask();
+            if (task == NULL) {
+                queue->UnlockQueue();
+                continue;
+            }
+            currentTask = task;
+            queue->UnlockQueue();
+            task->Run();
+            queue->LockQueue();
+            currentTask = NULL;
+            queue->FinishTask(task);
+            queue->NotifyQueue();
+            queue->UnlockQueue();
+        }
+    }
+    
+    void QueueRunnable::CancelCurrentTask()
+    {
+        queue->LockQueue();
+        if(currentTask)
+        {
+            currentTask->Cancel();
+        }
+        queue->UnlockQueue();
+    }
+    
+    bool QueueRunnable::TaskIsRunning() const
+    {
+        return currentTask != NULL;
+    }
+    
+    ThreadTaskQueue::ThreadTaskQueue():_tasklist(),_started(false),_suspended(false)
+    {
+        
+    }
+    
+    void ThreadTaskQueue::Start(unsigned int nThreads)
+    {
+        _mutex.lock();
+        if (_started) {
+            _mutex.unlock();
+            return;
+        }
+        _started = true;
+        _threads.reserve(nThreads);
+        for (int i = 0; i<nThreads; ++i) {
+            QueueRunnable* runnable = new QueueRunnable(this);
+            runnable->th = new thread(QueueRunnable::run_callback, runnable);
+            _threads.push_back(runnable);
+        }
+        _mutex.unlock();
+    }
+    
+    void ThreadTaskQueue::Stop()
+    {
+        _mutex.lock();
+        if (!_started) {
+            _mutex.unlock();
+            return;
+        }
+        _started = false;
+        for (std::list<ITask*>::iterator it = _tasklist.begin(); it!= _tasklist.end(); ++it) {
+            delete *it;
+        }
+        _tasklist.clear();
+        _mutex.notify_all();
+        std::vector<QueueRunnable*> copy(_threads);
+        _threads.clear();
+        _mutex.unlock();
+        for (std::vector<QueueRunnable*>::iterator it = copy.begin(); it!=copy.end(); ++it) {
+            (*it)->th->join();
+            thread* t = (*it)->th;
+            delete (*it);
+            delete t;
+        }
+    }
+    
+    bool ThreadTaskQueue::IsStarted() const
+    {
+        return _started;
+    }
+    
+    void ThreadTaskQueue::AddTask(ITask* task)
+    {
+        _mutex.lock();
+        if (_started) {
+            _tasklist.push_back(task);
+            _mutex.notify_all();
+        }
+        _mutex.unlock();
+    }
+    
+    void ThreadTaskQueue::GetTasks(ITask** tasksBuf, unsigned int taskBufSize) const
+    {
+        recursivelock* mutex = const_cast<recursivelock*>(&_mutex);
+        mutex->lock();
+        size_t count = 0;
+        for (std::list<ITask*>::const_iterator it = _tasklist.begin(); it!=_tasklist.end(); ++it) {
+            if (count<taskBufSize) {
+                tasksBuf[count] = *it;
+                count++;
+            }
+            else
+            {
+                break;
+            }
+        }
+        mutex->unlock();
+    }
+    
+    unsigned int ThreadTaskQueue::TaskCount() const
+    {
+        recursivelock* mutex = const_cast<recursivelock*>(&_mutex);
+        mutex->lock();
+        unsigned int count = (unsigned int)_tasklist.size();
+        mutex->unlock();
+        return count;
+    }
+    
+    void ThreadTaskQueue::CancelAll()
+    {
+        _mutex.lock();
+        for (std::vector<QueueRunnable*>::iterator it = _threads.begin(); it!=_threads.end(); ++it) {
+            (*it)->CancelCurrentTask();
+        }
+        for (std::list<ITask*>::const_iterator it = _tasklist.begin(); it!=_tasklist.end(); ++it) {
+            (*it)->Cancel();
+        }
+        _mutex.unlock();
+        
+    }
+    
+    void ThreadTaskQueue::WaitForFinish()
+    {
+        
+        while (true) {
+            _mutex.lock();
+            bool isExecuting = false;
+            for (std::vector<QueueRunnable*>::iterator it = _threads.begin(); it!=_threads.end(); ++it) {
+                if ((*it)->TaskIsRunning()) {
+                    isExecuting = true;
+                    break;
+                }
+            }
+            if (!isExecuting&&_tasklist.size() == 0) {
+                _mutex.unlock();
+                break;
+            }
+            _mutex.wait(100);
+            _mutex.unlock();
+        }
+        
+    }
+    
+    void ThreadTaskQueue::Suspend()
+    {
+        _mutex.lock();
+        _suspended = true;
+        _mutex.unlock();
+    }
+    
+    void ThreadTaskQueue::Resume()
+    {
+        _mutex.lock();
+        _suspended = false;
+        _mutex.notify_all();
+        _mutex.unlock();
+
+    }
+    
+    void ThreadTaskQueue::NotifyQueue()
+    {
+        _mutex.notify_all();
+    }
+    
+    ITask* ThreadTaskQueue::NextTask()
+    {
+        while (_started && (_tasklist.empty()||_suspended)) {
+            _mutex.wait(WAIT_TIMEOUT);//defensive waiting time limit.
+        }
+        ITask* task = NULL;
+        if (_tasklist.size()>0) {
+            task = _tasklist.front();
+            _tasklist.pop_front();
+        }
+        return task;
+    }
+    
+    inline
+    void ThreadTaskQueue::LockQueue()
+    {
+        _mutex.lock();
+    }
+    
+    inline
+    void ThreadTaskQueue::UnlockQueue()
+    {
+        _mutex.unlock();
+    }
+
+    inline
+    void ThreadTaskQueue::FinishTask(ITask* task)
+    {
+        if(task->GetCategory() != NoCategory)
+        {
+            _recyclerMutex.lock();
+            std::map<TaskCategory,RecyclerPair>::iterator it = _recyclers.find(task->GetCategory());
+            if(it!=_recyclers.end())
+            {
+                RecyclerPair pair = it->second;
+                _recyclerMutex.unlock();
+                pair.recycler(task,pair.context);
+                return;
+            }
+            _recyclerMutex.unlock();
+        }
+        //禁止析构
+        //delete task;
+
+    }
+    
+    void ThreadTaskQueue::SetTaskRecycler(TaskCategory cat, TaskRecycler recycler,void *context)
+    {
+        _recyclerMutex.lock();
+        std::pair<TaskCategory,RecyclerPair> pair(cat, RecyclerPair(recycler,context));
+        _recyclers.insert(pair);
+        _recyclerMutex.unlock();
+    }
+
+    void ThreadTaskQueue::ClearTaskRecycler(TaskCategory cat)
+    {
+        _recyclerMutex.lock();
+        std::map<TaskCategory,RecyclerPair>::iterator it = _recyclers.find(cat);
+        if(it!=_recyclers.end())
+        {
+            _recyclers.erase(it);
+        }
+        _recyclerMutex.unlock();
+    }
+
+    ThreadTaskQueue::~ThreadTaskQueue()
+    {
+        this->Stop();//Defensive stop.
+    }
+    
+}

TaskQueue/ThreadTaskQueue.h

@@ -0,0 +1,89 @@
+//
+//  ThreadTaskQueue.h
+//  LibDriveRating-CXX
+//
+//  Created by Melo Yao on 6/9/14.
+//  Copyright (c) 2014 AutoNavi. All rights reserved.
+//
+
+#ifndef __LibDriveRating_CXX__ThreadTaskQueue__
+#define __LibDriveRating_CXX__ThreadTaskQueue__
+
+#include "TQInterface.h"
+#include "BaseTask.h"
+#include <list>
+#include <map>
+#include <vector>
+#include "threadpp/threadpp.h"
+
+namespace tq
+{
+    class QueueRunnable;
+    
+    class ThreadTaskQueue:public IQueue
+    {
+    public:
+        ThreadTaskQueue();
+        
+        void Start(unsigned int nThreads = 1);
+        
+        void Stop();
+        
+        bool IsStarted() const;
+        
+        void AddTask(ITask* task);//Will transfer the ownership of task.
+        
+        void GetTasks(ITask** tasksBuf, unsigned int taskBufSize) const;
+        
+        unsigned int TaskCount() const;
+        
+        void CancelAll();
+        
+        void WaitForFinish() ;
+        
+        void Suspend();
+        
+        void Resume();
+
+        void SetTaskRecycler(TaskCategory cat, TaskRecycler recycler,void *context);
+
+        void ClearTaskRecycler(TaskCategory cat);
+        
+        ~ThreadTaskQueue();
+        
+    protected:
+        
+        void LockQueue();
+        
+        void UnlockQueue();
+        
+        void NotifyQueue();
+        
+        ITask* NextTask();
+
+        void FinishTask(ITask* task);
+        
+        friend class QueueRunnable;
+
+    private:
+        struct RecyclerPair
+        {
+            TaskRecycler recycler;
+            void *context;
+            RecyclerPair(TaskRecycler r,void* c):
+                recycler(r),context(c)
+            {
+            }
+        };
+
+        std::map<TaskCategory,RecyclerPair> _recyclers;
+        std::list<ITask*> _tasklist;
+        std::vector<QueueRunnable*> _threads;
+        threadpp::recursivelock _mutex;
+        threadpp::lock _recyclerMutex;
+        bool _started;
+        bool _suspended;
+    };
+}
+
+#endif /* defined(__LibDriveRating_CXX__ThreadTaskQueue__) */

TaskQueue/threadpp/impl/pthread_lock.h

@@ -0,0 +1,35 @@
+//
+//  pthread_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__pthread_lock__
+#define __threadpp__pthread_lock__
+//extern "C"
+//{
+#include <pthread.h>
+//}
+namespace threadpp
+{
+    class pthread_lock
+    {
+        pthread_mutex_t _mutex;
+        pthread_cond_t _cond;
+        void operator=(const pthread_lock& l){};
+        pthread_lock(const pthread_lock& l){};
+    public:
+        pthread_lock();
+        ~pthread_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+}
+#include "pthread_lock.hpp"
+#endif /* defined(__threadpp__pthread_lock__) */

TaskQueue/threadpp/impl/pthread_lock.hpp

@@ -0,0 +1,73 @@
+//
+//  pthread_lock.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__pthread_lock__hpp__
+#define __threadpp__pthread_lock__hpp__
+#include "../threadpp_assert.h"
+#include <errno.h>
+#include <cstring>
+
+#include <sys/time.h>
+
+static inline void timespec_for(struct timespec* t,unsigned long millisecs) {
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+    t->tv_nsec = tv.tv_usec*1000 + millisecs*1000000;
+    t->tv_sec = tv.tv_sec + (t->tv_nsec/1000000000);
+    t->tv_nsec = t->tv_nsec%1000000000;
+}
+
+namespace threadpp{
+    inline pthread_lock::pthread_lock()
+    {
+        pthread_mutex_init(&_mutex, NULL);
+        pthread_cond_init(&_cond, NULL);
+    }
+    
+    inline pthread_lock::~pthread_lock()
+    {
+        pthread_mutex_destroy(&_mutex);
+        pthread_cond_destroy(&_cond);
+    }
+    
+    inline void pthread_lock::lock()
+    {
+        int code = pthread_mutex_lock(&_mutex);
+        ASSERT(code == 0, "lock failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::unlock()
+    {
+        int code = pthread_mutex_unlock(&_mutex);
+        ASSERT(code == 0, "unlock failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::wait()
+    {
+        int code = pthread_cond_wait(&_cond, &_mutex);
+        ASSERT(code == 0 || code == ETIMEDOUT, "wait failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::wait(unsigned long millisecs)
+    {
+        struct timespec ts;
+        timespec_for(&ts,millisecs);
+        int code = pthread_cond_timedwait(&_cond, &_mutex, &ts);
+        ASSERT(code == 0 || code == ETIMEDOUT, "timed wait failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_lock::notify()
+    {
+        pthread_cond_signal(&_cond);
+    }
+    
+    inline void pthread_lock::notify_all()
+    {
+        pthread_cond_broadcast(&_cond);
+    }
+}
+#endif

TaskQueue/threadpp/impl/pthread_thread.h

@@ -0,0 +1,45 @@
+//
+//  pthread_thread.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__pthread_thread__
+#define __threadpp__pthread_thread__
+#include <pthread.h>
+namespace threadpp
+{
+    class pthread_thread
+    {
+        struct pthread_context
+        {
+            void(*fp)(void* context);
+            void* context;
+        } _context;
+        
+        pthread_t _thread;
+        pthread_thread(){};
+        void operator=(const pthread_thread& t){};
+        pthread_thread(const pthread_thread& t){};
+        static void* pthread_fp_delegate(void*);
+    public:
+        typedef void (*runnable)(void* ctx);
+        typedef unsigned long long id_type;
+        
+        static id_type null_id();
+        
+        pthread_thread(runnable r,void* t);
+        
+        ~pthread_thread();
+        void join();
+        void detach();
+        bool is_equal(const pthread_thread& t) const;
+        id_type get_id() const;
+        static id_type current_thread_id();
+        static void sleep(unsigned long millisecs);
+    };
+}
+#include "pthread_thread.hpp"
+#endif /* defined(__threadpp__pthread_thread__) */

TaskQueue/threadpp/impl/pthread_thread.hpp

@@ -0,0 +1,110 @@
+//
+//  pthread_thread.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__pthread_thread__hpp__
+#define __threadpp__pthread_thread__hpp__
+
+#include <errno.h>
+#include "../threadpp_assert.h"
+#include <cstring>
+#include <cmath>
+#include <unistd.h>
+namespace threadpp
+{
+    inline pthread_thread::id_type pthread_thread::null_id()
+    {
+        return 0;
+    }
+    
+    inline void* pthread_thread::pthread_fp_delegate(void* ctx)
+    {
+        pthread_thread::pthread_context* pctx =static_cast<pthread_thread::pthread_context*>(ctx);
+        pctx->fp(pctx->context);
+        return NULL;
+    }
+    
+    inline pthread_thread::pthread_thread(runnable r,void* t)
+    {
+        _context.fp = r;
+        _context.context = t;
+        int code = pthread_create(&_thread, NULL, pthread_thread::pthread_fp_delegate, &_context);
+        ASSERT(code==0,"create thread failed,error:%s",strerror(code));
+    }
+    
+//    pthread_thread::pthread_thread(runnable r,void* t,float priority)
+//    {
+//        _context.fp = r;
+//        _context.context = t;
+//        pthread_attr_t tattr;
+//        pthread_attr_init(&tattr);
+//        struct sched_param schp;
+//        int policy = SCHED_FIFO;
+//        pthread_attr_getschedpolicy(&tattr, &policy);
+//        pthread_attr_getschedparam(&tattr, &schp);
+//        float pr =fminf(1.0f,fmaxf(0.0f, priority));
+//        schp.sched_priority = sched_get_priority_min(policy) + pr*(sched_get_priority_max(policy) - sched_get_priority_min(policy));
+//        pthread_attr_setschedparam(&tattr, &schp);
+//        int code = pthread_create(&_thread, &tattr, pthread_thread::pthread_fp_delegate, &_context);
+//        ASSERT(code==0,"create thread failed,error:%s",strerror(code));
+//        pthread_attr_destroy(&tattr);
+//    }
+    
+    inline pthread_thread::~pthread_thread()
+    {
+        ASSERT(_thread == 0,"%s","must join or detach a thread before destructing it");
+    }
+    
+    inline void pthread_thread::join()
+    {
+        void* ret = NULL;
+        int code = pthread_join(_thread, &ret);
+        _thread = 0;
+        ASSERT(code==0,"join thread failed,error:%s",strerror(code));
+    }
+    
+    inline void pthread_thread::detach()
+    {
+        int code = pthread_detach(_thread);
+        _thread = 0;
+        ASSERT(code==0,"join thread failed,error:%s",strerror(code));
+
+    }
+    
+    inline bool pthread_thread::is_equal(const pthread_thread& t) const
+    {
+        return pthread_equal(_thread, t._thread);
+    }
+    
+    inline void pthread_thread::sleep(unsigned long millisecs)
+    {
+        usleep((useconds_t)millisecs*1000);
+    }
+    
+    inline pthread_thread::id_type pthread_thread::get_id() const
+    {
+#ifdef __APPLE__
+        __uint64_t tid;
+        return pthread_threadid_np(_thread, &tid);
+        return tid;
+#else
+        return (unsigned long long)(_thread);
+#endif
+    }
+    
+    inline pthread_thread::id_type pthread_thread::current_thread_id()
+    {
+#ifdef __APPLE__
+        __uint64_t tid;
+        pthread_threadid_np(pthread_self(), &tid);
+        return tid;
+#else
+        return (unsigned long long)(pthread_self());
+#endif
+    }
+}
+#endif

TaskQueue/threadpp/impl/std_lock.h

@@ -0,0 +1,33 @@
+//
+//  std_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__std_lock__
+#define __threadpp__std_lock__
+#include <mutex>
+#include <condition_variable>
+namespace threadpp
+{
+    class std_lock
+    {
+        std::mutex _mutex;
+        std::condition_variable_any _cond;
+        void operator=(const std_lock& l){};
+        std_lock(const std_lock& l){};
+    public:
+        std_lock();
+        ~std_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+}
+#include "std_lock.hpp"
+#endif /* defined(__threadpp__std_lock__) */

TaskQueue/threadpp/impl/std_lock.hpp

@@ -0,0 +1,52 @@
+//
+//  std_lock.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__std_lock__hpp__
+#define __threadpp__std_lock__hpp__
+#include "../threadpp_assert.h"
+
+namespace threadpp{
+    
+    inline std_lock::std_lock():_mutex(),_cond()
+    {
+    }
+    
+    inline std_lock::~std_lock()
+    {
+    }
+    
+    inline void std_lock::lock()
+    {
+        _mutex.lock();
+    }
+    
+    inline void std_lock::unlock()
+    {
+        _mutex.unlock();
+    }
+    
+    inline void std_lock::wait()
+    {
+        _cond.wait(_mutex);
+    }
+    
+    inline void std_lock::wait(unsigned long millisecs)
+    {
+        _cond.wait_for(_mutex, std::chrono::milliseconds(millisecs));
+    }
+    
+    inline void std_lock::notify()
+    {
+        _cond.notify_one();
+    }
+    
+    inline void std_lock::notify_all()
+    {
+        _cond.notify_all();
+    }
+}
+#endif

TaskQueue/threadpp/impl/std_thread.h

@@ -0,0 +1,40 @@
+//
+//  std_thread.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__std_thread__
+#define __threadpp__std_thread__
+#include <thread>
+
+namespace threadpp
+{
+    class std_thread
+    {
+        std::thread _thread;
+        std_thread(){};
+        void operator=(const std_thread& t){};
+        std_thread(const std_thread& t){};
+    public:
+        typedef unsigned long long id_type;
+        
+        typedef void (*runnable)(void* ctx);
+        
+        static id_type null_id();
+        
+        std_thread(runnable r,void* t);
+        
+        ~std_thread();
+        void join();
+        void detach();
+        bool is_equal(const std_thread& t) const;
+        id_type get_id() const;
+        static id_type current_thread_id();
+        static void sleep(unsigned long millisecs);
+    };
+}
+#include "std_thread.hpp"
+#endif /* defined(__threadpp__std_thread__) */

TaskQueue/threadpp/impl/std_thread.hpp

@@ -0,0 +1,64 @@
+//
+//  std_thread.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__std_thread__hpp__
+#define __threadpp__std_thread__hpp__
+#include <functional>
+#include <algorithm>
+namespace threadpp
+{
+    inline std_thread::id_type std_thread::null_id()
+    {
+        return 0;
+    }
+    
+    inline void* std_fp_delegate(std_thread::runnable r, void* context)
+    {
+        r(context);
+        return nullptr;
+    }
+    
+    inline std_thread::std_thread(runnable r,void* t):_thread(std::bind(std_fp_delegate,r,t))
+    {
+    }
+    
+    inline std_thread::~std_thread()
+    {
+        
+    }
+    
+    inline void std_thread::join()
+    {
+        _thread.join();
+    }
+    
+    inline void std_thread::detach()
+    {
+        _thread.detach();
+    }
+    
+    inline bool std_thread::is_equal(const std_thread& t) const
+    {
+        return _thread.get_id() == t._thread.get_id();
+    }
+    
+    inline std_thread::id_type std_thread::get_id() const
+    {
+        return std::hash<std::thread::id>()(_thread.get_id());
+    }
+    
+    inline void std_thread::sleep(unsigned long millisecs)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(millisecs));
+    }
+    
+    inline std_thread::id_type std_thread::current_thread_id()
+    {
+        return std::hash<std::thread::id>()(std::this_thread::get_id());
+    }
+}
+#endif

TaskQueue/threadpp/impl/win_lock.h

@@ -0,0 +1,37 @@
+//
+//  win_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__win_lock__
+#define __threadpp__win_lock__
+
+#include <windows.h>
+
+namespace threadpp
+{
+    class win_lock
+    {
+        CRITICAL_SECTION _mutex;
+        CONDITION_VARIABLE _cond;
+        volatile unsigned int _owner;
+        volatile unsigned int _count;
+        void operator=(const win_lock& l){};
+        win_lock(const win_lock& l){};
+    public:
+        win_lock();
+        ~win_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+}
+
+#include "win_lock.hpp"
+#endif /* defined(__threadpp__win_lock__) */

TaskQueue/threadpp/impl/win_lock.hpp

@@ -0,0 +1,80 @@
+//
+//  win_lock.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__win_lock__hpp__
+#define __threadpp__win_lock__hpp__
+
+#include "win_thread.h"
+#include "../threadpp_assert.h"
+#include "stdio.h"
+namespace threadpp{
+    
+    static inline void test_error(const char* title)
+    {
+        DWORD e = GetLastError();
+        ASSERT(e==0,"%s failed,error:%d",title,e);
+    }
+    
+    inline win_lock::win_lock():_owner(0),_count(0)
+    {
+        SetLastError(0);
+        InitializeCriticalSection(&_mutex);
+        InitializeConditionVariable(&_cond);
+        test_error("init");
+    }
+    
+    inline win_lock::~win_lock()
+    {
+        DeleteCriticalSection(&_mutex);
+    }
+    
+    inline void win_lock::lock()
+    {
+        SetLastError(0);
+        
+        EnterCriticalSection(&_mutex);
+        
+        test_error("lock");
+    }
+    
+    inline void win_lock::unlock()
+    {
+        SetLastError(0);
+        LeaveCriticalSection(&_mutex);
+        test_error("unlock");
+    }
+    
+    inline void win_lock::wait()
+    {
+        SetLastError(0);
+        SleepConditionVariableCS(&_cond,&_mutex,0xFFFFFFFF);
+        test_error("wait");
+    }
+    
+    inline void win_lock::wait(unsigned long millisecs)
+    {
+        SetLastError(0);
+        SleepConditionVariableCS(&_cond,&_mutex,millisecs);
+        DWORD e = GetLastError();
+        ASSERT(e==0||e == ERROR_TIMEOUT,"timed wait failed,error:",e);
+    }
+    
+    inline void win_lock::notify()
+    {
+        SetLastError(0);
+        WakeConditionVariable(&_cond);
+        test_error("notify");
+    }
+    
+    inline void win_lock::notify_all()
+    {
+        SetLastError(0);
+        WakeAllConditionVariable(&_cond);
+        test_error("notify all");
+    }
+}
+#endif

TaskQueue/threadpp/impl/win_thread.h

@@ -0,0 +1,53 @@
+//
+//  win_thread.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef __threadpp__win_thread__
+#define __threadpp__win_thread__
+
+#include <windows.h>
+
+namespace threadpp
+{
+    class win_thread
+    {
+        struct win_context
+        {
+            void(*fp)(void*);
+            void* context;
+        } _context;
+#if NO_CRT
+        typedef DWORD handle_t;
+#else
+		typedef unsigned handle_t;
+#endif
+		static handle_t __stdcall win_fp_delegate(void* context);
+        handle_t _thread_id;
+        HANDLE _handle;
+        win_thread(){};
+        void operator=(const win_thread& t){};
+        win_thread(const win_thread& t){};
+    public:
+        typedef void (*runnable)(void* ctx);
+        typedef unsigned int id_type;
+        
+        static id_type null_id();
+        
+        win_thread(runnable r,void* t);
+        
+        ~win_thread();
+        void join();
+        void detach();
+        bool is_equal(const win_thread& t) const;
+        id_type get_id() const;
+        static id_type current_thread_id();
+        static void sleep(unsigned long millisecs);
+    };
+}
+
+#include "win_thread.hpp"
+#endif /* defined(__threadpp__win_thread__) */

TaskQueue/threadpp/impl/win_thread.hpp

@@ -0,0 +1,81 @@
+//
+//  win_thread.cpp
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+#ifndef __threadpp__win_thread__hpp__
+#define __threadpp__win_thread__hpp__
+
+#include "../threadpp_assert.h"
+#if NO_CRT
+#else
+#include <process.h>
+#endif
+namespace threadpp
+{
+
+    inline win_thread::id_type win_thread::null_id()
+    {
+        return 0;
+    }
+    
+	inline win_thread::handle_t win_thread::win_fp_delegate(void *context)
+    {
+        win_context* wctx = static_cast<win_context*>(context);
+        wctx->fp(wctx->context);
+        return 0;
+    }
+    
+    inline win_thread::win_thread(runnable r,void* t)
+    {
+        _context.fp = r;
+        _context.context = t;
+#if NO_CRT
+		_handle = CreateThread(NULL,NULL,win_thread::win_fp_delegate,&_context,0,&_thread_id);
+#else
+		_handle = (HANDLE)_beginthreadex(NULL,0,win_thread::win_fp_delegate,&_context,0,&_thread_id);
+#endif		
+    }
+    
+    inline win_thread::~win_thread()
+    {
+        ASSERT(_handle == 0,"%s","must join or detach a thread before destructing it");
+    }
+    
+    inline void win_thread::join()
+    {
+        unsigned code = WaitForSingleObject(_handle,0xFFFFFFFF);
+        CloseHandle(_handle);
+        _handle = 0;
+        ASSERT(code == WAIT_OBJECT_0 || code == WAIT_ABANDONED, "failed to join,error:%d",code);
+    }
+    
+    inline void win_thread::detach()
+    {
+		CloseHandle(_handle);
+        _handle = 0;
+    }
+    
+    inline bool win_thread::is_equal(const win_thread& t) const
+    {
+        return _thread_id == t._thread_id;
+    }
+    
+    inline void win_thread::sleep(unsigned long millisecs)
+    {
+		Sleep(millisecs);
+    }
+    
+	inline win_thread::id_type win_thread::get_id() const
+	{
+		return static_cast<unsigned int>(_thread_id);
+	}
+
+	inline win_thread::id_type win_thread::current_thread_id()
+	{
+		return  static_cast<unsigned int>(GetCurrentThreadId());
+	}
+}
+#endif

TaskQueue/threadpp/recursive_lock.h

@@ -0,0 +1,101 @@
+//
+//  recursive_lock.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/16/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef threadpp_recursive_lock_h
+#define threadpp_recursive_lock_h
+#include "threadpp_assert.h"
+namespace threadpp
+{
+    template <typename locktype,typename threadtype>
+    class recursive_lock
+    {
+        locktype _lock;
+        volatile typename threadtype::id_type _owner;
+        volatile unsigned int _count;
+    public:
+        recursive_lock();
+        void lock();
+        void unlock();
+        void wait();
+        void wait(unsigned long millisecs);
+        void notify();
+        void notify_all();
+    };
+    
+    template <typename locktype,typename threadtype>
+    recursive_lock<locktype,threadtype>::recursive_lock():_owner(threadtype::null_id()),_count(0)
+    {
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::lock()
+    {
+        typename threadtype::id_type tid = threadtype::current_thread_id();
+        if(tid == _owner)
+        {
+            _count++;
+        }
+        else
+        {
+            _lock.lock();
+            _owner = tid;
+            _count=1;
+        }
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::unlock()
+    {
+        typename threadtype::id_type tid = threadtype::current_thread_id();
+        ASSERT(tid == _owner,"%s", "unlock failed,try to unlock not owned mutex");
+        _count--;
+        if (_count == 0) {
+            _owner = threadtype::null_id();
+            _lock.unlock();
+        }
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::wait()
+    {
+        typename threadtype::id_type owner = _owner;
+        unsigned count = _count;
+        _owner = threadtype::null_id();
+        _count = 0;
+        _lock.wait();
+        _owner = owner;
+        _count = count;
+
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::wait(unsigned long millisecs)
+    {
+        typename threadtype::id_type owner = _owner;
+        unsigned count = _count;
+        _owner = threadtype::null_id();
+        _count = 0;
+        _lock.wait(millisecs);
+        _owner = owner;
+        _count = count;
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::notify()
+    {
+        _lock.notify();
+    }
+    
+    template <typename locktype,typename threadtype>
+    void recursive_lock<locktype,threadtype>::notify_all()
+    {
+        _lock.notify_all();
+    }
+}
+
+#endif

TaskQueue/threadpp/threadpp.h

@@ -0,0 +1,40 @@
+//
+//  threadpp.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef threadpp_threadpp_h
+#define threadpp_threadpp_h
+#include "recursive_lock.h"
+#if __posix || __APPLE__ || __linux
+#include "impl/pthread_thread.h"
+#include "impl/pthread_lock.h"
+namespace threadpp
+{
+    typedef pthread_thread thread;
+    typedef pthread_lock lock;
+    typedef recursive_lock<pthread_lock, pthread_thread> recursivelock;
+}
+#elif defined(WIN32)
+#include "impl/win_thread.h"
+#include "impl/win_lock.h"
+namespace threadpp
+{
+    typedef win_thread thread;
+    typedef win_lock lock;
+    typedef recursive_lock<win_lock, win_thread> recursivelock;
+}
+#elif __cplusplus>=201103L
+#include "impl/std_thread.h"
+#include "impl/std_lock.h"
+namespace threadpp
+{
+    typedef std_thread thread;
+    typedef std_lock lock;
+    typedef recursive_lock<std_lock, std_thread> recursivelock;
+}
+#endif
+#endif

TaskQueue/threadpp/threadpp_assert.h

@@ -0,0 +1,33 @@
+//
+//  defines.h
+//  threadpp
+//
+//  Created by Melo Yao on 1/15/15.
+//  Copyright (c) 2015 Melo Yao. All rights reserved.
+//
+
+#ifndef threadpp_assert_h
+#define threadpp_assert_h
+
+//forward VC++ DEBUG symbol.
+#if defined(_DEBUG) && !defined(DEBUG)
+#define DEBUG _DEBUG
+#endif
+
+#if DEBUG //assertions
+#ifdef __cplusplus
+#include <cassert>
+#include <cstdio>
+#else
+#include <assert.h>
+#include <stdio.h>
+#endif
+#define ASSERT0(__cond__) assert(__cond__)
+#define ASSERT(__cond__,__msg__,...) \
+do {if(!(__cond__)){printf(__msg__,__VA_ARGS__);printf("\n");assert(false);}}while(0)
+#else
+#define ASSERT0(__cond__)
+#define ASSERT(__cond__,__msg__,...)
+#endif //assertions
+
+#endif

common.h

@@ -0,0 +1,81 @@
+#ifndef __COMMON__H__
+#define __COMMON__H__
+
+
+#undef max 
+#undef min
+
+#include "opencv/highgui.h"
+#include "opencv2/opencv.hpp"
+
+
+#include <pcl/features/moment_of_inertia_estimation.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/filters/project_inliers.h>
+#include <pcl/surface/convex_hull.h>
+#include <pcl/surface/concave_hull.h>
+
+#include <pcl/segmentation/sac_segmentation.h> 
+
+#include <pcl/console/parse.h>
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/common/io.h>
+#include <pcl/common/common.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/filters/extract_indices.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/radius_outlier_removal.h>
+
+#include <pcl/common/transforms.h>
+#include <pcl/filters/crop_hull.h>
+#include <pcl/filters//voxel_grid.h>
+
+
+
+//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"
+
+typedef pcl::PointXYZ PointT;
+typedef pcl::PointCloud<PointT> PointCloudT;
+typedef pcl::PointCloud<PointT>::Ptr	PtrCloud;
+typedef Eigen::Matrix4f		EMetrix;
+
+#include <glog/logging.h>
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.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;
+
+
+#endif

laser/Laser.cpp

@@ -0,0 +1,306 @@
+#include "Laser.h"
+
+CBinaryData::CBinaryData()
+	:m_buf(0)
+	,m_length(0)
+{
+}
+CBinaryData::CBinaryData(const char* buf, int len, DATA_type type)
+{
+	if (len > 0)
+	{
+		m_buf = (char*)malloc(len);
+		memcpy(m_buf, buf, len);
+		m_length = len;
+	}
+}
+CBinaryData::CBinaryData(const CBinaryData& data)
+{
+	if (m_buf)
+	{
+		free(m_buf);
+		m_buf = 0;
+		m_length = 0;
+	}
+	if (data.Length() > 0)
+	{
+		m_buf = (char*)malloc(data.Length());
+		memcpy(m_buf, data.Data(), data.Length());
+		m_length = data.Length();
+	}
+}
+CBinaryData::~CBinaryData()
+{
+	if (m_buf)
+	{
+		free(m_buf);
+		m_length = 0;
+	}
+}
+char* CBinaryData::Data()const {
+	return m_buf;
+}
+int CBinaryData::Length()const {
+	return m_length;
+}
+CBinaryData& CBinaryData::operator=(CBinaryData& data)
+{
+	if (m_buf)
+	{
+		free(m_buf);
+		m_buf = 0;
+		m_length = 0;
+	}
+	if (data.Length() > 0)
+	{
+		m_buf = (char*)malloc(data.Length());
+		memcpy(m_buf, data.Data(), data.Length());
+		m_length = data.Length();
+	}
+	return *this;
+}
+bool CBinaryData::operator==(const char* str)
+{
+	int length = strlen(str);
+	return  (strncmp((const char*)m_buf, str, length) == 0);
+}
+const char* CBinaryData::operator+(int n)
+{
+	if (n < m_length)
+		return m_buf + n;
+	else
+		return NULL;
+}
+CBinaryData& CBinaryData::operator+(CBinaryData& data)
+{
+	if (data.Length() > 0)
+	{
+		int length = m_length + data.Length();
+		char* buf = (char*)malloc(length);
+		memcpy(buf, m_buf, m_length);
+		memcpy(buf + m_length, data.Data(), data.Length());
+		free(m_buf);
+		m_buf = buf;
+		m_length = length;
+	}
+	return *this;
+}
+char& CBinaryData::operator[](int n)
+{
+	if (n >= 0 && n < m_length)
+		return m_buf[n];
+}
+//////////////
+
+CLaser::CLaser(int id, Automatic::stLaserCalibParam laser_param)
+	:m_ThreadRcv(0)
+	,m_ThreadPro(0)
+	,m_id(id)
+	,m_statu(eLaser_disconnect)
+	,m_dMatrix(0)
+	, m_point_callback_fnc(0)
+	, m_point_callback_pointer(0)
+	,m_laser_param(laser_param)
+	,m_bSave_file(false)
+{
+}
+
+
+CLaser::~CLaser()
+{
+	if (m_dMatrix)
+	{
+		free(m_dMatrix);
+		m_dMatrix = 0;
+	}
+}
+
+bool CLaser::Start()
+{
+	m_statu = eLaser_busy;
+	m_bStart_capture.Notify(true);
+	return true;
+}
+
+bool CLaser::Stop()
+{
+	m_bStart_capture.Notify(false);
+	m_binary_log_tool.close();
+	m_pts_log_tool.close();
+	return true;
+}
+
+void CLaser::SetMetrix(double* data)
+{
+	if (m_dMatrix == 0)
+	{
+		m_dMatrix = (double*)malloc(12 * sizeof(double));
+	}
+	memcpy(m_dMatrix, data, 12 * sizeof(double));
+}
+void CLaser::SetPointCallBack(PointCallBack fnc, void* pointer)
+{
+	m_point_callback_fnc = fnc;
+	m_point_callback_pointer = pointer;
+}
+
+bool CLaser::Connect()
+{
+	m_bThreadRcvRun.Notify(true);
+	if(m_ThreadRcv==0)
+		m_ThreadRcv = new std::thread(&CLaser::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->detach();
+	m_statu = eLaser_connect;
+	return true;
+}
+void CLaser::Disconnect()
+{
+	if (m_ThreadRcv)
+	{
+		delete m_ThreadRcv;
+		m_ThreadRcv = 0;
+	}
+	if (m_ThreadPro)
+	{
+		delete m_ThreadPro;
+		m_ThreadPro = NULL;
+	}
+	m_statu = eLaser_disconnect;
+}
+
+void CLaser::SetSaveDir(std::string strDir,bool bSaveFile)
+{
+	m_bSave_file = bSaveFile;
+	m_pts_log_tool.close();
+	m_binary_log_tool.close();
+
+	if (bSaveFile == false)
+		return;
+
+	m_pts_save_path = strDir;
+	char pts_file[255] = { 0 };
+	sprintf(pts_file, "%s/pts%d.txt", strDir.c_str(), m_id+1);
+	m_pts_log_tool.open(pts_file);
+
+	char bin_file[255] = { 0 };
+	sprintf(bin_file, "%s/laser%d.data", strDir.c_str(), m_id+1);
+	m_binary_log_tool.open(bin_file,true);
+}
+
+void CLaser::thread_recv()
+{
+	while (m_bThreadRcvRun.WaitFor(1))
+	{
+		if (m_bStart_capture.WaitFor(1) == false)
+			continue;
+		CBinaryData* data=new CBinaryData();
+		if (this->RecvData(*data))
+		{
+			m_queue_laser_data.push(data);
+		}
+		else
+		{
+			delete data;
+		}
+		
+	}
+}
+
+CPoint3D CLaser::transfor(CPoint3D point)
+{
+	if (m_dMatrix)
+	{
+		CPoint3D result;
+		double x = point.x;
+		double y = point.y;
+		double z = point.z;
+		result.x = x * m_dMatrix[0] + y*m_dMatrix[1] + z*m_dMatrix[2] + m_dMatrix[3];
+		result.y = x * m_dMatrix[4] + y*m_dMatrix[5] + z*m_dMatrix[6] + m_dMatrix[7];
+		result.z = x * m_dMatrix[8] + y*m_dMatrix[9] + z*m_dMatrix[10] + m_dMatrix[11];
+		return result;
+	}
+	else
+	{
+		return point;
+	}
+}
+
+void CLaser::thread_toXYZ()
+{
+	while (m_bThreadProRun.WaitFor(1))
+	{
+		if (m_bStart_capture.WaitFor(1) == false)
+		{
+			m_queue_laser_data.clear();
+			continue;
+		}
+		CBinaryData* pData=NULL;
+		bool bPop = m_queue_laser_data.front(pData);
+		DATA_type type = eUnknow;
+		if (bPop || m_last_data.Length() != 0)
+		{
+			std::vector<CPoint3D> cloud;
+			if (bPop)
+			{
+				if (pData == NULL)
+					continue;
+				if (m_bSave_file)
+					m_binary_log_tool.write(pData->Data(), pData->Length());
+				type= this->Data2PointXYZ(pData, cloud);
+			}
+			else
+			{
+				type = this->Data2PointXYZ(NULL, cloud);
+			}
+			if (type == eUnknow)
+			{
+				delete pData;
+				continue;
+			}
+
+			if (type == eData || type == eStart || type == eStop)
+			{
+				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);
+						m_pts_log_tool.write(buf, strlen(buf));
+					}
+				}
+				if (type == eStop)
+				{
+					if (m_bSave_file)
+					{
+						m_pts_log_tool.close();
+						m_binary_log_tool.close();
+					}
+					m_statu = eLaser_ready;
+				}
+
+			}
+			else if (type == eReady)
+			{
+				if (m_bSave_file)
+				{
+					m_pts_log_tool.close();
+					m_binary_log_tool.close();
+				}
+				m_statu = eLaser_ready;
+			}
+
+			m_queue_laser_data.try_pop();
+			delete pData;
+		}
+		
+	}
+}

laser/Laser.h

@@ -0,0 +1,236 @@
+#ifndef __LASER__HH__
+#define __LASER__HH__
+#include "Point2D.h"
+#include "Point3D.h"
+#include "LogFiles.h"
+#include "src/CalibParam.pb.h"
+#include "src/StdCondition.h"
+
+
+
+enum DATA_type
+{
+	eStart=0
+	,eReady
+	,eData
+	,eStop
+	,eHerror
+	,eUnknow
+};
+class CBinaryData
+{
+public:
+	CBinaryData();
+	CBinaryData(const CBinaryData& data);
+	~CBinaryData();
+	CBinaryData(const char* buf, int len, DATA_type type= eUnknow);
+	CBinaryData& operator=(CBinaryData& data);
+	bool operator==(const char* str);
+	const char* operator+(int n);
+	CBinaryData& operator+(CBinaryData& data);
+	char& operator[](int n);
+	char*		Data()const;
+	int			Length()const;
+
+protected:
+	char*			m_buf;
+	int				m_length;
+};
+
+
+#include <queue>  
+#include <memory>  
+#include <mutex>  
+#include <condition_variable>  
+template<typename T>
+class threadsafe_queue
+{
+private:
+	mutable std::mutex mut;
+	std::queue<T> data_queue;
+	std::condition_variable data_cond;
+public:
+	threadsafe_queue() {}
+	threadsafe_queue(threadsafe_queue const& other)
+	{
+		std::lock_guard<std::mutex> lk(other.mut);
+		data_queue = other.data_queue;
+	}
+	~threadsafe_queue()
+	{
+		while (!empty())
+		{
+			try_pop();
+		}
+	}
+	size_t size()
+	{
+		return data_queue.size();
+	}
+	void push(T new_value)//入队操作  
+	{
+		std::lock_guard<std::mutex> lk(mut);
+		data_queue.push(new_value);
+		data_cond.notify_one();
+	}
+	void wait_and_pop(T& value)//直到有元素可以删除为止  
+	{
+		std::unique_lock<std::mutex> lk(mut);
+		data_cond.wait(lk, [this] {return !data_queue.empty(); });
+		value = data_queue.front();
+		data_queue.pop();
+	}
+	std::shared_ptr<T> wait_and_pop()
+	{
+		std::unique_lock<std::mutex> lk(mut);
+		data_cond.wait(lk, [this] {return !data_queue.empty(); });
+		std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
+		data_queue.pop();
+		return res;
+	}
+	//只访问 不 pop
+	bool front(T& value)
+	{
+		std::lock_guard<std::mutex> lk(mut);
+		if (data_queue.empty())
+			return false;
+		value = data_queue.front();
+		return true;
+	}
+
+	bool try_pop(T& value)//不管有没有队首元素直接返回  
+	{
+		if (data_queue.empty())
+			return false;
+		std::lock_guard<std::mutex> lk(mut);
+		value = data_queue.front();
+		data_queue.pop();
+		return true;
+	}
+	std::shared_ptr<T> try_pop()
+	{
+		std::lock_guard<std::mutex> lk(mut);
+		if (data_queue.empty())
+			return std::shared_ptr<T>();
+		std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
+		data_queue.pop();
+		return res;
+	}
+	bool empty() const
+	{
+		std::lock_guard<std::mutex> lk(mut);
+		return data_queue.empty();
+	}
+	void clear()
+	{
+		while (!empty()) {
+			try_pop();
+		}
+	}
+};
+
+
+//////////////
+
+
+enum eLaserStatu
+{
+	eLaser_connect=0,
+	eLaser_ready ,	//空闲状态
+	eLaser_busy			//扫描中
+	, eLaser_disconnect	//断线
+
+};
+
+
+typedef void (*PointCallBack)(CPoint3D ,  void* );
+class CLaser
+{
+public:
+	CLaser(int id,Automatic::stLaserCalibParam laser_param);
+	virtual ~CLaser();
+	///连接数据源
+	virtual bool Connect();
+	virtual void Disconnect();
+	///开始、停止采集
+	virtual bool Start();
+	virtual bool Stop();
+	///设置点云变换矩阵（标定参数）
+	void SetMetrix(double* data);
+	///设置获取三维点回调函数
+	void SetPointCallBack(PointCallBack fnc,void* pointer);
+
+public:
+	///设置数据存储路径
+	void			SetSaveDir(std::string strDir,bool bSave=true);
+	///查询雷达是否空闲
+	bool			IsReady() { return m_statu == eLaser_ready; }
+	int				ID() { return m_id; }
+protected:
+	////获取原始数据包
+	virtual bool RecvData(CBinaryData& data) = 0;
+	////解析原始数据包成点云
+	virtual DATA_type Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)=0;
+	void thread_recv();
+	void thread_toXYZ();
+	////点云变换
+	virtual CPoint3D transfor(CPoint3D point);
+protected:
+	std::thread*		m_ThreadRcv;
+	std::thread*		m_ThreadPro;
+	StdCondition		m_bThreadRcvRun;
+	StdCondition		m_bThreadProRun;
+	int					m_id;
+	eLaserStatu			m_statu;
+	bool				m_bSave_file;
+	Automatic::stLaserCalibParam  m_laser_param;////配置参数
+
+    //数据处理相关
+	threadsafe_queue<CBinaryData*>		m_queue_laser_data;     // 数据队列
+	CBinaryData							m_last_data;			//上一报数据中未解析完的		
+	double*								m_dMatrix;              
+	PointCallBack						m_point_callback_fnc;
+	void*								m_point_callback_pointer;
+
+
+    //数据存储
+	CLogFile		m_binary_log_tool;										//存储二进制
+	CLogFile		m_pts_log_tool;										//存储点云
+	std::string		m_pts_save_path;
+	StdCondition	m_bStart_capture;								
+
+};
+
+
+class LaserRegistory
+{
+	typedef CLaser*  (*CreateLaserFunc)(int id, Automatic::stLaserCalibParam laser_param);
+public:
+	LaserRegistory(std::string name, CreateLaserFunc pFun) {
+		AddCreator(name, pFun);
+	}
+	static CLaser* CreateLaser(std::string name, int id,Automatic::stLaserCalibParam laser_param) {
+		if (GetFuncMap().count(name) == 0)
+            return 0;
+		return GetFuncMap()[name](id,laser_param);
+	}
+private:
+	static std::map<std::string, CreateLaserFunc>& GetFuncMap() {
+		static std::map<std::string, CreateLaserFunc>* g_map = new std::map<std::string, CreateLaserFunc>;
+		return *g_map;
+	}
+	void AddCreator(std::string name, CreateLaserFunc pFun) {
+		GetFuncMap()[name] = pFun;
+	}
+};
+
+
+#define RegisterLaser(NAME) \
+	static CLaser* Create_##NAME##_Laser(int id, Automatic::stLaserCalibParam param)	\
+	{																\
+		return new C##NAME##Laser(id,param);							\
+	}																\
+	LaserRegistory g_##NAME##_Laser(#NAME,Create_##NAME##_Laser);
+
+
+#endif

laser/LivoxHubLaser.cpp

@@ -0,0 +1,10 @@
+#include "LivoxHubLaser.h"
+
+CLivoxHubLaser::CLivoxHubLaser()
+{
+
+}
+CLivoxHubLaser::~CLivoxHubLaser()
+{
+
+}

laser/LivoxHubLaser.h

@@ -0,0 +1,19 @@
+#ifndef LIVOXHUBLASER_H
+#define LIVOXHUBLASER_H
+
+
+class CLivoxHubLaser
+{
+public:
+    CLivoxHubLaser();
+    ~CLivoxHubLaser();
+protected:
+    /*static void InitLivoxHub();
+    static void LidarDataCallbackHub(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *laser);
+    static void OnDeviceChangeHub(const DeviceInfo *info, DeviceEvent type);
+    static void OnDeviceBroadcastHub(const BroadcastDeviceInfo *info);
+    static void OnSampleCallbackHub(uint8_t status, uint8_t handle, uint8_t response, void *data);*/
+
+};
+
+#endif // LIVOXHUBLASER_H

laser/LivoxLaser.cpp

@@ -0,0 +1,253 @@
+
+#include "LivoxLaser.h"
+#include "../common.h"
+
+RegisterLaser(Livox)
+
+CLivoxLaser::DeviceItem CLivoxLaser::g_devices[kMaxLidarCount] = { 0 };
+
+std::map<uint8_t, std::string>	CLivoxLaser::g_handle_sn= std::map<uint8_t, std::string>();
+std::map<std::string, uint8_t>	CLivoxLaser::g_sn_handle = std::map<std::string, uint8_t>();
+std::map<std::string, CLivoxLaser*> CLivoxLaser::g_sn_laser= std::map<std::string, CLivoxLaser*>();
+CLivoxLaser*						CLivoxLaser::g_all_laser[kMaxLidarCount] = { 0 };
+unsigned int  CLivoxLaser::g_count[kMaxLidarCount] = { 0 };
+
+void CLivoxLaser::UpdataHandle()
+{
+	std::string sn = m_laser_param.sn();
+	if (g_sn_handle.find(sn) != g_sn_handle.end())
+	{
+		m_handle = g_sn_handle[sn];
+	}
+
+}
+
+
+bool CLivoxLaser::IsScanComplete()
+{
+	return g_count[m_handle] >= m_frame_maxnum;
+}
+
+void CLivoxLaser::InitLivox()
+{
+	static bool g_init = false;
+	if (g_init == false)
+	{
+		if (!Init()) {
+			LOG(INFO) << "livox sdk init failed...";
+		}
+		else
+		{
+			LivoxSdkVersion _sdkversion;
+			GetLivoxSdkVersion(&_sdkversion);
+			char buf[255] = { 0 };
+			sprintf(buf, "Livox SDK version %d.%d.%d .\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
+			LOG(INFO) << buf;
+			SetBroadcastCallback(OnDeviceBroadcast);
+			SetDeviceStateUpdateCallback(OnDeviceChange);
+			g_init = true;
+		}
+	}
+}
+
+CLivoxLaser::CLivoxLaser(int id, Automatic::stLaserCalibParam laser_param)
+	:CLaser(id, laser_param)
+{
+	
+	m_frame_maxnum = laser_param.frame_num();
+	if(laser_param.type()=="Livox")
+		g_sn_laser.insert(std::make_pair(laser_param.sn(), this));
+
+	InitLivox();
+}
+
+
+
+
+CLivoxLaser::~CLivoxLaser()
+{
+}
+
+void CLivoxLaser::OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
+	CLivoxLaser* laser = (CLivoxLaser*)data;
+	if (status == kStatusSuccess) {
+		if (response != 0) {
+			g_devices[handle].device_state = kDeviceStateConnect;
+		}
+	}
+	else if (status == kStatusTimeout) {
+		g_devices[handle].device_state = kDeviceStateConnect;
+	}
+}
+
+void CLivoxLaser::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
+{
+	if (info == NULL) {
+		return;
+	}
+	
+	uint8_t handle = info->handle;
+	if (handle >= kMaxLidarCount) {
+		return;
+	}
+	if (type == kEventConnect) {
+		//QueryDeviceInformation(handle, OnDeviceInformation, NULL);
+		if (g_devices[handle].device_state == kDeviceStateDisconnect) {
+			g_devices[handle].device_state = kDeviceStateConnect;
+			g_devices[handle].info = *info;
+		}
+	}
+	else if (type == kEventDisconnect) {
+		g_devices[handle].device_state = kDeviceStateDisconnect;
+	}
+	else if (type == kEventStateChange) {
+		g_devices[handle].info = *info;
+	}
+
+	if (g_devices[handle].device_state == kDeviceStateConnect) {
+		
+		if (g_devices[handle].info.state == kLidarStateNormal) {
+			if (g_devices[handle].info.type == kDeviceTypeHub) {
+				HubStartSampling(OnSampleCallback, NULL);
+			}
+			else {
+				LidarStartSampling(handle, OnSampleCallback, NULL);
+			}
+			g_devices[handle].device_state = kDeviceStateSampling;
+		}
+	}
+}
+void CLivoxLaser::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
+{
+	if (info == NULL) {
+		return;
+	}
+
+	//printf("Receive Broadcast Code %s\n", info->broadcast_code);
+	LOG(INFO) << " broadcast  sn : " << info->broadcast_code;
+	bool result = false;
+	uint8_t handle = 0;
+	result = AddLidarToConnect(info->broadcast_code, &handle);
+	
+	if (result == kStatusSuccess) {
+		/** Set the point cloud data for a specific Livox LiDAR. */
+		SetDataCallback(handle, LidarDataCallback, NULL);
+		g_devices[handle].handle = handle;
+		g_devices[handle].device_state = kDeviceStateDisconnect;
+
+		std::string sn = info->broadcast_code;
+		if (g_handle_sn.find(handle) != g_handle_sn.end())
+			g_handle_sn[handle] = sn;
+		else
+			g_handle_sn.insert(std::make_pair(handle,sn));
+
+		if (g_sn_handle.find(sn) != g_sn_handle.end())
+			g_sn_handle[sn] = handle;
+		else
+			g_sn_handle.insert(std::make_pair(sn,handle));
+		
+	}
+
+}
+
+void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data)
+{
+	///第一次解析数据， 刷新各实例的handle
+	CLivoxLaser* livox = g_all_laser[handle];
+	if (livox == 0)
+	{
+		if (g_handle_sn.find(handle) != g_handle_sn.end())
+		{
+			std::string sn = g_handle_sn[handle];
+			if (g_sn_laser.find(sn) != g_sn_laser.end())
+			{
+				livox = g_sn_laser[sn];
+				g_all_laser[handle] = livox;
+				if (livox)
+					livox->UpdataHandle();
+			}
+		}
+	}
+
+	
+	if (data && livox)
+	{
+		if (livox->m_bStart_capture.WaitFor(1))
+		{
+			if (livox->IsScanComplete())
+			{
+				livox->Stop();
+				return;
+			}
+			if (g_count[handle] >= livox->m_frame_maxnum)
+				return;
+			uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
+			LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
+			
+			CBinaryData* data_bin = new CBinaryData((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
+			livox->m_queue_livox_data.push(data_bin);
+			g_count[handle]++;
+			
+		}
+		else
+		{
+			//接收到数据，但未开始
+			livox->m_statu = eLaser_ready;
+            usleep(10*1000);
+		}
+	}
+	
+}
+
+bool CLivoxLaser::Connect()
+{
+	return CLaser::Connect();
+}
+void CLivoxLaser::Disconnect() 
+{
+	CLaser::Disconnect();
+}
+bool CLivoxLaser::Start()
+{
+	LOG(INFO) << " livox start :"<<this; 
+	//清除数据，开始
+	m_queue_livox_data.clear();
+	g_count[m_handle] = 0;
+	return CLaser::Start();
+}
+bool CLivoxLaser::Stop()
+{
+	return CLaser::Stop();
+}
+
+bool CLivoxLaser::RecvData(CBinaryData& data)
+{
+	CBinaryData* livox_data;
+	if (m_queue_livox_data.try_pop(livox_data))
+	{
+		data = *livox_data;
+		delete livox_data;
+		return true;
+	}
+	return false;
+}
+DATA_type CLivoxLaser::Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)
+{
+	LivoxRawPoint *p_point_data = (LivoxRawPoint *)pData->Data();
+	int count = pData->Length() / (sizeof(LivoxRawPoint));
+	
+	if (count <= 0)
+		return eUnknow;
+	for (int i = 0; i < count; ++i)
+	{
+		LivoxRawPoint point = p_point_data[i];
+		CPoint3D pt;
+		pt.x = point.x;
+		pt.y = point.y;
+		pt.z = point.z;
+		points.push_back(pt);
+	}
+	return eData;
+}
+
+

laser/LivoxLaser.h

@@ -0,0 +1,63 @@
+#ifndef __LIVOX_MID_40_LIDAR__H
+#define __LIVOX_MID_40_LIDAR__H
+#include "Laser.h"
+#include "livox_def.h"
+#include "livox_sdk.h"
+#include <map>
+
+
+class CLivoxLaser :public CLaser
+{
+protected:
+	typedef enum {
+		kDeviceStateDisconnect = 0,
+		kDeviceStateConnect = 1,
+		kDeviceStateSampling = 2,
+	} DeviceState;
+	typedef struct {
+		uint8_t handle;
+		DeviceState device_state;
+		DeviceInfo info;
+	} DeviceItem;
+
+
+public:
+	CLivoxLaser(int id, Automatic::stLaserCalibParam laser_param);
+	~CLivoxLaser();
+
+	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);
+	virtual bool IsScanComplete();
+	virtual void UpdataHandle();
+	
+protected:
+	static void InitLivox();
+	static void LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *laser);
+	static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
+	static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
+	static void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data);
+
+protected:
+	uint8_t									m_handle;
+	unsigned int							m_frame_maxnum;
+	threadsafe_queue<CBinaryData*>			m_queue_livox_data;
+
+	static DeviceItem						g_devices[kMaxLidarCount];
+	
+	static std::map<uint8_t,std::string>	g_handle_sn;
+	static std::map<std::string, uint8_t>	g_sn_handle;
+	static std::map<std::string, CLivoxLaser*> g_sn_laser;
+	static CLivoxLaser*						g_all_laser[kMaxLidarCount];
+	static unsigned int						g_count[kMaxLidarCount];
+
+
+};
+
+#endif
+

laser/LivoxMid100Laser.cpp

@@ -0,0 +1,65 @@
+
+#include "LivoxMid100Laser.h"
+#include "common.h"
+
+RegisterLaser(LivoxMid100)
+
+
+CLivoxMid100Laser::CLivoxMid100Laser(int id, Automatic::stLaserCalibParam laser_param)
+	:CLivoxLaser(id, laser_param)
+{
+	m_frame_maxnum = laser_param.frame_num();
+	if (laser_param.type() == "LivoxMid100")
+	{
+		std::string sn = laser_param.sn();
+		std::string sn1 = sn, sn2 = sn, sn3 = sn;
+		sn1 += "1";
+		sn2 += "2";
+		sn3 += "3";
+		g_sn_laser.insert(std::make_pair(sn1, this));
+		g_sn_laser.insert(std::make_pair(sn2, this));
+		g_sn_laser.insert(std::make_pair(sn3, this));
+	}
+}
+
+void CLivoxMid100Laser::UpdataHandle()
+{
+	std::string sn = m_laser_param.sn();
+	std::string sn1 = sn, sn2 = sn, sn3 = sn;
+	sn1 += "1";
+	sn2 += "2";
+	sn3 += "3";
+	if (g_sn_handle.find(sn1) != g_sn_handle.end())
+	{
+		m_handle1 = g_sn_handle[sn1];
+	}
+	if (g_sn_handle.find(sn2) != g_sn_handle.end())
+	{
+		m_handle2 = g_sn_handle[sn2];
+	}
+	if (g_sn_handle.find(sn3) != g_sn_handle.end())
+	{
+		m_handle3 = g_sn_handle[sn3];
+	}
+}
+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;
+}
+
+
+bool CLivoxMid100Laser::Start()
+{
+		LOG(INFO) << " livoxMid100 start :" << this;
+		//Çå³ýÊý¾Ý£¬¿ªÊ¼
+		m_queue_livox_data.clear();
+		g_count[m_handle1] = 0;
+		g_count[m_handle2] = 0;
+		g_count[m_handle3] = 0;
+		return CLaser::Start();
+}
+
+CLivoxMid100Laser::~CLivoxMid100Laser()
+{
+}

laser/LivoxMid100Laser.h

@@ -0,0 +1,24 @@
+#pragma once
+#include "LivoxLaser.h"
+#include "livox_def.h"
+#include "livox_sdk.h"
+#include <map>
+class CLivoxMid100Laser : public CLivoxLaser
+{
+
+public:
+	CLivoxMid100Laser(int id, Automatic::stLaserCalibParam laser_param);
+	~CLivoxMid100Laser();
+	virtual bool Start();
+
+protected:
+	virtual bool IsScanComplete();
+	virtual void UpdataHandle();
+
+protected:
+	uint8_t		m_handle1;
+	uint8_t		m_handle2;
+	uint8_t		m_handle3;
+};
+
+

laser/LogFiles.cpp

@@ -0,0 +1,145 @@
+
+#include "LogFiles.h"
+#include <string.h>
+
+const string CLogFiles::m_strFileNameLoglaserBinary1 = "laser1.data";
+const string CLogFiles::m_strFileNameLoglaserValue1 = "laser1.txt";
+const string CLogFiles::m_strFileNameLogPoints1 = "points1.txt";
+
+const string CLogFiles::m_strFileNameLoglaserBinary2 = "laser2.data";
+const string CLogFiles::m_strFileNameLoglaserValue2 = "laser2.txt";
+const string CLogFiles::m_strFileNameLogPoints2 = "points2.txt";
+
+
+
+CLogFile::CLogFile()
+{
+
+}
+
+
+CLogFile::~CLogFile()
+{
+	if (m_file)
+	{
+        m_lock.lock();
+		m_file.close();
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::open(const char *_Filename, bool binary /* = false */)
+{
+	if (!m_file.is_open())
+	{
+        m_lock.lock();
+		if (binary)
+			m_file.open(_Filename, ios_base::out | ios_base::binary);
+		else
+			m_file.open(_Filename, ios_base::out);
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::write(const char *_Str,	streamsize _Count)
+{
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+		m_file.write(_Str, _Count);
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::write_double(double val)
+{
+	char buffer[512] = { 0 };
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+        sprintf(buffer, "%f ", val);
+		m_file.write(buffer, strlen(buffer));
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::write_int(int val)
+{
+	char buffer[512] = { 0 };
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+        sprintf(buffer, "%d ", val);
+		m_file.write(buffer, strlen(buffer));
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::close()
+{
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+		m_file.close();
+        m_lock.unlock();
+	}
+}
+
+/****************************************************************************/
+
+CLogFiles::CLogFiles()
+	: m_bBinary(true)
+	, m_bValue(true)
+	, m_bPoints(true)
+{
+
+}
+
+CLogFiles::~CLogFiles()
+{
+	close_project();
+}
+
+void CLogFiles::close_project()
+{
+	m_logLaserBinary1.close();
+	m_logLaserValue1.close();
+	m_logPoints1.close();
+
+	m_logLaserBinary2.close();
+	m_logLaserValue2.close();
+	m_logPoints2.close();
+}
+
+void CLogFiles::new_project(const char * path)
+{
+	close_project();
+
+	if (m_bBinary) 
+	{
+		string filepath1 = string(path) + m_strFileNameLoglaserBinary1;
+		m_logLaserBinary1.open(filepath1.c_str(), true);
+
+		string filepath3 = string(path) + m_strFileNameLoglaserBinary2;
+		m_logLaserBinary2.open(filepath3.c_str(), true);
+	}
+
+	if (m_bValue)
+	{
+		string filepath1 = string(path) + m_strFileNameLoglaserValue1;
+		m_logLaserValue1.open(filepath1.c_str(), true);
+
+		string filepath3 = string(path) + m_strFileNameLoglaserValue2;
+		m_logLaserValue2.open(filepath3.c_str(), true);
+	}
+
+	if (m_bPoints)
+	{
+		string filepath2 = string(path) + m_strFileNameLogPoints1;
+		m_logPoints1.open(filepath2.c_str(), true);
+
+		string filepath4 = string(path) + m_strFileNameLogPoints2;
+		m_logPoints2.open(filepath4.c_str(), true);
+	}
+	
+}

laser/LogFiles.h

@@ -0,0 +1,73 @@
+#ifndef __LOG__FILE__HH
+#define __LOG__FILE__HH
+#include <string>
+#include <fstream>
+#include <mutex>
+
+using namespace std;
+
+class CLogFile
+{
+public:
+	CLogFile();
+	~CLogFile();
+
+	void open(const char *_Filename, bool binary = false);
+	void write(const char *_Str, streamsize _Count);
+	void close();
+	inline bool is_open() const { return m_file.is_open(); };
+
+	void write_double(double val);
+	void write_int(int val);
+
+public:
+	fstream m_file;
+
+public:
+    std::mutex m_lock;
+};
+
+
+class CLogFiles
+{
+public:
+	CLogFiles();
+	~CLogFiles();
+
+	void set_config(bool binary = true, bool value = true, bool points = true)
+	{
+		m_bBinary = binary;
+		m_bValue = value;
+		m_bPoints = points;
+	}
+
+	void new_project(const char * path);
+	void close_project();
+
+private:
+	bool m_bBinary;
+	bool m_bValue;
+	bool m_bPoints;
+
+
+public:		  
+	CLogFile m_logLaserBinary1;
+	CLogFile m_logLaserValue1;
+	CLogFile m_logPoints1;
+		
+	CLogFile m_logLaserBinary2;
+	CLogFile m_logLaserValue2;
+	CLogFile m_logPoints2;
+
+
+public:
+	static const string m_strFileNameLoglaserBinary1;
+	static const string m_strFileNameLoglaserValue1;
+	static const string m_strFileNameLogPoints1;
+
+	static const string m_strFileNameLoglaserBinary2;
+	static const string m_strFileNameLoglaserValue2;
+	static const string m_strFileNameLogPoints2;
+};
+
+#endif

laser/Point2D.cpp

@@ -0,0 +1,24 @@
+
+#include "Point2D.h"
+
+
+CPoint2D::CPoint2D(double x, double y)
+{
+	this->x = x;
+	this->y = y;
+}
+
+
+CPoint2D::~CPoint2D()
+{
+}
+
+double CPoint2D::GetDistance(CPoint2D &pt1, CPoint2D &pt2)
+{
+	return sqrt((pt1.x - pt2.x)*(pt1.x - pt2.x) + (pt1.y - pt2.y)*(pt1.y - pt2.y));
+}
+
+double CPoint2D::GetDistance(CPoint2D &pt)
+{
+	return sqrt((x - pt.x)*(x - pt.x) + (y - pt.y)*(y - pt.y));
+}

laser/Point2D.h

@@ -0,0 +1,35 @@
+#ifndef POINT_2D__HH__
+#define POINT_2D__HH__
+#include <math.h>
+
+#ifndef PI
+#define PI 3.141592653589793238462643383279502f   
+#endif
+
+#ifndef DEGREES
+#define DEGREES 0.01745329251994329576923690768489f                           
+#endif
+
+class CPoint2D
+{
+public:
+	double x, y;
+
+public:
+	CPoint2D(double x = 0, double y = 0);
+	virtual ~CPoint2D();
+	
+	inline double GetDistance(CPoint2D &pt);
+	static double GetDistance(CPoint2D &pt1, CPoint2D &pt2);
+};
+
+class CPointPolar2D
+{
+public:
+	double dist, theta;
+
+public:
+	CPointPolar2D(double d = 0, double t = 0) : dist(.0), theta(.0) { dist = d; theta = t; };
+	virtual ~CPointPolar2D() {};
+};
+#endif

laser/Point3D.cpp

@@ -0,0 +1,34 @@
+
+#include "Point3D.h"
+
+
+CPoint3D::CPoint3D()
+{
+	x = y = z = 0;
+}
+
+
+CPoint3D::~CPoint3D()
+{
+
+}
+
+CPoint3D::CPoint3D(double c1, double c2, double c3)
+{
+    x = c1;      y = c2;      z = c3;
+}
+
+
+
+CPoint3D &CPoint3D::operator=(const CPoint3D& pt) //ÖØÔصȺÅ
+{
+	x = pt.x;   z = pt.z;   y = pt.y;
+    return *this;
+}
+
+void CPoint3D::Translate(double cx, double cy, double cz) //ת»¯
+{
+    x += cx;
+    y += cy;
+    z += cz;
+}

laser/Point3D.h

@@ -0,0 +1,18 @@
+#ifndef __POINT_3D__HH
+#define __POINT_3D__HH
+class CPoint3D
+{
+public:
+	double x, y, z;
+	CPoint3D();
+	~CPoint3D();
+    CPoint3D(double c1, double c2, double c3);
+    CPoint3D& operator=(const CPoint3D& pt);
+	CPoint3D(const CPoint3D& pt)
+	{
+		*this = pt;
+	}
+    void Translate(double cx, double cy, double cz); //ת»¯
+};
+
+#endif

main.cpp

@@ -0,0 +1,45 @@
+#include "mainwindow.h"
+#include "qmessagebox.h"
+#include <QApplication>
+
+#include "Process.h"
+
+
+bool ReadProtoParam(std::string path, ::google::protobuf::Message& param)
+{
+    int fd = open(path.c_str(), O_RDONLY);
+    if (fd == -1) return false;
+    FileInputStream* input = new FileInputStream(fd);
+    bool success = google::protobuf::TextFormat::Parse(input, &param);
+    delete input;
+    close(fd);
+    return success;
+}
+
+
+
+Automatic::stCalibParam		m_laser_calib_param;
+CProcess*					m_pProcess=0;
+
+int main(int argc, char *argv[])
+{
+    QApplication a(argc, argv);
+    MainWindow w;
+    w.show();
+
+    char buf[255]={0};
+    getcwd(buf,255);
+    sprintf(buf,"%s/setting/calib.prototxt",buf);
+
+    if(!ReadProtoParam(buf,m_laser_calib_param))
+    {
+        QMessageBox::about(NULL, "Error", "Read proto failed...");
+    }
+    m_pProcess=new CProcess(m_laser_calib_param,&w);
+    if(m_pProcess->Init()==false)
+    {
+        QMessageBox::about(NULL, "Error", m_pProcess->LastError().c_str());
+    }
+
+    return a.exec();
+}

mainwindow.cpp

@@ -0,0 +1,63 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+
+
+MainWindow::MainWindow(QWidget *parent) :
+    QMainWindow(parent),
+    ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+    setWindowIcon(QIcon("./Resource/log.jpg"));
+
+    m_listbox=new QListWidget(this);//创建一个列表框对象
+    m_listbox->setGeometry(10,10,150,150);//设置列表框的位置和大小
+    m_listbox->insertItem(0,"Item 1");//向列表框中插入条目
+    m_listbox->insertItem(1,"Item 2");
+
+
+
+
+}
+
+MainWindow::~MainWindow()
+{
+    delete ui;
+}
+
+void MainWindow::PLCSlot(uint16_t* data,int size)
+{
+    printf("plc slots \n");
+    static unsigned short last_data[255]={0};
+    for(int i=0;i<size;++i)
+    {
+        if(data[i]!=last_data[i])
+        {
+            char buf[255]={0};
+            sprintf(buf,"P[%d] %d ---> %d",i,last_data[i],data[i]);
+            m_listbox->insertItem(m_listbox->count(),buf);
+            m_listbox->scrollToBottom();
+        }
+    }
+}
+#include "qmessagebox.h"
+#include "src/pathcreator.h"
+void MainWindow::on_m_test_btn_clicked()
+{
+    PathCreator pathCreator;
+    pathCreator.CreateDatePath("/home/zx/data");
+
+}
+
+#include "Process.h"
+extern CProcess* m_pProcess;
+void MainWindow::on_m_add_clicked()
+{
+
+    m_pProcess->PushTask(0x03,1,false);
+
+    /*int static k=3;
+    char buf[255]={0};
+    sprintf(buf,"Item %d",k++);
+    m_listbox->insertItem(m_listbox->count(),buf);
+    m_listbox->scrollToBottom();*/
+}

mainwindow.h

@@ -0,0 +1,35 @@
+#ifndef MAINWINDOW_H
+#define MAINWINDOW_H
+
+#include <QMainWindow>
+#include <QListWidget>
+#include <QPushButton>
+
+namespace Ui {
+class MainWindow;
+}
+
+class MainWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    explicit MainWindow(QWidget *parent = 0);
+    ~MainWindow();
+
+private slots:
+    void on_m_test_btn_clicked();
+    void on_m_add_clicked();
+
+private slots:
+    void PLCSlot(uint16_t* data,int size);
+
+private:
+    QPushButton* m_test_button;
+    QListWidget* m_listbox;
+
+private:
+    Ui::MainWindow *ui;
+};
+
+#endif // MAINWINDOW_H

mainwindow.ui

@@ -0,0 +1,69 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>869</width>
+    <height>474</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainWindow</string>
+  </property>
+  <widget class="QWidget" name="centralWidget">
+   <widget class="QPushButton" name="m_test_btn">
+    <property name="geometry">
+     <rect>
+      <x>350</x>
+      <y>40</y>
+      <width>89</width>
+      <height>51</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>get l r t b</string>
+    </property>
+   </widget>
+   <widget class="QPushButton" name="m_add">
+    <property name="geometry">
+     <rect>
+      <x>350</x>
+      <y>110</y>
+      <width>91</width>
+      <height>25</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>add</string>
+    </property>
+   </widget>
+   <zorder>m_add</zorder>
+   <zorder>m_test_btn</zorder>
+  </widget>
+  <widget class="QMenuBar" name="menuBar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>869</width>
+     <height>22</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QToolBar" name="mainToolBar">
+   <attribute name="toolBarArea">
+    <enum>TopToolBarArea</enum>
+   </attribute>
+   <attribute name="toolBarBreak">
+    <bool>false</bool>
+   </attribute>
+  </widget>
+  <widget class="QStatusBar" name="statusBar"/>
+ </widget>
+ <layoutdefault spacing="6" margin="11"/>
+ <resources/>
+ <connections/>
+</ui>

modbus/LibmodbusWrapper.cpp

@@ -0,0 +1,115 @@
+
+#include "LibmodbusWrapper.h"
+
+namespace modbus
+{
+    CLibmodbusWrapper::CLibmodbusWrapper()
+        :_ctx(NULL)
+        , _ip("")
+        ,_port(-1)
+        ,_slave_id(-1)
+    {
+    }
+
+
+    CLibmodbusWrapper::~CLibmodbusWrapper()
+    {
+        deinitialize();
+    }
+
+    void CLibmodbusWrapper::deinitialize()
+    {
+        if (_ctx)
+        {
+            modbus_close(_ctx);
+            modbus_free(_ctx);
+            _ctx = NULL;
+            _ip = "";
+            _port = -1;
+            _slave_id = -1;
+        }
+    }
+    int CLibmodbusWrapper::initialize(const char *ip, int port, int slave_id)
+    {
+        int rc = 0;
+
+        this->deinitialize();
+
+        _ctx = modbus_new_tcp(ip, port);
+        if (_ctx == NULL)
+        {
+            //创建失败
+            fprintf(stderr, "CLibmodbusWrapper: Unable to allocate libmodbus context\n");
+            return -1;
+        }
+
+        // 设置调试模式
+        modbus_set_debug(_ctx, FALSE);
+
+        // 设置连接的slave号，此处为1
+        rc = modbus_set_slave(_ctx, slave_id);
+        if (rc == -1)
+        {
+            fprintf(stderr, "CLibmodbusWrapper: Invalid slave ID\n");
+            modbus_free(_ctx);
+            _ctx = NULL;
+            return -1;
+        }
+
+        rc = modbus_connect(_ctx);
+        if (rc == -1)
+        {
+            fprintf(stderr, "CLibmodbusWrapper: Connection failed: %s\n", modbus_strerror(errno));
+            modbus_free(_ctx);
+            _ctx = NULL;
+            return -1;
+        }
+
+        _ip = ip;
+        _port = port;
+        _slave_id = slave_id;
+
+        return 0;
+    }
+
+    int CLibmodbusWrapper::read_registers(int addr, int nb, uint16_t *dest)
+    {
+        if (modbus_read_registers(_ctx, addr, nb, dest) == -1)
+        {
+            fprintf(stderr, "CLibmodbusWrapper: Read registers failed: %s\n", modbus_strerror(errno));
+            return -1;
+        }
+        return 0;
+    }
+
+    int CLibmodbusWrapper::write_registers(int addr, int nb, uint16_t *dest)
+    {
+        if (modbus_write_registers(_ctx, addr, nb, dest) == -1)
+        {
+            fprintf(stderr, "CLibmodbusWrapper: Write registers failed: %s\n", modbus_strerror(errno));
+            return -1;
+        }
+        return 0;
+    }
+
+
+    int CLibmodbusWrapper::read_register(int addr, uint16_t *dest)
+    {
+        if (modbus_read_registers(_ctx, addr, 1, dest) == -1)
+        {
+            fprintf(stderr, "CLibmodbusWrapper: Read registers failed: %s\n", modbus_strerror(errno));
+            return -1;
+        }
+        return 0;
+    }
+
+    int CLibmodbusWrapper::write_register(int addr, uint16_t *dest)
+    {
+        if (modbus_write_registers(_ctx, addr, 1, dest) == -1)
+        {
+            fprintf(stderr, "CLibmodbusWrapper: Write registers failed: %s\n", modbus_strerror(errno));
+            return -1;
+        }
+        return 0;
+    }
+}

modbus/LibmodbusWrapper.h

@@ -0,0 +1,36 @@
+#pragma once
+#include <stdio.h>
+#include <errno.h>
+#include <string>
+#include "modbus.h"
+
+namespace modbus
+{
+    class CLibmodbusWrapper
+    {
+    public:
+        CLibmodbusWrapper();
+        virtual ~CLibmodbusWrapper();
+        int initialize(const char *ip, int port, int slave_id);
+        void deinitialize();
+
+    public:
+        int read_registers(int addr, int nb, uint16_t *dest);
+        int write_registers(int addr, int nb, uint16_t *dest);
+
+        int read_register(int addr, uint16_t *dest);
+        int write_register(int addr, uint16_t *dest);
+
+        inline bool is_connected() { return (0 == _ctx) ? false : true; };
+        inline std::string getIP() { return _ip; };
+        inline int getPort() { return _port; };
+        inline int getSlave() { return _slave_id; };
+
+    private:
+        modbus_t* _ctx;
+    private:
+        std::string _ip;
+        int _port;
+        int _slave_id;
+    };
+};

modbus/PLCMonitor.cpp

@@ -0,0 +1,509 @@
+
+#include "PLCMonitor.h"
+#include <unistd.h>
+#include "../common.h"
+
+namespace modbus
+{
+#ifndef WIN32
+#define Sleep(T)  usleep((T)*1000)
+#endif
+    const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_READY = 1000*1000;		//完成信号到心跳延时
+    const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_PINGPANG = 100*1000;
+    const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_START = 300*1000;
+    const clock_t CPLCMonitor::PLC_LASER_TIMEOUT_MAXMEASURE = 60000*1000;
+
+	CPLCMonitor::CPLCMonitor(void* pOwnerObject)
+        :_monitoring(false)
+		, _heartbeat_write_clock(clock())
+		, m_callback(0)
+		,m_pointer(0)
+        ,m_PlcDataCallback(0)
+        ,m_thread_read(0)
+    {
+        memset(&_value, 0, sizeof(_value));
+    }
+
+    CPLCMonitor::~CPLCMonitor()
+    {
+		_monitoring = false;
+		if (m_thread_read)
+		{
+            if(m_thread_read->joinable())
+                m_thread_read->join();
+            m_thread_read = 0;
+		}
+    }
+
+    int CPLCMonitor::connect(const char *ip, int port, int slave_id)
+    {
+        cs_pcl.lock();
+        if (-1 == dev.initialize(ip, port, slave_id))
+        {
+            cs_pcl.unlock();
+            return -1;
+        }
+        cs_pcl.unlock();
+
+        _monitoring = true;
+        if (FALSE == Start())
+        {
+            dev.deinitialize();
+            return -1;
+        }
+        m_thread_read = new std::thread(thread_monitor, this);
+		m_strIP = ip;
+		m_port = port;
+		m_slave_id = slave_id;
+
+        return 0;
+    }
+
+    void CPLCMonitor::set_callback(CommandCallback func, PLCMonitorCallback monitorCallback,void* pointer)
+	{
+		m_callback = func;
+        m_PlcDataCallback=monitorCallback;
+		m_pointer = pointer;
+	}
+
+	int CPLCMonitor::reconnect()
+	{
+        cs_pcl.lock();
+		if (-1 == dev.initialize(m_strIP.c_str(), m_port, m_slave_id))
+		{
+            cs_pcl.unlock();
+			return -1;
+		}
+        cs_pcl.unlock();
+		return 0;
+	}
+
+    void CPLCMonitor::disconnect()
+    {
+        _monitoring = false;
+        Join(1000);
+
+
+        cs_pcl.lock();
+        dev.deinitialize();
+        cs_pcl.unlock();
+    }
+
+    int CPLCMonitor::read_registers(int addr, int nb, uint16_t *dest)
+    {
+        cs_pcl.lock();
+        int rc = dev.read_registers(addr, nb, dest);
+        cs_pcl.unlock();
+		if (0 != rc)
+		{
+			reconnect();//端开，重连
+		}
+        return rc;
+    }
+
+    int CPLCMonitor::write_registers(int addr, int nb, uint16_t *dest)
+    {
+        cs_pcl.lock();
+        int rc = dev.write_registers(addr, nb, dest);
+        cs_pcl.unlock();
+
+        return rc;
+    }
+
+    int CPLCMonitor::read_register(int addr, uint16_t *dest)
+    {
+        cs_pcl.lock();
+        int rc = dev.read_register(addr, dest);
+        cs_pcl.unlock();
+        return rc;
+    }
+
+    int CPLCMonitor::write_register(int addr, uint16_t *dest)
+    {
+        cs_pcl.lock();
+        int rc = dev.write_register(addr, dest);
+        cs_pcl.unlock();
+
+        return rc;
+    }
+
+    int CPLCMonitor::clear_laserstatus_register(uint16_t value)
+    {
+        cs_pcl.lock();
+        int rc = dev.write_register(REG_WHISKBOOMLASER_STATUS, &value);
+        cs_pcl.unlock();
+        _heartbeat_write_clock = clock();
+
+        return rc;
+    }
+
+    int CPLCMonitor::write_laserstatus_register(uint16_t value)
+    {
+        uint16_t dest = 0xff;
+        cs_pcl.lock();
+
+        int rc = dev.read_register(REG_WHISKBOOMLASER_STATUS, &dest);
+        if (PLC_LASER_ERROR != dest)
+        {
+            rc = dev.write_register(REG_WHISKBOOMLASER_STATUS, &value);
+        }
+        cs_pcl.unlock();
+
+        _heartbeat_write_clock = clock();
+
+		if (0 != rc)
+		{
+			reconnect();//端开，重连
+		}
+
+        return rc;
+    }
+
+    int CPLCMonitor::write_laserresult_register(int addr,uint16_t *pvalue)
+    {
+        uint16_t dest = 0xff;
+        cs_pcl.lock();
+
+		int rc= dev.write_registers(addr, 10, pvalue);
+        /*int rc = dev.read_register(REG_WHISKBOOMLASER_STATUS, &dest);
+        if (PLC_LASER_ERROR != dest)
+        {
+            rc = dev.write_registers(REG_WHISKBOOMLASER_STATUS, 7, pvalue);
+        }*/
+        cs_pcl.unlock();
+
+        _heartbeat_write_clock = clock();
+
+        return rc;
+    }
+
+	void CPLCMonitor::MeasureComplete(bool bOK)
+	{
+		//_measure_finished = TRUE;
+		if (!bOK)
+		{
+			write_laserstatus_register(PLC_LASER_FINISH_FAILED);
+		}
+		else
+		{
+			write_laserstatus_register(PLC_LASER_FINISH_OK);
+		}
+	}
+
+    bool CPLCMonitor::setMeasureResult(int addr, struct whiskboom_laser_value * p)
+    {
+		if (p == 0||addr<0)	//失败，没有摆扫结果，硬件错误；
+		{
+			return false;
+		}
+		else
+		{
+			uint16_t response[10] = { 0 };
+			memset(response, 0, sizeof(uint16_t) * 10);
+			if (p->corrected)
+			{
+				response[6] = eLaser_data_ok;
+			}
+			else {
+				response[6] = eLaser_data_failed;
+			}
+			response[0] = (p->x);
+			response[1] = p->y;
+			response[2] = (int(p->a) % 18000);
+			response[3] = p->l;
+			response[4] = p->w;
+			response[5] = p->h;
+			response[7] = p->l;			////轴距
+			write_laserresult_register(addr,(uint16_t *)response);
+			return true;
+		}
+    }
+
+	void CPLCMonitor::Monitoring()
+	{
+		const int SIGNAL_NUM = 64;
+		const int SIGNAL_ADDR = 0;
+		static uint16_t value[SIGNAL_NUM] = { 0 };
+        static uint16_t last_value[SIGNAL_NUM] = { 0 };
+		int rc = read_registers(SIGNAL_ADDR, SIGNAL_NUM, last_value);
+		
+        LOG(INFO) << "\tStart signal monitoring thread";
+		while (_monitoring)
+		{
+			memset(value, 0, SIGNAL_NUM * sizeof(uint16_t));
+			int rc = read_registers(SIGNAL_ADDR, SIGNAL_NUM, value);
+			
+			if (rc == 0)
+			{
+				for (int i = REG_PARKSTATUS; i < SIGNAL_NUM; ++i)
+				{
+					if (value[i] != last_value[i])
+					{
+                        if(m_PlcDataCallback && m_pointer)
+                            m_PlcDataCallback(value,SIGNAL_NUM,m_pointer);
+                        break;
+					}
+				}
+				memcpy(last_value, value, sizeof(uint16_t)*SIGNAL_NUM);
+			}
+
+            Sleep(200);
+		}
+        LOG(INFO) << "\tStop signal monitor thread!";
+	}
+
+    void CPLCMonitor::thread_monitor(void* lp)
+	{
+		CPLCMonitor* plc = (CPLCMonitor*)lp;
+		plc->Monitoring();
+	}
+
+    void CPLCMonitor::Run()
+    {
+        uint16_t value[2] = { 0xffff,0xffff };
+        uint16_t parking_status = 0xff;
+        uint16_t response[7] = { 0 };
+        int rc = -1;
+        clock_t current_clock;
+
+        uint16_t finite_state_machines = PLC_LASER_READY;
+        write_laserstatus_register(finite_state_machines);
+        uint16_t las_parkstatus = 0;
+
+        while (_monitoring)
+        {
+            memset(value, 0xff, sizeof(value));
+
+            rc = read_registers(REG_PARKSTATUS, 2, value);
+            if (-1 == rc)
+            {
+                fprintf(stderr, "CPLCMonitor: read regiser failed\n");
+            }
+
+            parking_status = value[0];
+            finite_state_machines = value[1];
+
+
+			//上次状态为：开始测量，测量中
+			if (PLC_LASER_START == finite_state_machines
+				|| PLC_LASER_WORKING == finite_state_machines)
+			{
+				if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_READY)
+				{
+					write_laserstatus_register(PLC_LASER_PING);
+					finite_state_machines = PLC_LASER_PING;
+				}
+			}
+
+
+            //PLC 我要停车等待状态
+            if (PLC_PARKING_WAIT == parking_status|| 2 == parking_status)
+            {
+                current_clock = clock();
+
+                //激光上次状态为：测量完成成功或者测量完成失败的状态
+                if (PLC_LASER_FINISH_OK == finite_state_machines
+                    || PLC_LASER_FINISH_FAILED == finite_state_machines)
+                {
+                    //超过10秒，开始返回就绪状态
+                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_READY)
+                    {
+						//先清除数据
+						if (PLC_LASER_FINISH_FAILED == finite_state_machines)
+						{
+							/*memset(response, 0, 7 * sizeof(uint16_t));
+							write_laserresult_register((uint16_t *)response);*/
+						}
+                        write_laserstatus_register(PLC_LASER_READY);
+						finite_state_machines = PLC_LASER_READY;
+                    }
+                }
+
+
+                //激光上次状态为：就绪或者心跳状态1，
+                else if (PLC_LASER_READY == finite_state_machines
+                    || PLC_LASER_PONG == finite_state_machines)
+                {
+                    //超过3秒，开始返回心跳状态2
+                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_PINGPANG)
+                    {
+                        write_laserstatus_register(PLC_LASER_PING);
+						finite_state_machines = PLC_LASER_PING;
+                    }
+                }
+                //激光上次状态为：心跳状态2
+                else if (PLC_LASER_PING == finite_state_machines)
+                {
+                    //超过3秒，开始返回心跳状态1
+                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_PINGPANG)
+                    {
+                        write_laserstatus_register(PLC_LASER_PONG);
+						finite_state_machines = PLC_LASER_PONG;
+                    }
+                }
+                //激光上次状态为：测量开始，测量中）
+                else if (PLC_LASER_START == finite_state_machines
+                    || PLC_LASER_WORKING == finite_state_machines)
+                {
+                    //此时激光可能在测量状态中,让激光测量停止，结果：
+                    //    1. 如果未在测量状态，激光接受状态不会有任何返回值，
+                    //    2. 如果在测量状态会有测量失败信息须处理
+                    LOG(ERROR) << "\t 测量中开始信号终止";
+                   /* cs_cmd.Lock();
+					for (std::vector<CLaserNet*>::iterator it = _cmdlistLaserPointer.begin(); it != _cmdlistLaserPointer.end(); ++it)
+						(*it)->Stop();
+					cs_cmd.Unlock();*/
+
+                    write_laserstatus_register(PLC_LASER_READY);
+					finite_state_machines = PLC_LASER_READY;
+                }
+                //不可修复错误，等系统重启
+                else if (PLC_LASER_ERROR == finite_state_machines)
+                {
+                    fprintf(stderr, "\n\n\n\n\n\n不可修复错误，等系统重启\n\n\n\n\n\n");
+                }
+                else
+                {
+                    fprintf(stderr, "\n\n\n\n\n\n&&&&&&&&&!!!!!!!!!!!!!@@@@@@@@@@@@@@$$$$$$$$$$$$$$$$$$$完全不可能出现的状态\n\n\n\n\n\n");
+                }
+                las_parkstatus = PLC_PARKING_WAIT;
+
+            }
+            //PLC 我要停车测量开始
+            else if (parking_status>0)
+            {
+                //激光上次状态为：就绪，心跳状态1，心跳状态2
+                if(PLC_LASER_READY == finite_state_machines
+                        || PLC_LASER_PING == finite_state_machines
+                        || PLC_LASER_PONG == finite_state_machines
+                        || PLC_LASER_FINISH_OK == finite_state_machines
+                        || PLC_LASER_FINISH_FAILED == finite_state_machines)
+                {
+					/////  测量
+                    if (PLC_PARKING_WAIT == las_parkstatus && parking_status==1)
+                    {
+						//置测量结果为未完成状态
+                        _measure_finished = FALSE;
+						
+                        //测量开始
+						if (m_callback)
+						{
+							//char plate = parking_status;
+							
+							uint16_t param = 0x03;
+							m_callback(true, param, m_pointer, parking_status);
+						}
+
+                        //返回测量开始状态
+                        write_laserstatus_register(PLC_LASER_START);
+						finite_state_machines = PLC_LASER_START;
+                    }
+					////// 电子围栏测量
+					/*else if ((PLC_LASER_READY == finite_state_machines
+						|| PLC_LASER_PING == finite_state_machines
+						|| PLC_LASER_PONG == finite_state_machines)
+						&&parking_status == 2)
+					{
+						//置测量结果为未完成状态
+						_measure_finished = FALSE;
+
+						//测量开始
+						if (m_callback)
+						{
+							uint16_t param = 0x03;
+							m_callback(true, param, m_pointer, parking_status);
+						}
+
+						//返回测量开始状态
+						write_laserstatus_register(PLC_LASER_START);
+						finite_state_machines = PLC_LASER_START;
+					}*/
+                }
+
+				//上次状态为：测量完成成功，测量完成错误，
+				if (PLC_LASER_FINISH_OK == finite_state_machines
+					|| PLC_LASER_FINISH_FAILED == finite_state_machines)
+				{
+					if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_PINGPANG)
+					{
+						write_laserstatus_register(PLC_LASER_PING);
+						finite_state_machines = PLC_LASER_PING;
+					}
+				}
+
+                //激光上次状态为:开始测量
+                else if (PLC_LASER_START == finite_state_machines)
+                {
+					current_clock = clock();  
+                    //超过200毫秒，返回测量中
+                    if (current_clock - _heartbeat_write_clock >= PLC_LASER_TIMEOUT_START)
+                    {
+                        write_laserstatus_register(PLC_LASER_WORKING);
+						finite_state_machines = PLC_LASER_WORKING;
+                    }
+                }
+                else if (PLC_LASER_WORKING == finite_state_machines)
+                {
+                    if (TRUE == _measure_finished)
+                    {
+						//测量完成
+                       /* memset(response, 0, sizeof(uint16_t) * 7);
+                        if (_value.corrected)
+                        {
+                            response[0] = PLC_LASER_FINISH_OK;
+                        }
+                        else {
+                            response[0] = PLC_LASER_FINISH_FAILED;
+                        }
+						response[1] = _value.x;
+						response[2] = _value.y;
+						response[3] = (int(_value.a) % 18000);
+						response[4] = _value.l;
+						response[5] = _value.w;
+						response[6] = _value.h;
+                        write_laserresult_register((uint16_t *)response);*/
+						write_laserstatus_register(PLC_LASER_FINISH_FAILED);
+						finite_state_machines = PLC_LASER_FINISH_FAILED;
+                    }
+                    else {
+                        current_clock = clock();
+                        if (current_clock - _heartbeat_write_clock > PLC_LASER_TIMEOUT_MAXMEASURE) //10分钟还没返回测量结果， 说明系统可能出错了
+                        {
+                            write_laserstatus_register(PLC_LASER_ERROR);
+							finite_state_machines = PLC_LASER_ERROR;
+                        }
+                    }
+                }
+                else if (PLC_LASER_ERROR == finite_state_machines)
+                {
+
+                    fprintf(stderr, "\n\n\n\n\n\n不可修复错误，等系统重启\n\n\n\n\n\n");
+                }
+                else
+                {
+                    fprintf(stderr, "\n\n\n\n\n\n&&&&&&&&&!!!!!!!!!!!!!@@@@@@@@@@@@@@$$$$$$$$$$$$$$$$$$$完全不可能出现的状态\n\n\n\n\n\n");
+                }
+
+                las_parkstatus = PCL_PARKING_REQUEST;
+            }
+            else
+            {
+				if (PCL_PARKING_ERROR != las_parkstatus)
+                {
+                    LOG(ERROR)<<("\t严重警告:PLC我要停车状态异常，不可修复错误，等系统重启");
+					//停止所有激光
+					if (m_callback)
+					{
+						char laser = 0xFF;
+						m_callback(false, uint16_t(laser), m_pointer, parking_status);
+					}
+                    write_laserstatus_register(PLC_LASER_READY);
+					finite_state_machines = PLC_LASER_READY;
+                }
+                Sleep(200);
+				las_parkstatus = PCL_PARKING_ERROR;
+            }
+            Sleep(50);
+        }
+    }
+}

modbus/PLCMonitor.h

@@ -0,0 +1,139 @@
+#ifndef __PLC_MONITOR__HH_
+#define __PLC_MONITOR__HH_
+#include "LibmodbusWrapper.h"
+#include "Runnable.h"
+#include <string.h>
+#include <mutex>
+#include <vector>
+#include <time.h>
+#include <QObject>
+
+struct whiskboom_laser_value
+{
+    float x;
+    float y;
+    float a;
+    float l;
+    float w;
+    float h;
+    int corrected;
+};
+
+#define WM_LOG_MSG						WM_USER+1001
+#define WM_MONITOR_MSG					WM_USER+1002
+
+namespace modbus
+{
+#define REG_PARKSTATUS (0)
+#define REG_WHISKBOOMLASER_STATUS (1)
+#define REG_WHISKBOOMLASER_CENTERX (2)
+#define REG_WHISKBOOMLASER_CENTERY (3)
+#define REG_WHISKBOOMLASER_ANGLEA (4)
+#define REG_WHISKBOOMLASER_LENGTH (5)
+#define REG_WHISKBOOMLASER_WIDTH (6)
+#define REG_WHISKBOOMLASER_HEIGHT (7)
+
+    enum PLC_PARKING_STATUS
+    {
+        PLC_PARKING_WAIT = 0,
+        PCL_PARKING_REQUEST,
+        PCL_PARKING_ERROR
+    };
+
+	enum LASER_DATA_CORRECT
+	{
+		eLaser_data_init=0,
+		eLaser_data_ok,
+		eLaser_data_failed
+	};
+
+    enum PCL_LASER_STATUS
+    {
+        PLC_LASER_READY = 0,
+        PLC_LASER_START,
+        PLC_LASER_WORKING,
+        PLC_LASER_FINISH_OK,
+        PLC_LASER_FINISH_FAILED,
+        PLC_LASER_ERROR,
+        PLC_LASER_PING=254,
+        PLC_LASER_PONG
+    };
+
+
+	typedef void(*CommandCallback)(bool On, uint16_t param,void* pointer,int action_type);
+    typedef void(*PLCMonitorCallback)(uint16_t* data,int size,void* pointer);
+
+    class CPLCMonitor :
+        public CRunnable
+    {
+
+    public:
+        CPLCMonitor(void* pMainObject=NULL);
+        ~CPLCMonitor();
+    public:
+        int connect(const char *ip, int port, int slave_id);
+        void disconnect();
+		int reconnect();
+        void set_callback(CommandCallback func, PLCMonitorCallback monitorCallback,void* pointer);
+    private:
+        std::mutex cs_cmd;
+
+    private:
+        bool _measure_finished;
+        struct whiskboom_laser_value _value;
+    public:
+		bool setMeasureResult(int addr, struct whiskboom_laser_value * value = NULL);
+		void MeasureComplete(bool bOK=true);
+	public:
+		int write_laserstatus_register(uint16_t value);
+	protected:
+		CommandCallback		m_callback;
+		void*				m_pointer;
+        PLCMonitorCallback  m_PlcDataCallback;
+
+    public:
+        bool isMonitor() { return _monitoring == true; };
+    private:
+        bool _monitoring;
+
+    private:
+        CLibmodbusWrapper dev;
+    
+    private:
+        clock_t _heartbeat_write_clock;
+
+    private:
+        int read_registers(int addr, int nb, uint16_t *dest);
+        int write_registers(int addr, int nb, uint16_t *dest);
+
+        int read_register(int addr, uint16_t *dest);
+        int write_register(int addr, uint16_t *dest);
+
+        int clear_laserstatus_register(uint16_t value);
+        int write_laserresult_register(int addr ,uint16_t *pvalue);
+
+    private:
+        static const clock_t PLC_LASER_TIMEOUT_READY;
+		static const clock_t PLC_LASER_TIMEOUT_PINGPANG;
+		static const clock_t PLC_LASER_TIMEOUT_START;
+		static const clock_t PLC_LASER_TIMEOUT_MAXMEASURE;
+
+    public:
+        virtual void Run();
+	private:
+		void Monitoring();
+        static void thread_monitor(void* lp);
+        void*						m_logWnd;
+        void*						m_laser_data_wnd;
+        std::thread*				m_thread_read;
+    private:
+        std::mutex cs_pcl;
+
+		std::string				m_strIP;
+		int						m_port;
+		int						m_slave_id;
+
+    };
+}
+
+#endif

modbus/Runnable.cpp

@@ -0,0 +1,39 @@
+
+#include "Runnable.h"
+
+
+CRunnable::CRunnable(const std::string threadName/*="noNamed"*/)
+    :status(STOPPED)
+    ,m_pthread(0)
+{
+}
+
+bool CRunnable::Start(bool suspend/*=false*/)
+{
+    m_pthread = new std::thread(&CRunnable::staticThreadFunc,this);
+    return (m_pthread!=0);
+}
+
+int CRunnable::Join(int timeout /*=-1*/)
+{
+    if(m_pthread)
+    {
+        if(m_pthread->joinable())
+        {
+            m_pthread->join();
+            return 0;
+        }
+    }
+    else
+    {
+        return -1;
+    }
+}
+
+
+
+/*static*/ void CRunnable::staticThreadFunc(void *arg)
+{
+    CRunnable* runnable = (CRunnable*)arg;
+    runnable->Run();
+}

modbus/Runnable.h

@@ -0,0 +1,46 @@
+#ifndef __RUNNABLE__HH
+#define __RUNNABLE__HH
+
+#include <thread>
+#include <cstdint>
+#include <cstdio>
+#include <cstdlib>
+#include <string>
+#include <iostream>
+#include <exception>
+
+
+
+class CRunnable
+{
+private:
+    enum THREAD_STATUS { RUNNING, SUSPENDED, STOPPED };
+
+public:
+    CRunnable(const std::string threadName = "noNamed");
+    ~CRunnable() {}
+
+    bool Start(bool suspend = false);
+    int Join(int timeout = -1);
+
+
+public:
+    virtual void Run() = 0;
+
+
+public:
+    bool isRunning() { return (RUNNING == status); }
+    bool isSuspended() { return (SUSPENDED == status); }
+    bool isStopped() { return (STOPPED == status); }
+    bool isAlive() { return (STOPPED != status); }
+    THREAD_STATUS getStatus() { return status; };
+private:
+    void setStatus(THREAD_STATUS status) { this->status = status; }
+    static void  staticThreadFunc(void* arg);
+
+protected:
+    std::thread*        m_pthread;
+    volatile THREAD_STATUS status;
+};
+
+#endif

script/CalibParam.proto

@@ -0,0 +1,91 @@
+
+syntax="proto2";
+package Automatic;
+
+message stValidRegion{
+    optional float          min_x=1 [default=0];
+    optional float          max_x=2 [default=12000];
+    optional float          min_y=3 [default=0];
+    optional float          max_y=4 [default=5000];
+    optional float          min_z=5 [default=-1000];
+    optional float          max_z=6 [default=3000];
+}
+
+message stCameraPosition{
+    optional float          pos_x=1 [default=0.5];
+    optional float          pos_y=2 [default=0.5];
+    optional float          pos_z=3 [default=2];
+    optional float          focus_x=4 [default=0.5];
+    optional float          focus_y=5 [default=0.5];
+    optional float          focus_z=6 [default=0.5];
+    optional float          direct_x=7 [default=0];
+    optional float          direct_y=8 [default=1];
+    optional float          direct_z=9 [default=0];
+}
+
+message stAxis3D{
+    required double         x=1;
+    required double         y=2;
+    required double         z=3;
+}
+
+
+message stPoint{
+    required float x=1;
+    required float y=2;
+}
+
+message stPlateArea{
+    repeated stPoint        point=1;
+	required int64			plc_addr=2;
+}
+
+message stPLCParam
+{
+    optional string         plc_ip=1;
+    optional int64          plc_port=2;
+}
+
+message stLaserCalibParam
+{
+    optional string         laser_ip=1;
+    optional int64          laser_port=2;
+    optional int64          laser_port_remote=3;
+    optional double         mat_r00=4 [default=1.0];
+    optional double         mat_r01=5 [default=1.0];
+    optional double         mat_r02=6 [default=1.0];
+    optional double         mat_r03=7 [default=1.0];
+    optional double         mat_r10=8 [default=1.0];
+    optional double         mat_r11=9 [default=1.0];
+    optional double         mat_r12=10 [default=1.0];
+    optional double         mat_r13=11 [default=1.0];
+    optional double         mat_r20=12 [default=1.0];
+    optional double         mat_r21=13 [default=1.0];
+    optional double         mat_r22=14 [default=1.0];
+    optional double         mat_r23=15 [default=1.0];
+    optional double 		axis_x_theta=16;
+	optional double 		axis_y_theta=17;
+	optional double 		axis_z_theta=18;
+	optional double 		translation_x=19;
+	optional double 		translation_y=20;
+	optional double 		translation_z=21;
+	
+    optional double         install_angle=22 [default=0.0];
+    optional bool           scan_direction=23 [default=true];
+	optional string 		sn=24;
+	optional int64 			frame_num=25 [default=3000];
+	optional string 		type=26 [default=""];
+}
+
+message stCalibParam
+{
+    optional stValidRegion          valid_region=1;
+    repeated stCameraPosition       camera_position=2;
+    repeated stLaserCalibParam      laser=3;
+    optional stPLCParam             plc=4;
+    required string                 local_ip=5;
+    optional  string                debug_file=6 [default=""];
+    repeated stPlateArea            plate_area=7;
+    optional string                 project_dir=8 [default="D:\\data\\Projects"];
+	optional bool 					is_calib=9 [default=true];
+}

script/Probuf.sh

@@ -0,0 +1 @@
+protoc -I=./ --cpp_out=../src CalibParam.proto

src/StdCondition.cpp

@@ -0,0 +1,35 @@
+#include "StdCondition.h"
+
+StdCondition::StdCondition():m_value(false)
+{
+}
+
+StdCondition::StdCondition(bool init):m_value(init)
+{
+}
+
+StdCondition::~StdCondition()
+{
+}
+bool StdCondition::isTrue(StdCondition* scd)
+{
+	if (scd == 0)return false;
+	return scd->m_value;
+}
+
+void StdCondition::Wait()
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_cv.wait(loc,std::bind(isTrue,this));
+}
+bool StdCondition::WaitFor(unsigned int millisecond)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	return m_cv.wait_for(loc, std::chrono::milliseconds(millisecond), std::bind(isTrue, this));
+}
+void StdCondition::Notify(bool istrue)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_value = istrue;
+	m_cv.notify_all();
+}

src/StdCondition.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <thread>  
+#include <mutex>  
+#include <chrono>  
+#include <condition_variable>
+
+class StdCondition
+{
+public:
+	StdCondition();
+	StdCondition(bool init);
+	~StdCondition();
+	void Wait();
+	bool WaitFor(unsigned int millisecond);
+	void Notify(bool istrue);
+
+protected:
+	static bool isTrue(StdCondition* scd);
+protected:
+	bool m_value;
+	std::mutex m_mutex;
+	std::condition_variable m_cv;
+};
+

src/measuretask.cpp

@@ -0,0 +1,410 @@
+#include "measuretask.h"
+#include "pathcreator.h"
+std::string Error_string(ERROR_CODE code)
+{
+    std::string StrError;
+    switch (code)
+    {
+    case eSucc:StrError = "测量正确";
+        break;
+    case eArea:StrError = "跨车位区域或者车位区域无车";
+        break;
+    case eLidar:
+        StrError = "两雷达数据不重合";
+        break;
+    case eLimitL:
+        StrError = "左边界超界";
+        break;
+    case eLimitR:
+        StrError = "右边界超界";
+        break;
+    case eLimitT:
+        StrError = "上边界超界";
+        break;
+    case eLimitB:
+        StrError = "下边界超界";
+        break;
+    case eNoCar:
+        StrError = "测量失败， 场景内未找到车辆";
+        break;
+    case eMulCar:
+        StrError = "车位区域内存在多辆车";
+        break;
+    case eDistance:
+        StrError = "车辆间距离过进，机械臂无法下降";
+        break;
+    case eCloud:
+        StrError = "雷达断线，无点云数据";
+        break;
+    default:
+        break;
+    }
+    return StrError;
+}
+
+bool RegionInRegion(std::vector<cv::Point2f>& mini_poly, std::vector<cv::Point2f>& large_poly)
+{
+    for (int i = 0; i < mini_poly.size(); ++i)
+    {
+        if (cv::pointPolygonTest(large_poly, mini_poly[i], false) < 0)
+            return false;
+    }
+    return true;
+}
+
+double distance_polys(std::vector<cv::Point2f> poly1, std::vector<cv::Point2f> poly2)
+{
+    std::vector<cv::Point2f>& minpoly = poly1.size() >= poly2.size() ? poly2 : poly1;
+    std::vector<cv::Point2f>& maxpoly = poly1.size() >= poly2.size() ? poly1 : poly2;
+    double mindis = (numeric_limits<double>::max)();
+    for (int i = 0; i < minpoly.size(); ++i)
+    {
+        double dis = -cv::pointPolygonTest(maxpoly, minpoly[i], true);
+        if (dis < mindis)
+        {
+            mindis = dis;
+        }
+    }
+    return mindis;
+}
+
+std::mutex* MeasureTask::g_mutex = new std::mutex();
+
+MeasureTask::MeasureTask(std::vector<CLaser*> lasers, void* locater,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;
+    m_position_id = positon_id;
+    //m_searchPath.m_strProjectPath = (m_calib_param.project_dir()+"/").c_str();
+    m_ptr = 0;
+}
+
+ERROR_CODE MeasureTask::SelectResult(int plate_index, std::vector<CarPosition> results, CarPosition& result)
+{
+    //一两车不考虑区域问题
+    if (results.size() == 1)
+    {
+        result = results[0];
+        return eSucc;
+    }
+
+    LOG(INFO) << "选择号牌机对应车辆，号牌编号：" << plate_index;
+    if (plate_index >= m_calib_param.plate_area_size() || plate_index < 0)
+    {
+        LOG(ERROR) << "当前车位没有车辆,车位ID：" << plate_index;
+        return eArea;
+    }
+
+    std::vector<cv::Point2f> area;
+    for (int i = 0; i < m_calib_param.plate_area(plate_index).point_size(); ++i)
+    {
+        Automatic::stPoint stP = m_calib_param.plate_area(plate_index).point(i);
+        cv::Point2f point;
+        point.x = stP.x();
+        point.y = stP.y();
+        area.push_back(point);
+    }
+    if (area.size() < 3)
+    {
+        LOG(ERROR) << " plate aera points size<3  size=" << area.size();
+        return eArea;
+    }
+    double max_distance = -1.0;
+
+    int num = 0;
+    int id = -1;
+    for (int i = 0; i < results.size(); ++i)
+    {
+        cv::Point2f vtx[4];
+        results[i].rrect.points(vtx);
+        std::vector<cv::Point2f> poly(vtx, vtx + 4);
+        if (RegionInRegion(poly, area))
+        {
+            result = results[i];
+            LOG(INFO) << vtx[0] << " " << vtx[1] << " " << vtx[2] << " " << vtx[3] << " ";
+            num++;
+            id = i;
+        }
+    }
+    if (num == 1)		//找到符合要求车辆，rs[id]
+    {
+        cv::Point2f vtx[4];
+        results[id].rrect.points(vtx);
+        std::vector<cv::Point2f> poly(vtx, vtx + 4);
+        for (int i = 0; i < results.size(); ++i)
+        {
+            if (i != id)
+            {
+                cv::Point2f vtx1[4];
+                results[i].rrect.points(vtx1);
+                std::vector<cv::Point2f> poly1(vtx1, vtx1 + 4);
+                if (distance_polys(poly, poly1) < 400.)
+                    return eDistance;
+            }
+        }
+        return eSucc;
+    }
+    if (num > 1)
+        return eMulCar;
+    return eArea;
+}
+
+ERROR_CODE MeasureTask::Dispatch_posID(std::vector<CarPosition>& results)
+{
+    if (results.size() == 0)
+        return eNoCar;
+
+    if ( m_calib_param.plate_area_size()==0 )
+    {
+        LOG(ERROR) << "\t未配置车位信息，无法标识车位";
+        return eArea;
+    }
+
+    std::vector<std::vector<cv::Point2f>> areas;
+    for (int k = 0; k < m_calib_param.plate_area_size(); ++k)
+    {
+        std::vector<cv::Point2f> area;
+        for (int i = 0; i < m_calib_param.plate_area(k).point_size(); ++i)
+        {
+            Automatic::stPoint stP = m_calib_param.plate_area(k).point(i);
+            cv::Point2f point;
+            point.x = stP.x();
+            point.y = stP.y();
+            area.push_back(point);
+        }
+        areas.push_back(area);
+    }
+
+    double max_distance = -1.0;
+
+    int num = 0;
+    for (int i = 0; i < results.size(); ++i)
+    {
+
+        //判断结果是否合法
+        CarPosition rs = results[i];
+        ERROR_CODE code = rs.error_code;
+
+        if (rs.x < 0 || rs.y < 0)
+        {
+            code = eArea;
+            continue;
+        }
+        //// 分配车位
+        cv::Point2f center = results[i].rrect.center;
+        for (int j = 0; j < areas.size(); ++j)
+        {
+            if (cv::pointPolygonTest(areas[j], center, false) >= 0)
+            {
+                results[i].pos_id = j;
+                LOG(INFO) << "\tCar[" << i << "],center:" << results[i].rrect.center << ",size:" << results[i].rrect.size << ",车位ID:" << j;
+                num++;
+            }
+        }
+        /*cv::Point2f vtx[4];
+        results[i].rrect.points(vtx);
+        std::vector<cv::Point2f> poly(vtx, vtx + 4);
+        for (int j = 0; j < areas.size(); ++j)
+        {
+            if (RegionInRegion(poly, areas[j]))
+            {
+                results[i].pos_id = j;
+                LOG(INFO) << "\tCar[" << i << "],center:" << results[i].rrect.center << ",size:" << results[i].rrect.size << ",车位ID:" << j;
+                num++;
+            }
+        }*/
+        if (results[i].pos_id < 0)
+            continue;
+        float l = 4200;
+        float w = 2630;
+        cv::RotatedRect rect = rs.rrect;
+        rect.center = rs.rrect.center;
+
+        cv::Point2f pt[4];
+        rect.points(pt);
+        float minx = areas[results[i].pos_id][0].x;//m_calib_param.valid_region().min_x();
+        float maxx = areas[results[i].pos_id][3].x;//m_calib_param.valid_region().max_x();
+        float miny = areas[results[i].pos_id][1].y;//m_calib_param.valid_region().min_y();
+        float maxy = areas[results[i].pos_id][0].y;//m_calib_param.valid_region().max_y();
+        for (int i = 0; i < 4; ++i)
+        {
+            cv::Point2f point = pt[i];
+            LOG(INFO) << "error pt " << i << ":" << point.x << "," << point.y;
+            if (point.x < minx)
+            {
+
+                code = eLimitL;
+                //break;
+            }
+            if (point.x > maxx)
+            {
+
+                code = eLimitR;
+                //break;
+            }
+            if (point.y < miny)
+            {
+
+                code = eLimitB;
+                //break;
+            }
+
+        }
+
+        results[i].error_code = code;
+
+        if (code != eSucc)
+        {
+            LOG(INFO) << "\tCar[" << i << "] error,info:"<<Error_string(code);
+            continue;
+        }
+
+    }
+    if (num >= 1)		//找到符合要求车辆，rs[id]
+    {
+        return eSucc;
+    }
+    return eArea;
+}
+
+void MeasureTask::PushPoint(CPoint3D point, void* pointer)
+{
+    MeasureTask* task = reinterpret_cast<MeasureTask*>(pointer);
+    PointT pcl_point;
+    pcl_point.x = point.x;
+    pcl_point.y = point.y;
+    pcl_point.z = point.z;
+
+    float min_x = task->m_calib_param.valid_region().min_x();
+    float max_x = task->m_calib_param.valid_region().max_x();
+    float min_y = task->m_calib_param.valid_region().min_y();
+    float max_y = task->m_calib_param.valid_region().max_y();
+    float min_z = task->m_calib_param.valid_region().min_z();
+    float max_z = task->m_calib_param.valid_region().max_z();
+
+    if (pcl_point.x<min_x || pcl_point.x>max_x || pcl_point.y<min_y ||
+        pcl_point.y>max_y || pcl_point.z<min_z || pcl_point.z>max_z)
+        return;
+
+    std::lock_guard<std::mutex> lk(task->m_mutex);
+    task->m_cloud.push_back(pcl_point);
+
+}
+void  MeasureTask::Main()
+{
+    ////等待雷达空闲资源
+    while (!m_exit)
+    {
+        g_mutex->lock();
+        if (IsLaserReady())
+        {
+            PathCreator pathCreator;
+            pathCreator.CreateDatePath(m_calib_param.project_dir());
+            LOG(INFO) << "\tTask dir:" << pathCreator.GetCurPath();
+            //启动摆扫
+            for (int i = 0; i < m_lasers.size(); ++i)
+            {
+                m_lasers[i]->SetSaveDir(pathCreator.GetCurPath());
+                m_lasers[i]->SetPointCallBack(PushPoint, this);
+                m_lasers[i]->Start();
+            }
+            g_mutex->unlock();
+            break;
+        }
+        g_mutex->unlock();
+        usleep(100*1000);
+    }
+
+    while (!m_exit && !IsLaserReady()) { usleep(1); }	//等待摆扫结束
+
+    std::vector<CarPosition> results;
+    ERROR_CODE code = eSucc;
+    if (m_locater)
+    {
+        if (m_cloud.size() == 0)
+        {
+            code = eCloud;
+        }
+        else if (/*m_locater->Locate(m_cloud.makeShared(), results, strPath.GetBuffer(), code)*/0)
+        {
+            code = Dispatch_posID(results);
+            if (code == eSucc)
+            {
+                /////写入结果到plc
+                for (int i = 0; i < results.size(); ++i)
+                {
+                    CarPosition				rs = results[i];
+                    rs.pos_id = 0;
+                    whiskboom_laser_value value;
+                    //value.a = int((rs.a+4.1) * 100);
+                    value.a = int((rs.a) * 100);
+                    value.corrected = (rs.error_code == eSucc);
+                    value.h = rs.h;
+                    value.l = rs.l;
+                    value.w = rs.w;
+                    value.x = rs.x ;
+                    value.y = rs.y;
+
+                    // 正常返回状态3
+                    LOG(INFO) << "\t post：" << rs.a << "°" << ", " << " center:(" <<
+                        rs.x << "," << rs.y << "), size:" << "(" << rs.l << ", " << rs.w << ", " << rs.h << ")"<<",rs.code:"<<Error_string(rs.error_code);
+                    if (m_plc)
+                        m_plc->setMeasureResult(m_calib_param.plate_area(rs.pos_id).plc_addr(), &value);
+                }
+            }
+        }
+        else
+        {
+            LOG(ERROR) << "\t测量失败 : " << Error_string(code);
+        }
+    }
+    ///写入测量完成信号
+    if (m_plc)
+        m_plc->MeasureComplete(code==eSucc);
+
+
+    /////回调函数，窗口显示结果
+    /*Locate_Message msg;
+    msg.error_code = code;
+    msg.positions = results;
+    msg.cloud = m_cloud.makeShared();
+    time_t tt = time(NULL);//时间cuo
+    tm* t = localtime(&tt);
+    char time[255] = { 0 };
+    sprintf(time, "%d-%02d-%02d %02d:%02d:%02d", t->tm_year + 1900, t->tm_mon + 1,
+        t->tm_mday, t->tm_hour, t->tm_min, t->tm_sec);
+    msg.locate_time = time;
+    msg.save_dir = strPath.GetBuffer();
+
+    std::vector<int> IDs;
+    for (int i = 0; i < m_lasers.size(); ++i)
+        IDs.push_back(m_lasers[i]->ID() + 1);
+    msg.laser_id = IDs;
+
+    if (m_post_result_func && m_ptr)
+        m_post_result_func(msg, m_ptr);*/
+
+
+    delete this;
+}
+void  MeasureTask::Cancel()
+{
+    m_exit = true;
+}
+
+bool MeasureTask::IsLaserReady()
+{
+    bool ready = true;
+    for (int i = 0; i < m_lasers.size(); ++i)
+    {
+        ready = (ready && m_lasers[i]->IsReady());
+    }
+
+    return ready;
+}
+
+////////////////////////////////

src/measuretask.h

@@ -0,0 +1,97 @@
+#ifndef MEASURETASK_H
+#define MEASURETASK_H
+
+#include "common.h"
+#include "TaskQueue/BaseTask.h"
+#include "laser/Laser.h"
+#include "modbus/PLCMonitor.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;
+
+std::string Error_string(ERROR_CODE code);
+bool RegionInRegion(std::vector<cv::Point2f>& mini_poly, std::vector<cv::Point2f>& large_poly);
+double distance_polys(std::vector<cv::Point2f> poly1, std::vector<cv::Point2f> poly2);
+
+//typedef void(*ResultCallback)(Locate_Message message, void*);
+
+class MeasureTask : public tq::BaseTask
+{
+public:
+    MeasureTask(std::vector<CLaser*> lasers,void* locater,int position_id,
+        modbus::CPLCMonitor* pcl, Automatic::stCalibParam	param);
+    virtual void   Main();
+    virtual void   Cancel();
+    //void			SetResultCallback(void* ptr,ResultCallback func = NULL);
+
+protected:
+    static void		PushPoint(CPoint3D point, void* pointer);
+    bool			IsLaserReady();
+    ERROR_CODE		SelectResult(int plate_index, std::vector<CarPosition> results, CarPosition& result);
+    //žùŸÝ³µÁŸÎ»Ö÷ÖÅ䳵λID, ²»ÔÚ³µÎ»µÄ³µÁŸÐÅϢɟ³ý
+    ERROR_CODE		Dispatch_posID(std::vector<CarPosition>& results);
+
+
+protected:
+    std::vector<CLaser*>		m_lasers;
+    void*					m_locater;
+    modbus::CPLCMonitor*		m_plc;
+
+    int							m_position_id;		//³µÎ»id
+    Automatic::stCalibParam		m_calib_param;		//ÓÐЧÇøÓò
+    PointCloudT					m_cloud;
+    mutable std::mutex			m_mutex;
+
+
+    static	std::mutex*			g_mutex;
+    bool						m_exit;
+
+    //CSearchPath					m_searchPath;
+    //ResultCallback				m_post_result_func;
+    void*						m_ptr;
+};
+
+
+#endif // MEASURETASK_H

src/pathcreator.cpp

@@ -0,0 +1,82 @@
+#include "pathcreator.h"
+
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+
+PathCreator::PathCreator()
+{
+
+}
+
+PathCreator::~PathCreator()
+{
+
+}
+
+std::string PathCreator::GetCurPath()
+{
+    return m_current_path;
+}
+bool PathCreator::Mkdir(std::string dirName)
+{
+    uint32_t beginCmpPath = 0;
+    uint32_t endCmpPath = 0;
+    std::string fullPath = "";
+
+        if('/' != dirName[0])
+        {
+            fullPath = getcwd(nullptr, 0);
+            beginCmpPath = fullPath.size();
+            fullPath = fullPath + "/" + dirName;
+        }
+        else
+        {
+            //Absolute path
+            fullPath = dirName;
+            beginCmpPath = 1;
+        }
+        if (fullPath[fullPath.size() - 1] != '/')
+        {
+            fullPath += "/";
+        }
+        endCmpPath = fullPath.size();
+
+        //create dirs;
+        for(uint32_t i = beginCmpPath; i < endCmpPath ; i++ )
+        {
+            if('/' == fullPath[i])
+            {
+                std::string curPath = fullPath.substr(0, i);
+                if(access(curPath.c_str(), F_OK) != 0)
+                {
+                    if(mkdir(curPath.c_str(), /*S_IRUSR|S_IRGRP|S_IROTH|S_IWUSR|S_IWGRP|S_IWOTH*/0777) == -1)
+                    {
+                        printf("mkdir(%s) failed\n", curPath.c_str());
+                        return false;
+                    }
+                }
+            }
+        }
+        m_current_path=fullPath;
+        return true;
+
+}
+
+bool PathCreator::CreateDatePath(std::string root)
+{
+    time_t tt;
+    time( &tt );
+    tt = tt + 8*3600;  // transform the time zone
+    tm* t= gmtime( &tt );
+    char buf[255]={0};
+    sprintf(buf,"%s/%d%02d%02d-%02d%02d%02d",root.c_str(),
+               t->tm_year + 1900,
+               t->tm_mon + 1,
+               t->tm_mday,
+               t->tm_hour,
+               t->tm_min,
+               t->tm_sec);
+    return Mkdir(buf);
+}

src/pathcreator.h

@@ -0,0 +1,17 @@
+#ifndef PATHCREATOR_H
+#define PATHCREATOR_H
+#include <string>
+
+class PathCreator
+{
+public:
+    PathCreator();
+    ~PathCreator();
+    std::string GetCurPath();
+    bool Mkdir(std::string dir);
+    bool CreateDatePath(std::string root);
+protected:
+    std::string m_current_path;
+};
+
+#endif // PATHCREATOR_H