v2.1

CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 2.8)
+project(hins_se_driver)
+
+set( CMAKE_BUILD_TYPE Release )
+set( CMAKE_BUILD_TYPE Debug )
+if( CMAKE_BUILD_TYPE STREQUAL "Release")
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -std=c++11 -fPIC")
+else()
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -std=c++11 -fPIC")
+endif()
+
+
+file(GLOB_RECURSE HEADER_FILES include/*.h)
+file(GLOB_RECURSE SOURCE_FILES src/*.cpp)
+
+find_package(catkin REQUIRED COMPONENTS
+roscpp
+rospy
+std_msgs
+geometry_msgs
+sensor_msgs
+tf
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES
+  CATKIN_DEPENDS )
+
+#set(lib_files )
+
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+add_executable(${PROJECT_NAME} ${SOURCE_FILES} ${HEADER_FILES})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} )
+
+# install executables and/or libraries
+install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include
+DESTINATION "${CMAKE_INSTALL_PREFIX}" COMPONENT header)
+
+# install files
+foreach(dir launch)
+install(DIRECTORY ${dir}/
+DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)
+

CMakeLists.txt.user

@@ -0,0 +1,375 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE QtCreatorProject>
+<!-- Written by QtCreator 4.8.2, 2021-03-19T10:04:04. -->
+<qtcreator>
+ <data>
+  <variable>EnvironmentId</variable>
+  <value type="QByteArray">{0ecb8702-28d5-4134-b587-a0b8643a8bbb}</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">false</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="ClangCodeModel.CustomCommandLineKey"/>
+   <value type="bool" key="ClangCodeModel.UseGlobalConfig">true</value>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.Target.0</variable>
+  <valuemap type="QVariantMap">
+   <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">桌面</value>
+   <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">桌面</value>
+   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{d6473b3e-f747-4f26-ab57-f336a893711c}</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
+    <valuelist type="QVariantList" key="CMake.Configuration"/>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/develop/lead_develop/build-hins_driver_laser</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CMake Build</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</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="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CMake Build</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</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">Default</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Default</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeBuildConfiguration</value>
+   </valuemap>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
+    <valuelist type="QVariantList" key="CMake.Configuration">
+     <value type="QString">CMAKE_BUILD_TYPE:STRING=Debug</value>
+    </valuelist>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/develop/lead_develop/build-hins_driver_laser</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CMake Build</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</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="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CMake Build</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</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">Debug</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeBuildConfiguration</value>
+   </valuemap>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.2">
+    <valuelist type="QVariantList" key="CMake.Configuration">
+     <value type="QString">CMAKE_BUILD_TYPE:STRING=Release</value>
+    </valuelist>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/develop/lead_develop/build-hins_driver_laser</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CMake Build</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</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="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CMake Build</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</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">Release</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeBuildConfiguration</value>
+   </valuemap>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.3">
+    <valuelist type="QVariantList" key="CMake.Configuration">
+     <value type="QString">CMAKE_BUILD_TYPE:STRING=RelWithDebInfo</value>
+    </valuelist>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/develop/lead_develop/build-hins_driver_laser</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CMake Build</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</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="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CMake Build</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</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 with Debug Information</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Release with Debug Information</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeBuildConfiguration</value>
+   </valuemap>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.4">
+    <valuelist type="QVariantList" key="CMake.Configuration">
+     <value type="QString">CMAKE_BUILD_TYPE:STRING=MinSizeRel</value>
+    </valuelist>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/develop/lead_develop/build-hins_driver_laser</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CMake Build</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</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="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
+      <valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets">
+       <value type="QString">all</value>
+      </valuelist>
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CMake Build</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</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">Minimum Size Release</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Minimum Size Release</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeBuildConfiguration</value>
+   </valuemap>
+   <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">5</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 Configuration</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.CustomExecutableRunConfiguration.Executable"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Custom Executable</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
+    <value type="QString" key="RunConfiguration.Arguments"></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>
+    <value type="QString" key="RunConfiguration.WorkingDirectory"></value>
+    <value type="QString" key="RunConfiguration.WorkingDirectory.default"></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">20</value>
+ </data>
+ <data>
+  <variable>Version</variable>
+  <value type="int">20</value>
+ </data>
+</qtcreator>

include/connection_address.h

@@ -0,0 +1,75 @@
+/*********************************************************************
+*
+* Software License Agreement ()
+*
+*  Copyright (c) 2020, HINS, Inc.
+*  All rights reserved.
+*
+* Author: Hu Liankui
+* Create Date: 8/9/2020
+*********************************************************************/
+
+/**
+ * @brief tcp连接地址描述
+ */
+
+#pragma once
+
+#include <string>
+
+using namespace std;
+
+namespace hins {
+
+class ConnectionAddress final
+{
+public:
+  ConnectionAddress(){;}
+  ConnectionAddress(const string& address, const int& port)
+    : address_(address)
+    , port_(port)
+    , description_("Connection address and port")
+  {}
+  ConnectionAddress(const string& address, const int& port, const string& description)
+    : address_(address)
+    , port_(port)
+    , description_(description)
+  {}
+
+  void SetAddress(const string& address)
+  {
+    address_ = address;
+  }
+
+  void SetPort(const int& port)
+  {
+    port_ = port;
+  }
+
+  void SetDescription(const string& description)
+  {
+    description_ = description;
+  }
+
+  const string GetAddress() const
+  {
+    return address_;
+  }
+
+  int GetPort() const
+  {
+    return port_;
+  }
+
+  const string GetDescription() const
+  {
+    return description_;
+  }
+
+private:
+  string address_;
+  int port_;
+  string description_;
+};
+
+}

include/hins/laser_data_receiver.h

@@ -0,0 +1,103 @@
+/*********************************************************************
+*
+* Software License Agreement ()
+*
+*  Copyright (c) 2020, HINS, Inc.
+*  All rights reserved.
+*
+* Author: Hu Liankui
+* Create Date: 8/9/2020
+*********************************************************************/
+
+/**
+ * @brief 兴颂激光雷达数据获取
+ */
+
+#pragma once
+
+#define BOOST_CB_DISABLE_DEBUG
+#include <string>
+#include <iostream>
+#include <mutex>
+#include <condition_variable>
+#include <deque>
+#include <array>
+#include <thread>
+#include <boost/bind.hpp>
+#include <boost/asio.hpp>
+#include <boost/thread.hpp>
+#include <boost/circular_buffer.hpp>
+#include "connection_address.h"
+#include "hins/protoc.h"
+
+namespace hins{
+
+class LaserDataReceiver;
+using LaserDataReceiverPtr = LaserDataReceiver*;
+using LaserDataReceiverHdr = std::shared_ptr<LaserDataReceiver>;
+
+class LaserDataReceiver final
+{
+public:
+  LaserDataReceiver(const ConnectionAddress &conn_info);
+
+  ~LaserDataReceiver();
+
+  int SyncWrite();
+  int SyncWrite(const std::array<unsigned char, 12> command);
+  bool Connect();
+
+  bool IsConnected();
+
+  void Disconnect();
+
+  bool CheckConnection();
+
+  ScanData GetFullScan();
+
+private:
+  void HandleSocketRead(const boost::system::error_code& error);
+
+  int32_t FindPacketStart();
+
+  /**
+   * @brief WriteBufferBack write data to buffer tail
+   * @param src
+   * @param num_bytes
+   */
+  void WriteBufferBack(char* src, std::size_t num_bytes);
+
+  /**
+   * @brief HandleNextPacket parse the data in ring buffer
+   * @return true - parse success; false - otherwise
+   */
+  bool HandleNextPacket();
+
+  bool RetrivePacket();
+
+  void ReadBufferFront(char* dst, const uint16_t& num_bytes);
+
+private:
+  ConnectionAddress conn_info_;
+  bool is_connected_;
+  boost::thread io_service_thread_;
+  boost::asio::io_service io_service_;
+  
+  boost::asio::streambuf inbuf_;                  // Boost::Asio streambuffer
+  std::istream instream_;                         // Input stream
+
+  boost::asio::ip::tcp::socket* tcp_socket_ptr_;  // reveiving socket
+
+  boost::circular_buffer<char> ring_buffer_;      // Internal ringbuffer for temporarily storing reveived data
+
+  std::mutex scan_mutex_;                         // Protection against data races between ROS and IO threads
+  std::condition_variable data_notifier_;         // Data notification condition variable
+
+  std::deque<ScanData> scan_data_;                // Double ended queue with sucessfully received and parsed data, organized as single complete scans
+  std::deque<AreaData> area_data_;
+  
+  uint64_t last_data_time_;                       // time in seconds since epoch, when last data was received
+};
+
+}
+

include/hins/protoc.h

@@ -0,0 +1,72 @@
+/*********************************************************************
+*
+* Software License Agreement ()
+*
+*  Copyright (c) 2020, HINS, Inc.
+*  All rights reserved.
+*
+* Author: Hu Liankui
+* Create Date: 8/9/2020
+*********************************************************************/
+
+#pragma once
+
+#include <array>
+#include <vector>
+
+namespace hins {
+
+const std::array<unsigned char, 8> kStartCapture {0x52, 0x41, 0x75, 0x74, 0x6f, 0x01, 0x87, 0x80};
+
+struct ScanData
+{
+  // 距离数据
+  std::vector<std::uint32_t> distance_data;
+  
+  // 强度数据
+  std::vector<std::uint32_t> amplitude_data;
+};
+
+struct AreaData
+{
+std::array<unsigned char, 8> identifier;    // 标识符
+char channel;                               // 传感器通道值
+char input_state;
+char output_state;
+char led_state;
+uint16_t error_state;
+uint16_t check;
+};
+
+
+struct ShadowsFilterParam
+{
+  float min_angle;
+  float max_angle;
+  int neighbors;            // 删除窗口
+  int window;               // 搜索计算窗口
+  int shadows_filter_level; 
+  int traverse_step;          // 遍历步长
+};
+
+#pragma pack(1)
+
+struct XSPackageHeader
+{
+  char head[6];              // 5个标识符+一个预留位
+  uint16_t start_angle;      // 起始角度
+  uint16_t end_angle;        // 终止角度
+  uint16_t data_size;        // 这个包里面总的测量点数
+};
+
+const uint16_t kXSPackageHeadSize = sizeof(XSPackageHeader);
+const uint16_t kXSAreaDataSize = sizeof(AreaData);
+const uint16_t kMaxDistance = 10000;
+const uint16_t kMaxIntensity = 30000;
+
+
+#pragma pack()
+
+
+}
+

include/hins/xingsong_driver.h

@@ -0,0 +1,73 @@
+/*********************************************************************
+ *
+ * Software License Agreement ()
+ *
+ *  Copyright (c) 2020, HINS, Inc.
+ *  All rights reserved.
+ *
+ * Author: Hu Liankui
+ * Create Date: 8/9/2020
+ *********************************************************************/
+
+/**
+ * @brief 兴颂激光雷达驱动
+ */
+
+#pragma once
+
+#include "hins/laser_data_receiver.h"
+#include <set>
+namespace hins
+{
+
+  class XingSongDriver;
+  using XingSongDriverHdr = std::shared_ptr<XingSongDriver>;
+
+  class XingSongDriver
+  {
+  public:
+    XingSongDriver(const ConnectionAddress &conn_info);
+    XingSongDriver(const ConnectionAddress &conn_info, ShadowsFilterParam shadows_filter_param_);
+    ~XingSongDriver();
+
+    bool StartCapturingTCP();
+
+    ScanData GetFullScan();
+    
+    bool Connect()
+    {
+      return data_receiver_ptr_->Connect();
+    }
+
+    bool IsConnected()
+    {
+      return data_receiver_ptr_->IsConnected();
+    }
+
+    void Disconnect()
+    {
+      data_receiver_ptr_->Disconnect();
+    }
+
+    int SyncWrite(const std::array<unsigned char, 12> command)
+    {
+      return data_receiver_ptr_->SyncWrite(command);
+    }
+
+  private:
+    void RunMain();
+    void ShadowsFilterInit(int scan_num);
+    void ShadowsFilter(ScanData& data, int scan_num);
+  
+  private:
+    ShadowsFilterParam shadows_filter_param_;
+    ConnectionAddress conn_info_;
+    LaserDataReceiverPtr data_receiver_ptr_;
+    std::thread guard_thread_;
+    std::vector<float> shadows_filter_threshold_min_;
+    std::vector<float> shadows_filter_threshold_max_;
+    set<int> shadows_del_index_;
+  };
+
+
+}

include/utils.h

@@ -0,0 +1,34 @@
+/*********************************************************************
+*
+* Software License Agreement ()
+*
+*  Copyright (c) 2020, HINS, Inc.
+*  All rights reserved.
+*
+* Author: Hu Liankui
+* Create Date: 8/9/2020
+*********************************************************************/
+
+/**
+ * @brief useful tool
+ */
+
+#pragma once
+#include <inttypes.h>
+#include <sys/time.h>
+#include <string>
+
+namespace hins {
+
+/**
+ * @brief Now ms timestamp
+ * @return
+ */
+static uint64_t Now()
+{
+  struct timeval tv;
+  gettimeofday(&tv, NULL);
+  return tv.tv_sec * 1000 + tv.tv_usec / 1000;
+}
+
+} // end of namespace hins

launch/hins_driver.launch~

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<launch> 
+ <node pkg="hins_se_driver" type="hins_se_driver" name="hins_se_driver" output="screen" >
+  <param name="laser_ip" type="string" value="192.168.1.88"/>
+  <param name="port" type="int" value="8080"/>
+  <param name="topic_name" type="string" value="scan"/>
+  <param name="frame_name" type="string" value="laser"/>
+  
+  <param name="shadows_filter_level" type="int" value="0"/>
+  
+  <param name="shadows_filter_max_angle" value="175"/>
+  <param name="shadows_filter_min_angle" value="5"/>
+  <param name="shadows_filter_traverse_step" type="int" value="1"/>
+  <param name="shadows_filter_window" type="int" value="3"/>
+  
+ </node>
+ 
+  <!-- launch rviz -->
+  <!--<node name="laser" pkg="rviz" type="rviz" required="true"
+        args="-d $(find hins_SE_series_driver)/launch/laser.rviz" /> -->
+
+</launch>
+
+

launch/hins_se_driver.launch

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<launch> 
+ <node pkg="hins_se_driver" type="hins_se_driver" name="hins_se_driver" output="screen" >
+  <param name="laser_ip" type="string" value="192.168.1.88"/>
+  <param name="port" type="int" value="8080"/>
+  <param name="topic_name" type="string" value="scan"/>
+  <param name="frame_name" type="string" value="laser"/>
+  
+  <param name="shadows_filter_level" type="int" value="2"/>
+  
+  <param name="shadows_filter_max_angle" value="175"/>
+  <param name="shadows_filter_min_angle" value="5"/>
+  <param name="shadows_filter_traverse_step" type="int" value="1"/>
+  <param name="shadows_filter_window" type="int" value="3"/>
+  
+ </node>
+ 
+  <!-- launch rviz -->
+  <!--<node name="laser" pkg="rviz" type="rviz" required="true"
+        args="-d $(find hins_SE_series_driver)/launch/laser.rviz" /> -->
+
+</launch>
+
+

launch/laser.rviz

@@ -0,0 +1,149 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /LaserScan1
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: ""
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: ""
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: laser
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 18.546350479125977
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7403981685638428
+      Target Frame: <Fixed Frame>
+      Yaw: 5.36857795715332
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000282000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1267
+  X: 2208
+  Y: 231

package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package>
+  <name>hins_se_driver</name>
+  <version>1.0.0</version>
+  <description>The hins navigation lidar driver package</description>
+  <maintainer email="huliangkui@xingsong.com">Liankui Hu</maintainer>
+  <license>HINS</license>
+  <url type="website">http://xingsong.com</url>
+  <author email="huliangkui@xingsong.com">Liankui Hu</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+</package>

readme.txt

@@ -0,0 +1,18 @@
+1、防拖尾过滤功能
+  方法 : 在launch文件内修改对应参数 : "shadows_filter_max_angle", "shadows_filter_min_angle", "shadows_filter_traverse_step", "shadows_filter_window", "shadows_filter_level"
+  说明 : 以shadows_filter_traverse_step为步长，遍历雷达数据。每次搜索遍历点的后shadows_filter_window个点，若两者的垂直夹角小于shadows_filter_min_angle，或者大于shadows_filter_max_angle时，则将较长的点设置为60.001m
+  1.当shadows_filter_level为-1时，按照具体参数配置过滤算法
+    1)shadows_filter_max_angle : 筛选的夹角最大值
+    2)shadows_filter_min_angle : 筛选的夹角最小值
+    3)shadows_filter_traverse_step : 遍历点数的步长(每n个点检测一次)
+    4)shadows_filter_window : 搜索窗口大小(检测遍历点的后n个值)
+  2.当shadows_filter_level为0时，忽略参数设置，不增加防拖尾过滤功能
+  3.当shadows_filter_level为1时，过滤速度较快，筛选角度大
+  4.当shadows_filter_level为2时，过滤速度较慢，筛选角度大
+  5.当shadows_filter_level为3时，过滤速度较慢，筛选角度小
+  6.若launch文件没有设置以上参数，则默认如下：
+    1)shadows_filter_level:-1               # 按照具体参数配置防拖尾过滤器
+    2)shadows_filter_max_angle:175          # 筛选的夹角最大值为175度
+    3)shadows_filter_min_angle:5            # 筛选的夹角最小值为5度
+    4)shadows_filter_traverse_step:1        # 遍历每个点
+    5)shadows_filter_window:2               # 搜索遍历点后2个点是否存在拖尾现象

src/hins/laser_data_receiver.cpp

@@ -0,0 +1,449 @@
+#include "hins/laser_data_receiver.h"
+#include "hins/protoc.h"
+#include "utils.h"
+#include <iostream>
+#include <fstream>
+
+// #define DEBUG
+namespace hins {
+
+LaserDataReceiver::LaserDataReceiver(const ConnectionAddress& conn_info)
+  : conn_info_(conn_info)
+  , is_connected_(false)
+  , inbuf_(4096)
+  , instream_(&inbuf_)
+  , tcp_socket_ptr_(0)
+  , ring_buffer_(65536)
+  , scan_data_()
+  , last_data_time_(hins::Now())
+{
+  std::cout << "Connecting to TCP data channel at hostname: " << conn_info.GetAddress()
+          << " tcp_port:" << conn_info.GetPort() << std::endl;
+
+  if(Connect())
+    std::cout << "Lidar connect success." << std::endl;
+  else
+    std::cout << "Lidar connect failed." << std::endl;
+}
+
+LaserDataReceiver::~LaserDataReceiver()
+{
+  Disconnect();
+
+  if(tcp_socket_ptr_)
+    delete tcp_socket_ptr_;
+}
+
+bool LaserDataReceiver::IsConnected()
+{
+  return is_connected_;
+}
+
+void LaserDataReceiver::Disconnect()
+{
+  is_connected_ = false;
+  try
+  {
+    if( tcp_socket_ptr_ != nullptr )
+      tcp_socket_ptr_->close();
+
+    io_service_.stop();
+    if( boost::this_thread::get_id() != io_service_thread_.get_id() )
+      io_service_thread_.join();
+
+  }
+  catch (std::exception& e)
+  {
+    std::cout << "Exception:" << e.what() << std::endl;
+  }
+  std::cout << "Disconnect." << std::endl;
+}
+
+bool LaserDataReceiver::CheckConnection()
+{
+  if( !IsConnected() )
+    return false;
+  if( (hins::Now() - last_data_time_)/1000.0 > 5.0 ) // 5.0s 断线
+  {
+    Disconnect();
+    std::cout << "超时断线，超时时间:" << (hins::Now() - last_data_time_)/1000.0 << std::endl;
+    return false;
+  }
+  return true;
+}
+
+ScanData LaserDataReceiver::GetFullScan()
+{
+  std::unique_lock<std::mutex> lock(scan_mutex_);
+  while( CheckConnection() && scan_data_.size() < 2 )
+    data_notifier_.wait_for(lock, std::chrono::seconds(1));
+
+
+  ScanData data;
+  if( scan_data_.size() >= 2 && IsConnected())
+  {
+    data = ScanData(std::move(scan_data_.front()));
+    scan_data_.pop_front();
+  }
+  else
+    std::cout << "null data" << std::endl;
+  return data;
+}
+
+void LaserDataReceiver::HandleSocketRead(const boost::system::error_code &error)
+{
+  if(!error)
+  {
+    // 1. 将所有数据推送到缓冲区
+    instream_.clear();
+    while(!instream_.eof())
+    {
+      char buf[4096];
+      instream_.read(buf, 4096);
+      int bytes_read = instream_.gcount();
+      WriteBufferBack(buf, bytes_read);       // 将 buf 的数据写到 ring_buffer_ 里
+    }
+
+    // 2. 继续读取数据，直到数据被读取完
+    while(HandleNextPacket()) {}
+
+    // 3. 继续异步读取数据
+    boost::asio::async_read(*tcp_socket_ptr_, inbuf_, boost::bind(&LaserDataReceiver::HandleSocketRead, this, boost::asio::placeholders::error));
+  }
+  else
+  {
+    if( error.value() != 995 )
+      std::cout << "ERROR: data connection error:"
+                << error.message()
+                << " "
+                << error.value()
+                << std::endl;
+    Disconnect();
+  }
+}
+
+int LaserDataReceiver::SyncWrite()
+{
+  boost::system::error_code ec;
+
+  int ret = tcp_socket_ptr_->write_some(boost::asio::buffer(kStartCapture), ec);
+  if(ec)
+  {
+    std::cout << "write failed:%" << boost::system::system_error(ec).what() << std::endl;
+    ret = -1;
+  }
+  #ifdef DEBUG
+  std::cout << "SyncWrite kStartCapture done" << std::endl;
+  #endif
+  return ret;
+}
+
+
+int LaserDataReceiver::SyncWrite(const std::array<unsigned char, 12> command)
+{
+  boost::system::error_code ec;
+
+  int ret = tcp_socket_ptr_->write_some(boost::asio::buffer(command), ec);
+  if(ec)
+  {
+    std::cout << "write failed:" << boost::system::system_error(ec).what() << std::endl;
+    ret = -1;
+  }
+  #ifdef DEBUG
+  std::cout << "SyncWrite command done" << std::endl;
+  #endif
+  return ret;
+}
+
+// CRC循环校验码-12位命令
+// 修改command的最后两位
+// 返回循环校验码的10进制数字
+unsigned int CRC_Verify_len12(std::array<unsigned char, 12> *command)
+{
+  unsigned int i, j;
+  unsigned int wCrc = 0xffff;
+  unsigned int wPolynom = 0xA001;
+  /*-------------------------------------*/
+  for (i = 0; i < 12 - 2; i++) // 后两位为校验位
+  {
+    wCrc ^= command->at(i);
+    for (j = 0; j < 8; j++)
+    {
+      if (wCrc & 0x0001)
+      {
+        wCrc = (wCrc >> 1) ^ wPolynom;
+      }
+      else
+      {
+        wCrc = wCrc >> 1;
+      }
+    }
+  }
+  unsigned int tmp1 = (wCrc & 0xff00) >> 8;           // 高位
+  unsigned int tmp2 = (wCrc & 0x00ff);                // 低位
+  command->at(10) = tmp2;
+  command->at(11) = tmp1;
+  return wCrc;
+}
+
+bool LaserDataReceiver::Connect()
+{
+  try
+  {
+    // 创建hostname/ip
+    boost::asio::ip::tcp::resolver resolver(io_service_);
+    boost::asio::ip::tcp::resolver::query query(conn_info_.GetAddress(), std::to_string(conn_info_.GetPort()));
+    boost::asio::ip::tcp::resolver::iterator endpoint_iterator = resolver.resolve(query);
+    boost::asio::ip::tcp::resolver::iterator end;
+
+    if(nullptr == tcp_socket_ptr_)
+    {
+      tcp_socket_ptr_ = new boost::asio::ip::tcp::socket(io_service_);
+    }
+    else
+    {
+      delete tcp_socket_ptr_;
+      tcp_socket_ptr_ = new boost::asio::ip::tcp::socket(io_service_);
+    }
+
+    boost::system::error_code error = boost::asio::error::host_not_found;
+
+    // 迭代端点并建立连接
+    while (error && endpoint_iterator != end)
+    {
+      // tcp_socket_ptr_->close();
+      tcp_socket_ptr_->connect(*endpoint_iterator++, error);
+    }
+    if (error)
+    {
+      std::cout << "Connect failed." << error.message().c_str() << std::endl;
+      return false;
+    }
+
+    // 开始异步读取数据
+    boost::asio::async_read(*tcp_socket_ptr_, inbuf_, boost::bind(&LaserDataReceiver::HandleSocketRead, this, boost::asio::placeholders::error));
+    io_service_thread_ = boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
+    is_connected_ = true;
+  }
+  catch (std::exception& e)
+  {
+    std::cout << "Exception:" << e.what() << std::endl;
+    is_connected_ = false;
+    return false;
+  }
+  return true;
+}
+
+// 根据协议寻找数据帧头
+int32_t LaserDataReceiver::FindPacketStart()
+{
+  if(ring_buffer_.size() < kXSPackageHeadSize)
+    return -1;
+
+  // 兴颂LS雷达数据的帧头
+  for(size_t i = 0; i < ring_buffer_.size() - 5; i++)
+  {
+    // 距离数据
+    if(0x43 == ((unsigned char)ring_buffer_[i])   &&
+       0x6c == ((unsigned char)ring_buffer_[i+1]) &&
+       0x6f == ((unsigned char)ring_buffer_[i+2]) &&
+       0x75 == ((unsigned char)ring_buffer_[i+3]) &&
+       0x64 == ((unsigned char)ring_buffer_[i+4]))
+    {
+      i = i << 2;
+      i |= 0x01;     // bit0置1 
+      return i;
+    }
+    else
+      // 区域数据
+      if(0x57 == ((unsigned char)ring_buffer_[i])   &&
+         0x53 == ((unsigned char)ring_buffer_[i+1]) &&
+         0x69 == ((unsigned char)ring_buffer_[i+2]) &&
+         0x6d == ((unsigned char)ring_buffer_[i+3]) &&
+         0x75 == ((unsigned char)ring_buffer_[i+4]))
+      {
+        i = i << 2;
+        i |= 0x02;     // bit1置1
+        return i;
+      }
+  }
+  return -2;
+}
+
+// 将src数据写到 ring_buffer_ 里
+void LaserDataReceiver::WriteBufferBack(char *src, std::size_t num_bytes)
+{
+  if(ring_buffer_.size() + num_bytes > ring_buffer_.capacity())
+    throw std::exception();
+
+  ring_buffer_.resize(ring_buffer_.size() + num_bytes);           // 修改 ring_buffer_ 的大小
+  char* pone = ring_buffer_.array_one().first;                    // ring_buffer_ 的 array_one 的头指针
+  std::size_t pone_size = ring_buffer_.array_one().second;        // ring_buffer_ 的 array_one 的大小
+  char* ptwo = ring_buffer_.array_two().first;                    // ring_buffer_ 的 array_two 的头指针
+  std::size_t ptwo_size = ring_buffer_.array_two().second;        // ring_buffer_ 的 array_two 的大小
+
+  // 将src数据写到 ring_buffer_ 里
+  if(ptwo_size >= num_bytes)
+  {
+    std::memcpy(ptwo + ptwo_size - num_bytes, src, num_bytes);
+  }
+  else
+  {
+    std::memcpy(pone + pone_size + ptwo_size - num_bytes, src, num_bytes - ptwo_size);
+    std::memcpy(ptwo, src + num_bytes - ptwo_size, ptwo_size);
+  }
+}
+
+bool LaserDataReceiver::HandleNextPacket()
+{
+  if(scan_data_.empty())
+  {
+    scan_data_.emplace_back();
+  }
+
+  if(RetrivePacket())
+  {
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
+
+bool LaserDataReceiver::RetrivePacket()
+{
+  bool ret = false;
+  int16_t head_index = FindPacketStart();                     // 寻找帧头
+  if(head_index < 0)
+    return ret;
+  // std::cout << "找到帧头了" << std::endl;
+  if(head_index & 0x01==1)      // 距离数据
+  {
+    head_index = head_index >> 2;
+    // 寻找帧头并处理数据
+    if(ring_buffer_.size() - head_index >= kXSPackageHeadSize)
+    {
+      ring_buffer_.erase_begin(head_index);                     // 删除 帧头前的数据
+      head_index = 0;
+
+      // 1. 解析数据帧帧头
+      char head_buf[kXSPackageHeadSize];
+      ReadBufferFront(head_buf, kXSPackageHeadSize);
+
+      XSPackageHeader header;
+      header.start_angle = (unsigned char)head_buf[6] << 8;     // 起始角度
+      header.start_angle |= (unsigned char)head_buf[7];
+      header.end_angle = (unsigned char)head_buf[8] << 8;       // 终止角度
+      header.end_angle |= (unsigned char)head_buf[9];
+      header.data_size = (unsigned char)head_buf[10] << 8;      // 测量点总数
+      header.data_size |= (unsigned char)head_buf[11];
+
+      std::unique_lock<std::mutex> lock(scan_mutex_);
+
+      // 2. 解析帧内容
+      ScanData& scan_data = scan_data_.back();
+      uint16_t body_size = kXSPackageHeadSize + header.data_size * 4;
+
+      if((ring_buffer_.size() - head_index) >= body_size)
+      {
+        #ifdef DEBUG
+        std::cout << "begin:" << header.start_angle 
+                  << "  end:" << header.end_angle 
+                  << "  data_size:" << header.data_size
+                  << std::endl;
+        #endif
+
+        char* body_buf = new char[body_size];
+        ReadBufferFront(body_buf, body_size);                   // 将当前帧的数据复制到 body_buf 中
+
+        ring_buffer_.erase_begin(head_index + body_size);       // 删除 ring_buffer_ 的数据
+
+        for(int i = 0; i < header.data_size; i++)
+        {
+          unsigned short int distance;
+          unsigned short int intensity;
+          distance = (unsigned char)body_buf[i*4 + 13] << 8;
+          distance |= (unsigned char)body_buf[i*4 + 12];
+          intensity = (unsigned char)body_buf[i*4 + 15] << 8;
+          intensity |= (unsigned char)body_buf[i*4 + 14];
+
+          if(distance > kMaxDistance)
+          {
+            distance = kMaxDistance;
+          }
+          if(intensity > kMaxIntensity)
+          {
+            intensity = kMaxIntensity;
+          }
+
+          scan_data.distance_data.push_back(distance);
+          scan_data.amplitude_data.push_back(intensity);
+        }
+
+        delete [] body_buf;
+
+        if(header.end_angle == 315)
+        {
+          scan_data_.emplace_back();
+          if(scan_data_.size() > 5)
+          {
+            scan_data_.pop_front();
+            std::cout << "buffer data too many, drops it" << std::endl;
+          }
+          data_notifier_.notify_one();
+          last_data_time_ = hins::Now();
+          ret = true;
+        }
+
+        if(FindPacketStart() >= 0)
+        {
+          ret = true;
+        }
+      }
+    }
+    else
+      if(head_index & 0x02==1)      // 区域数据
+      {
+        head_index = head_index >> 2;
+        if(ring_buffer_.size() - head_index >= kXSAreaDataSize)
+        {
+          AreaData area_data;
+          char area_buf[kXSPackageHeadSize];
+          ring_buffer_.erase_begin(head_index);                     // 删除 帧头前的数据
+          ReadBufferFront(area_buf, kXSPackageHeadSize);
+          area_data.channel = (unsigned char)area_buf[5];           // 传感器通道值
+          area_data.input_state = (unsigned char)area_buf[6];
+          area_data.output_state = (unsigned char)area_buf[7];
+          area_data.led_state = (unsigned char)area_buf[8];
+          area_data.error_state = (unsigned char)area_buf[9];
+          area_data.check = (unsigned char)area_buf[9];
+        }
+      }
+  }
+  return ret;
+}
+
+// 将ring_buffer_前num_bytes的数据拷贝到dst
+void LaserDataReceiver::ReadBufferFront(char *dst, const uint16_t &num_bytes)
+{
+  if( ring_buffer_.size() < num_bytes )
+    throw std::exception();
+
+  char* pone = ring_buffer_.array_one().first;                  // 指向环形缓冲区ring_buffer_的array_one的开始地址
+  std::size_t pone_size = ring_buffer_.array_one().second;      // ring_buffer_的array_one的大小
+  char* ptwo = ring_buffer_.array_two().first;                  // 指向环形缓冲区ring_buffer_的array_two的开始地址
+
+  if( pone_size >= num_bytes )                                  // ring_buffer_的array_one的大小大于num_bytes
+  {
+    std::memcpy(dst, pone, num_bytes);                          // 复制环形缓冲区的数据到dst中
+  }
+  else
+  {
+    std::memcpy(dst, pone, pone_size);                          // 复制环形缓冲区的array_one数据到dst中
+    std::memcpy(dst + pone_size, ptwo, num_bytes - pone_size);  // 复制环形缓冲区的array_two数据到dst中
+  }
+}
+
+}

src/hins/xingsong_driver.cpp

@@ -0,0 +1,235 @@
+#include "hins/xingsong_driver.h"
+
+// #define DEBUG
+
+namespace hins {
+
+XingSongDriver::XingSongDriver(const ConnectionAddress &conn_info)
+  : conn_info_(conn_info)
+  , data_receiver_ptr_(nullptr)
+{
+  guard_thread_ = std::thread(&XingSongDriver::RunMain, this);
+  shadows_filter_param_.max_angle = -1.0;
+}
+
+XingSongDriver::XingSongDriver(const ConnectionAddress &conn_info, ShadowsFilterParam shadows_filter_param)
+  : conn_info_(conn_info)
+  , data_receiver_ptr_(nullptr)
+  , shadows_filter_param_(shadows_filter_param)
+{ 
+  guard_thread_ = std::thread(&XingSongDriver::RunMain, this);
+}
+
+XingSongDriver::~XingSongDriver()
+{
+  if(data_receiver_ptr_)
+  {
+    delete data_receiver_ptr_;
+  }
+}
+
+bool XingSongDriver::StartCapturingTCP()
+{
+  if(data_receiver_ptr_)
+  {
+    delete data_receiver_ptr_;
+  }
+
+  data_receiver_ptr_ = new LaserDataReceiver(conn_info_);
+  if(!data_receiver_ptr_->IsConnected())
+  {
+    return false;
+  }
+  if(data_receiver_ptr_->SyncWrite() > 0)
+  {
+    return true;
+  }
+  return false;
+}
+
+
+/*
+* 防拖尾过滤参数初始化
+*/
+void XingSongDriver::ShadowsFilterInit(int scan_num)
+{
+  shadows_filter_threshold_max_.clear();
+  shadows_filter_threshold_min_.clear();
+
+  float max, min, min_angle, max_angle, angle_increment;
+  angle_increment = 2*M_PI/(float)scan_num;
+  int window;
+
+  switch (shadows_filter_param_.shadows_filter_level)
+  {
+    case 1:                                         // 快速，筛选角度大,搜索窗口小
+      min_angle = M_PI*5.0f/180.0f;                 // 转换为弧度制
+      max_angle = M_PI*175.0f/180.0f;               // 转换为弧度制
+      window = 1;
+      break;
+
+    case 2:                                         // 较慢，筛选角度大，搜索窗口适中
+      min_angle = M_PI*5.0f/180.0f;                 // 转换为弧度制
+      max_angle = M_PI*175.0f/180.0f;               // 转换为弧度制
+      window = 3;
+      break;
+
+    case 3:                                         // 较慢，筛选角度较小,搜索窗口大
+      min_angle = M_PI*15.0f/180.0f;                // 转换为弧度制
+      max_angle = M_PI*165.0f/180.0f;               // 转换为弧度制
+      window = 5;
+      break;
+    
+    default:                                                           // 按照shadows_filter_param_配置
+      min_angle = M_PI*shadows_filter_param_.min_angle/180.0f;         // 转换为弧度制
+      max_angle = M_PI*shadows_filter_param_.max_angle/180.0f;         // 转换为弧度制
+      window = shadows_filter_param_.window;
+
+      break;
+  }
+#ifdef DEBUG
+  std::cout << "shadows_filter_level:" << shadows_filter_param_.shadows_filter_level
+      << "  min_angle:" << shadows_filter_param_.min_angle
+      << "  max_angle:" << shadows_filter_param_.max_angle
+      << std::endl;
+  std::cout << "max:";
+#endif
+  for(int i=0;i < window;i++)
+  {
+    max = sin(max_angle)/sin(M_PI-max_angle-angle_increment*(i+1));
+    min = sin(min_angle)/sin(M_PI-min_angle-angle_increment*(i+1));
+
+    shadows_filter_threshold_max_.push_back(max);
+    shadows_filter_threshold_min_.push_back(min);
+    #ifdef DEBUG
+    std::cout << shadows_filter_threshold_max_.back() << "  ";
+    #endif
+  }  // if(data.distance_data.size() != scan_all_point_num_)
+  //   data.distance_data.clear();
+}
+
+
+/*
+* 防拖尾过滤
+*/
+void XingSongDriver::ShadowsFilter(ScanData& scan_data, int scan_num)
+{
+  #ifdef DEBUG
+  int del_num = 0;
+  #endif
+  // if(GetLaserSteadyTime() > 0)
+  // {
+    ShadowsFilterInit(scan_num);
+  // }
+  int search_index, search_index_tmp, del_index, del_index_tmp;
+  float a_b_rate;
+
+  // 清除上一次的删除点集合
+  shadows_del_index_.clear();
+  
+  // 每traverse_step根激光计算一次
+  for (int i = 0; i < scan_num; i += shadows_filter_param_.traverse_step)
+  {
+    if(scan_data.distance_data[i] > kMaxDistance)                        // 如果search_index_tmp激光超出范围，则跳过
+      continue;
+
+    for(search_index = i+1;                                              // 搜索[i,i+window]内是否存在拖影现象
+        search_index <= i + shadows_filter_param_.window;
+        search_index++)
+    {
+      search_index_tmp = search_index;  
+
+      // 边界
+      if(search_index_tmp < 0)                                                                        
+        search_index_tmp = 0;
+      else
+        if(search_index_tmp >= scan_num)
+          search_index_tmp = scan_num - 1;
+
+      if( (scan_data.distance_data[search_index_tmp] > kMaxDistance) ||                               // 如果search_index_tmp激光超出范围，则跳过
+          (scan_data.distance_data[i] > kMaxDistance)                  )
+        continue;
+
+      a_b_rate = (float)scan_data.distance_data[i]/(float)scan_data.distance_data[search_index_tmp];  // i与search_index激光比较
+      if( (a_b_rate < shadows_filter_threshold_min_[abs(search_index_tmp-i-1)]) ||                      // 如果存在拖影现象
+          (a_b_rate > shadows_filter_threshold_max_[abs(search_index_tmp-i-1)])   )        
+        if (scan_data.distance_data[i] < scan_data.distance_data[search_index_tmp])
+        {
+          #ifdef DEBUG
+          del_num++;
+          #endif
+          // scan_data.distance_data[search_index_tmp] = kMaxDistance+1;
+          shadows_del_index_.insert(search_index_tmp);
+
+        }
+        else
+        {
+          #ifdef DEBUG
+          del_num++;
+          #endif
+          // scan_data.distance_data[i] = kMaxDistance + 1;
+          shadows_del_index_.insert(i);
+        }
+    }
+  }
+
+  // 删除拖尾点
+  for(auto iter = shadows_del_index_.begin(); iter != shadows_del_index_.end(); ++iter) 
+  {
+    scan_data.distance_data[*iter] = kMaxDistance + 1;
+  }
+  #ifdef DEBUG
+  std::cout << "del_num:" << del_num << std::endl;
+  #endif
+}
+
+ScanData XingSongDriver::GetFullScan()
+{
+  if (data_receiver_ptr_)
+  {
+    if (shadows_filter_param_.max_angle < 0 || shadows_filter_param_.shadows_filter_level == 0)
+      return data_receiver_ptr_->GetFullScan();
+    ScanData fullscan = data_receiver_ptr_->GetFullScan();
+    int scan_num = fullscan.distance_data.size();
+    ShadowsFilter(fullscan, scan_num);                      // 过滤拖尾点
+    return fullscan;
+  }
+  else
+    return ScanData();
+}
+
+void XingSongDriver::RunMain()
+{
+  if(StartCapturingTCP())
+  {
+    std::cout << "兴颂雷达启动成功." << std::endl;
+  }
+  else
+  {
+    std::cout << "兴颂雷达启动失败，请检查网络连接." << std::endl;
+  }
+
+  while(true) // @todo: stop loop condition need to modified
+  {
+    if(IsConnected())
+    {
+      //
+    }
+    else
+    {
+      while(IsConnected()){
+        std::cout << "激光雷达断线，正在重新连接 ... " << std::endl;
+        if(StartCapturingTCP()){
+          std::cout << "激光雷达重新连接成功." << std::endl;
+        }
+        usleep(1000 * 50); // 50ms
+      }
+    }
+
+    usleep(1000*25); // 25ms
+  }
+
+  Disconnect();
+}
+
+}

src/main.cpp

@@ -0,0 +1,110 @@
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include "hins/xingsong_driver.h"
+
+using namespace hins;
+
+sensor_msgs::LaserScanPtr ToLaserScan(const ScanData &data, const float& start_angle, const float& end_angle, const std::string& frame_name);
+void ParamInit(ros::NodeHandle &nh_private ,ConnectionAddress &laser_conn_info, ShadowsFilterParam &shadows_filter_param);
+
+int main(int argc, char** argv){
+
+  // 1. ROS 初始化
+  ros::init(argc, argv, "hins_driver_laser");
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private("~");
+
+  float start_angle, end_angle;
+  std::string topic_name,frame_name;
+  nh_private.param<float>("start_angle", start_angle, 0.);
+  nh_private.param<float>("end_angle", end_angle, 2 * M_PI);
+  nh_private.param<std::string>("topic_name", topic_name, std::string("scan1"));
+  nh_private.param<std::string>("frame_name", frame_name, std::string("laser1"));
+
+  ROS_INFO("advertise: %s", topic_name.c_str());
+  ros::Publisher scan_pub = nh_.advertise<sensor_msgs::LaserScan>(topic_name, 3, true);
+
+  // 2. 雷达驱动
+  ConnectionAddress laser_conn_info;
+  ShadowsFilterParam shadows_filter_param;
+
+  // 初始化驱动参数
+  ParamInit(nh_private,laser_conn_info, shadows_filter_param);
+
+  // 启动雷达
+  XingSongDriverHdr driver_hdr = std::make_shared<XingSongDriver>(laser_conn_info,shadows_filter_param);
+
+  // 3. ros 消息发布
+  ros::Rate r(50);
+  while(ros::ok()){
+    ScanData data = driver_hdr->GetFullScan();
+    sensor_msgs::LaserScanPtr scan_msg_ptr = ToLaserScan(data, start_angle, end_angle, frame_name);
+    if(scan_msg_ptr->ranges.size() != 0)            // 如果数据长度有误，则不发布
+    {  
+      scan_pub.publish(scan_msg_ptr);
+    }
+    r.sleep();
+  }
+}
+
+sensor_msgs::LaserScanPtr ToLaserScan(const ScanData &data, const float& start_angle, const float& end_angle, const std::string& frame_name)
+{
+  sensor_msgs::LaserScanPtr ret = boost::make_shared<sensor_msgs::LaserScan>();
+
+  ret->header.frame_id = frame_name;
+  ret->header.stamp = ros::Time::now();
+
+  ret->range_min = 0.05;
+  ret->range_max = 60.0;
+  ret->angle_min = M_PI/2.0f;
+  ret->angle_max = 3.0 * M_PI/2.0f;
+
+  ret->time_increment = 1/20/float(data.distance_data.size());        // 一个周期时间除以扫描点数
+
+  ret->angle_increment = 1.5 * M_PI/float(data.distance_data.size());
+
+  ret->ranges.resize(data.distance_data.size(), 0);
+  ret->intensities.resize(data.amplitude_data.size(), 0);
+  
+  for( std::size_t i = 0; i < data.distance_data.size(); i++ )
+  {
+    float angle = ret->angle_min + i * ret->angle_increment;
+  
+    float dis = float(data.distance_data[i])/1000.0f;
+    
+    if(angle < start_angle || angle > end_angle)
+      ret->ranges[i]  = 1024.0;
+    else
+      ret->ranges[i] = dis;
+    ret->intensities[i] = data.amplitude_data[i];
+  }
+
+  return ret;
+}
+
+
+void ParamInit(ros::NodeHandle &nh_private ,ConnectionAddress &laser_conn_info, ShadowsFilterParam &shadows_filter_param)
+{
+  std::string server_address;
+  float start_angle, end_angle, shadows_filter_max_angle, shadows_filter_min_angle;
+  int port, shadows_filter_neighbors, shadows_filter_window, shadows_filter_level, shadows_filter_traverse_step;
+
+  nh_private.param("laser_ip", server_address, std::string("192.168.1.88"));
+  nh_private.param<int>("laser_port", port, 8080);
+  nh_private.param<float>("shadows_filter_max_angle", shadows_filter_max_angle, float(175.0));
+  nh_private.param<float>("shadows_filter_min_angle", shadows_filter_min_angle, float(5.0));
+  nh_private.param<int>("shadows_filter_neighbors", shadows_filter_neighbors, 1);
+  nh_private.param<int>("shadows_filter_window", shadows_filter_window, 2);
+  nh_private.param<int>("shadows_filter_traverse_step", shadows_filter_traverse_step, 1);
+  nh_private.param<int>("shadows_filter_level", shadows_filter_level, -1);                           // 默认按照具体参数配置
+
+  laser_conn_info.SetAddress(server_address);
+  laser_conn_info.SetPort(port);
+
+  shadows_filter_param.max_angle = shadows_filter_max_angle;
+  shadows_filter_param.min_angle = shadows_filter_min_angle;
+  shadows_filter_param.neighbors = shadows_filter_neighbors;
+  shadows_filter_param.window = shadows_filter_window;
+  shadows_filter_param.traverse_step = shadows_filter_traverse_step;
+  shadows_filter_param.shadows_filter_level = shadows_filter_level;
+}