commit 7b372aa0c4374a84c62507c3356454f7847a5d26
Author: HinSon <2636178756@qq.com>
Date: Mon Dec 27 17:23:19 2021 +0800
v2.1
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..a163563
--- /dev/null
+++ b/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)
+
diff --git a/CMakeLists.txt.user b/CMakeLists.txt.user
new file mode 100644
index 0000000..79b568f
--- /dev/null
+++ b/CMakeLists.txt.user
@@ -0,0 +1,375 @@
+
+
+
+
+
+ EnvironmentId
+ {0ecb8702-28d5-4134-b587-a0b8643a8bbb}
+
+
+ ProjectExplorer.Project.ActiveTarget
+ 0
+
+
+ ProjectExplorer.Project.EditorSettings
+
+ true
+ false
+ true
+
+ Cpp
+
+ CppGlobal
+
+
+
+ QmlJS
+
+ QmlJSGlobal
+
+
+ 2
+ UTF-8
+ false
+ 4
+ false
+ 80
+ true
+ false
+ 1
+ true
+ false
+ 0
+ true
+ true
+ 0
+ 8
+ true
+ 1
+ true
+ true
+ true
+ false
+
+
+
+ ProjectExplorer.Project.PluginSettings
+
+
+ true
+
+
+
+ ProjectExplorer.Project.Target.0
+
+ 桌面
+ 桌面
+ {d6473b3e-f747-4f26-ab57-f336a893711c}
+ 0
+ 0
+ 0
+
+
+ /home/develop/lead_develop/build-hins_driver_laser
+
+
+
+
+ all
+
+ true
+ CMake Build
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Build
+
+ ProjectExplorer.BuildSteps.Build
+
+
+
+
+
+ all
+
+ true
+ CMake Build
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Clean
+
+ ProjectExplorer.BuildSteps.Clean
+
+ 2
+ false
+
+ Default
+ Default
+ CMakeProjectManager.CMakeBuildConfiguration
+
+
+
+ CMAKE_BUILD_TYPE:STRING=Debug
+
+ /home/develop/lead_develop/build-hins_driver_laser
+
+
+
+
+ all
+
+ true
+ CMake Build
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Build
+
+ ProjectExplorer.BuildSteps.Build
+
+
+
+
+
+ all
+
+ true
+ CMake Build
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Clean
+
+ ProjectExplorer.BuildSteps.Clean
+
+ 2
+ false
+
+ Debug
+ Debug
+ CMakeProjectManager.CMakeBuildConfiguration
+
+
+
+ CMAKE_BUILD_TYPE:STRING=Release
+
+ /home/develop/lead_develop/build-hins_driver_laser
+
+
+
+
+ all
+
+ true
+ CMake Build
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Build
+
+ ProjectExplorer.BuildSteps.Build
+
+
+
+
+
+ all
+
+ true
+ CMake Build
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Clean
+
+ ProjectExplorer.BuildSteps.Clean
+
+ 2
+ false
+
+ Release
+ Release
+ CMakeProjectManager.CMakeBuildConfiguration
+
+
+
+ CMAKE_BUILD_TYPE:STRING=RelWithDebInfo
+
+ /home/develop/lead_develop/build-hins_driver_laser
+
+
+
+
+ all
+
+ true
+ CMake Build
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Build
+
+ ProjectExplorer.BuildSteps.Build
+
+
+
+
+
+ all
+
+ true
+ CMake Build
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Clean
+
+ ProjectExplorer.BuildSteps.Clean
+
+ 2
+ false
+
+ Release with Debug Information
+ Release with Debug Information
+ CMakeProjectManager.CMakeBuildConfiguration
+
+
+
+ CMAKE_BUILD_TYPE:STRING=MinSizeRel
+
+ /home/develop/lead_develop/build-hins_driver_laser
+
+
+
+
+ all
+
+ true
+ CMake Build
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Build
+
+ ProjectExplorer.BuildSteps.Build
+
+
+
+
+
+ all
+
+ true
+ CMake Build
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Clean
+
+ ProjectExplorer.BuildSteps.Clean
+
+ 2
+ false
+
+ Minimum Size Release
+ Minimum Size Release
+ CMakeProjectManager.CMakeBuildConfiguration
+
+ 5
+
+
+ 0
+ Deploy
+
+ ProjectExplorer.BuildSteps.Deploy
+
+ 1
+ Deploy Configuration
+
+ ProjectExplorer.DefaultDeployConfiguration
+
+ 1
+
+
+ false
+ false
+ 1000
+
+ true
+
+ false
+ false
+ false
+ false
+ true
+ 0.01
+ 10
+ true
+ 1
+ 25
+
+ 1
+ true
+ false
+ true
+ valgrind
+
+ 0
+ 1
+ 2
+ 3
+ 4
+ 5
+ 6
+ 7
+ 8
+ 9
+ 10
+ 11
+ 12
+ 13
+ 14
+
+ 2
+
+
+ Custom Executable
+
+ ProjectExplorer.CustomExecutableRunConfiguration
+
+ 3768
+ false
+ true
+ false
+ false
+ true
+
+
+
+ 1
+
+
+
+ ProjectExplorer.Project.TargetCount
+ 1
+
+
+ ProjectExplorer.Project.Updater.FileVersion
+ 20
+
+
+ Version
+ 20
+
+
diff --git a/include/connection_address.h b/include/connection_address.h
new file mode 100644
index 0000000..712b112
--- /dev/null
+++ b/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
+
+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_;
+};
+
+}
diff --git a/include/hins/laser_data_receiver.h b/include/hins/laser_data_receiver.h
new file mode 100644
index 0000000..7e88e60
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include "connection_address.h"
+#include "hins/protoc.h"
+
+namespace hins{
+
+class LaserDataReceiver;
+using LaserDataReceiverPtr = LaserDataReceiver*;
+using LaserDataReceiverHdr = std::shared_ptr;
+
+class LaserDataReceiver final
+{
+public:
+ LaserDataReceiver(const ConnectionAddress &conn_info);
+
+ ~LaserDataReceiver();
+
+ int SyncWrite();
+ int SyncWrite(const std::array 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 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 scan_data_; // Double ended queue with sucessfully received and parsed data, organized as single complete scans
+ std::deque area_data_;
+
+ uint64_t last_data_time_; // time in seconds since epoch, when last data was received
+};
+
+}
+
diff --git a/include/hins/protoc.h b/include/hins/protoc.h
new file mode 100644
index 0000000..d2312af
--- /dev/null
+++ b/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
+#include
+
+namespace hins {
+
+const std::array kStartCapture {0x52, 0x41, 0x75, 0x74, 0x6f, 0x01, 0x87, 0x80};
+
+struct ScanData
+{
+ // 距离数据
+ std::vector distance_data;
+
+ // 强度数据
+ std::vector amplitude_data;
+};
+
+struct AreaData
+{
+std::array 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()
+
+
+}
+
diff --git a/include/hins/xingsong_driver.h b/include/hins/xingsong_driver.h
new file mode 100644
index 0000000..429629e
--- /dev/null
+++ b/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
+namespace hins
+{
+
+ class XingSongDriver;
+ using XingSongDriverHdr = std::shared_ptr;
+
+ 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 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 shadows_filter_threshold_min_;
+ std::vector shadows_filter_threshold_max_;
+ set shadows_del_index_;
+ };
+
+
+}
diff --git a/include/utils.h b/include/utils.h
new file mode 100644
index 0000000..4e7e353
--- /dev/null
+++ b/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
+#include
+#include
+
+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
diff --git a/launch/hins_driver.launch~ b/launch/hins_driver.launch~
new file mode 100644
index 0000000..3215470
--- /dev/null
+++ b/launch/hins_driver.launch~
@@ -0,0 +1,24 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/hins_se_driver.launch b/launch/hins_se_driver.launch
new file mode 100644
index 0000000..7cf3f0e
--- /dev/null
+++ b/launch/hins_se_driver.launch
@@ -0,0 +1,24 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/laser.rviz b/launch/laser.rviz
new file mode 100644
index 0000000..2334a6b
--- /dev/null
+++ b/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:
+ 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:
+ 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
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000..81463f9
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,25 @@
+
+
+ hins_se_driver
+ 1.0.0
+ The hins navigation lidar driver package
+ Liankui Hu
+ HINS
+ http://xingsong.com
+ Liankui Hu
+
+ catkin
+ geometry_msgs
+ sensor_msgs
+ nav_msgs
+ roscpp
+ rospy
+ std_msgs
+
+ geometry_msgs
+ sensor_msgs
+ nav_msgs
+ roscpp
+ rospy
+ std_msgs
+
diff --git a/readme.txt b/readme.txt
new file mode 100644
index 0000000..4e5c894
--- /dev/null
+++ b/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个点是否存在拖尾现象
diff --git a/src/hins/laser_data_receiver.cpp b/src/hins/laser_data_receiver.cpp
new file mode 100644
index 0000000..842af62
--- /dev/null
+++ b/src/hins/laser_data_receiver.cpp
@@ -0,0 +1,449 @@
+#include "hins/laser_data_receiver.h"
+#include "hins/protoc.h"
+#include "utils.h"
+#include
+#include
+
+// #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 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 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 *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 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中
+ }
+}
+
+}
diff --git a/src/hins/xingsong_driver.cpp b/src/hins/xingsong_driver.cpp
new file mode 100644
index 0000000..354a43e
--- /dev/null
+++ b/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();
+}
+
+}
diff --git a/src/main.cpp b/src/main.cpp
new file mode 100644
index 0000000..b208345
--- /dev/null
+++ b/src/main.cpp
@@ -0,0 +1,110 @@
+#include
+#include
+#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("start_angle", start_angle, 0.);
+ nh_private.param("end_angle", end_angle, 2 * M_PI);
+ nh_private.param("topic_name", topic_name, std::string("scan1"));
+ nh_private.param("frame_name", frame_name, std::string("laser1"));
+
+ ROS_INFO("advertise: %s", topic_name.c_str());
+ ros::Publisher scan_pub = nh_.advertise(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(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();
+
+ 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("laser_port", port, 8080);
+ nh_private.param("shadows_filter_max_angle", shadows_filter_max_angle, float(175.0));
+ nh_private.param("shadows_filter_min_angle", shadows_filter_min_angle, float(5.0));
+ nh_private.param("shadows_filter_neighbors", shadows_filter_neighbors, 1);
+ nh_private.param("shadows_filter_window", shadows_filter_window, 2);
+ nh_private.param("shadows_filter_traverse_step", shadows_filter_traverse_step, 1);
+ nh_private.param("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;
+}
\ No newline at end of file