fix: driver LGA60

Every driver must normalize its device's native scale into ROS REP-103
right-handed CCW (0 = front, + = left). Spells out the three stackable
device quirks to absorb — shifted 0° reference, clockwise scale,
upside-down mounting — and warns that a CW scale decoded as CCW mirrors
the world invisibly on single-lidar devices: verify handedness against
the real room, not just against motion.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>

CMakeLists.txt

@@ -11,10 +11,6 @@ find_package(Threads REQUIRED)
 
 option(XLIDAR_BUILD_EXAMPLES "Build example/demo binaries" ON)
 
-# Where the rplidar plugin finds the Slamtec SDK sources (include/ + src/).
-# Empty + not auto-detected -> the rplidar plugin is skipped with a warning.
-set(XLIDAR_RPLIDAR_SDK_DIR "" CACHE PATH "Path to the Slamtec RPLIDAR SDK (dir containing include/ and src/)")
-
 add_subdirectory(src)
 add_subdirectory(plugins)
 

README.md

@@ -32,6 +32,8 @@ xlidar_driver/
 │   ├── driver_sick_tim/        # → driver_sick_tim.so    (sick_tim_driver)
 │   ├── driver_sick_safety/     # → driver_sick_safety.so (sick_nanoscan3_driver)
 │   └── driver_espe/            # → driver_espe.so        (espe_lga60_driver)
+├── third_party/
+│   └── rplidar_sdk/            # SDK Slamtec, build thẳng vào driver_rplidar.so
 ├── examples/                   # list_drivers, example, lidar_app
 └── docs/diagnostics.md         # nghiên cứu layout dữ liệu chẩn đoán
 ```
@@ -59,19 +61,16 @@ xlidar_driver/
 ## Cài đặt
 
 Yêu cầu: Linux, CMake ≥ 3.16, C++17. Không có dependency ngoài (pthread,
-dl). Plugin rplidar cần thêm source SDK của Slamtec.
+dl); SDK của hãng (Slamtec cho plugin rplidar) đã nằm sẵn trong
+`third_party/` — clone là build được.
 
 ```bash
-cmake -S . -B build -DCMAKE_BUILD_TYPE=Release \
-      -DXLIDAR_RPLIDAR_SDK_DIR=/path/to/rplidar_sdk   # dir chứa include/ + src/
+cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
 cmake --build build -j"$(nproc)"
 ```
 
 Kết quả: `build/src/liblidar_manager.so`, plugin trong `build/plugins/*.so`,
-demo trong `build/examples/`. Không truyền `XLIDAR_RPLIDAR_SDK_DIR` thì các
-vị trí quen thuộc được tự dò (`third_party/rplidar_sdk`,
-`../rplidar_sdk/sdk`, `../xloc-monorepo/xlocd/deps/rplidar_sdk`); không thấy
-SDK thì plugin rplidar bị bỏ qua, phần còn lại build bình thường.
+demo trong `build/examples/`.
 
 Tùy chọn: `-DXLIDAR_BUILD_EXAMPLES=OFF`. Hỗ trợ `cmake --install` +
 `find_package(xlidar_driver)` (target `xlidar::lidar_manager`).
@@ -265,6 +264,16 @@ không handshake CoLa2/TCP.
 −180°), preset −160…160°, range 0.05…50 m. Frame đo `HISN` (header 16 B
 big-endian; điểm = distance mm + intensity, distance 50 000 mm = không có
 phản hồi → ∞); frame vùng `WSimu` (nếu thiết bị gửi) được đọc lấy từ lỗi.
+Mỗi lượt quét được thiết bị chia thành NHIỀU packet `HISN`: các bộ đếm điểm
+trong header là của riêng packet đó, còn vòng quét là cửa sổ cố định
+20°→340° = 320°/bước điểm — driver ghép các packet theo `start_angle` và chỉ
+phát scan khi packet đóng ở 340°. Điểm không packet nào gửi = NaN. Bước góc
+lấy từ packet mở vòng (span/số điểm) rồi giữ nguyên.
+Chu kỳ quay đo host-side giữa hai vòng: `scan_time` là thời gian QUÉT
+(period × 320/360, đúng như driver ROS của hãng dùng để đóng dấu), nên spin
+rate suy ra từ `1/scan_time` cao hơn cơ khí 360/320; vòng đầu chưa đo được
+thì để 0. Trường `time` 16-bit trong header là bộ đếm thiết bị đơn vị chưa
+xác định (header của hãng ghi "chưa kích hoạt") → `timestamp_ms` = 0.
 Độ phân giải (0.025–0.5°), tốc độ quay, mức lọc nhiễu theo cấu hình đã nạp
 bằng tool Windows của hãng — driver không tự đổi. Chưa verify phần cứng.
 
@@ -290,7 +299,7 @@ comment đầu mỗi file plugin và [docs/diagnostics.md](docs/diagnostics.md).
 | `intensities` | Cường độ 0–255 |
 | `range_min` / `range_max` | Dải đo hợp lệ (m) |
 | `timestamp_ms` | Đồng hồ thiết bị (ms); 0 nếu giao thức không có |
-| `time_increment` / `scan_time` | Chỉ rplidar đo được (chu kỳ grab thực); driver khác = 0 |
+| `time_increment` / `scan_time` | Chỉ rplidar (chu kỳ grab thực) và ESPE (chu kỳ vòng quét, × 320/360) đo được; driver khác = 0 |
 
 `ExtraInfo`: metadata thô tuỳ giao thức — field thiết bị không có giữ
 `std::nullopt`:

include/lidar_interface.hpp

@@ -79,6 +79,21 @@ inline const char* to_string(ErrorCode e) {
 
 // ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds).
 // ranges[i] is at angle_min + i*angle_increment, in sweep order.
+//
+// ANGLE CONVENTION CONTRACT (ROS REP-103, right-handed): 0 = device front,
+// positive = LEFT, increasing counter-clockwise viewed from the top. Every
+// driver must NORMALIZE its device's native scale into this frame — three
+// independent, stackable device quirks to absorb:
+//   1. shifted 0° reference        -> ModelConfig::angle_offset_deg
+//      (OLEI LR-1F/1FMI/1BS5: 0° at the rear; SICK TiM: front at 90°);
+//   2. CLOCKWISE angle scale       -> mirror the raw angle (θ -> −θ) BEFORE
+//      the offset (all Slamtec RPLIDARs per the Interface Protocol spec;
+//      OLEI LR-1BS5, field-verified). Decoding a CW scale as CCW mirrors the
+//      whole world left/right — undetectable with a single lidar (the SLAM
+//      map is self-consistently mirrored and heading checks still pass), so
+//      verify handedness against the real room, not just against motion;
+//   3. upside-down mounting        -> the physical flip reverses the
+//      apparent rotation, one more angle negation (the driver's `inverted`).
 struct LaserScan {
     uint32_t timestamp_ms = 0;   // device clock (ms); 0 if not on the wire
     float angle_min       = 0.f; // rad

plugins/driver_espe/espe_driver.cpp

@@ -29,6 +29,25 @@ constexpr uint16_t kMaxIntensity    = 30000;
 constexpr uint32_t kMaxPointsPerRev = 12800; // 320° at the finest 0.025° step
 constexpr int      kConnectTimeoutMs = 2000;
 
+// The head measures over a fixed window of every turn — from 20° to 340° in
+// device angles, 0° at the rear — and is blind over the remaining 40°. Both
+// the revolution's size and every packet's position are anchored to that
+// window (the vendor ROS driver hard-codes the same two numbers). Consistent
+// with MODEL_ESPE_LGA60's -160…160 preset once angle_offset_deg (-180) is
+// applied.
+constexpr uint16_t kSweepStartDeg = 20;
+constexpr uint16_t kSweepEndDeg   = 340;
+constexpr float    kSweepSpanDeg  = static_cast<float>(kSweepEndDeg - kSweepStartDeg);
+constexpr float    kFullTurnDeg   = 360.f;
+
+// Rotation periods above this are stalls/reconnects, not a spin rate (the
+// device runs at 10 or 20 Hz): don't publish timing derived from them.
+constexpr float kMaxRevPeriodS = 1.f;
+
+// A point no packet ever delivered — "invalid", distinct from the infinity
+// that means the device looked and got no return.
+const float kMissingPoint = std::numeric_limits<float>::quiet_NaN();
+
 uint16_t be16(const uint8_t* p) {
     return static_cast<uint16_t>((p[0] << 8) | p[1]);
 }
@@ -69,8 +88,11 @@ ErrorCode EspeDriver::open() {
     }
 
     recv_buf_.clear();
+    pending_ranges_.clear();
+    pending_intensities_.clear();
+    angle_inc_deg_  = 0.f;
     points_total_   = 0;
-    pending_time_ = 0;
+    have_last_rev_  = false;
     scan_ready_     = false;
     espe_error_status_.reset();
     latest_diag_  = Diagnostics{};
@@ -147,6 +169,7 @@ bool EspeDriver::parse_buffer() {
             recv_buf_.erase(0, sizeof(kRangeMagic));  // bogus header — resync
             continue;
         }
+        // Both counters describe this packet; the clamp is the vendor's.
         if (data_size > measure_size) data_size = measure_size;
 
         size_t frame_size = kRangeHeaderSize + static_cast<size_t>(data_size) * 4;
@@ -161,39 +184,86 @@ bool EspeDriver::parse_buffer() {
     }
 }
 
-// Range frame: "HISN", then big-endian u16 start_angle, end_angle (deg),
-// data_size (points in this frame), data_position (cumulative points incl.
-// this frame), measure_size (points per revolution), time; then data_size ×
-// 4 B little-endian (u16 distance mm, u16 intensity).
-void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
-    uint16_t start_angle   = be16(frame + 4);
-    uint16_t end_angle     = be16(frame + 6);
-    uint16_t data_position = be16(frame + 10);
-    uint16_t measure_size  = be16(frame + 12);
-    pending_time_          = be16(frame + 14);
+// Latch the angular step and size the revolution around it. Kept stable once
+// latched: the wire angles are whole degrees, so a step re-derived from a
+// mid-sweep packet jitters, and re-sizing would drop the sweep in flight.
+void EspeDriver::set_resolution(float inc_deg) {
+    if (!(inc_deg > 0.f)) return;
+    const long total = std::lround(kSweepSpanDeg / inc_deg);
+    if (total < 2 || total > static_cast<long>(kMaxPointsPerRev)) return;  // implausible step
+    if (angle_inc_deg_ > 0.f && static_cast<uint32_t>(total) == points_total_) return;
 
-    // First frame of a revolution (or geometry changed) → start a new one.
-    if (points_total_ != measure_size || data_position <= data_size) {
-        points_total_   = measure_size;
-        rev_start_deg_  = static_cast<float>(start_angle);
-        angle_inc_deg_  = static_cast<float>(end_angle - start_angle) / measure_size;
-        pending_ranges_.assign(points_total_, 0.f);
+    angle_inc_deg_ = inc_deg;
+    points_total_  = static_cast<uint32_t>(total);
+    begin_revolution();
+}
+
+void EspeDriver::begin_revolution() {
+    if (points_total_ == 0) return;
+    pending_ranges_.assign(points_total_, kMissingPoint);
     pending_intensities_.assign(points_total_, 0.f);
 }
-    if (angle_inc_deg_ <= 0.f) { points_total_ = 0; return; }
 
-    // start_angle is normally constant across the revolution, so this is just
-    // the cumulative position; the angle term covers firmware that advances it.
-    int32_t begin = static_cast<int32_t>(std::lround(
-                        (static_cast<float>(start_angle) - rev_start_deg_) / angle_inc_deg_))
+// Range frame: "HISN", then big-endian u16 start_angle, end_angle (the
+// angular window THIS packet covers, whole degrees), data_size (points in
+// this packet's payload), data_position and measure_size (the vendor's
+// "position"/"count" of the current packet's points), time; then data_size ×
+// 4 B little-endian (u16 distance mm, u16 intensity).
+//
+// A packet is a slice of the sweep, not a revolution: the device splits every
+// 20°→340° sweep into several of them, the first opening at 20° and the last
+// closing at 340°. So the revolution holds 320°/step points, NOT measure_size
+// — reading measure_size as the revolution size (as this driver first did)
+// emits one scan per packet, each covering only that packet's few degrees.
+void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
+    const uint16_t start_angle   = be16(frame + 4);
+    const uint16_t end_angle     = be16(frame + 6);
+    const uint16_t data_position = be16(frame + 10);
+    const uint16_t measure_size  = be16(frame + 12);
+    // frame + 14 is a 16-bit device counter the vendor header marks as "time
+    // flag (not enabled)"; see finish_scan() for why it is not a timestamp.
+
+    // Step = this packet's angular span / its point count. Taken from the
+    // packet that opens a sweep — the one the vendor driver trusts — or from
+    // whatever arrives first while nothing is latched yet.
+    if (start_angle == kSweepStartDeg || angle_inc_deg_ <= 0.f) {
+        const float span = static_cast<float>(end_angle) - static_cast<float>(start_angle);
+        if (span > 0.f && measure_size > 0)
+            set_resolution(span / static_cast<float>(measure_size));
+    }
+    if (angle_inc_deg_ <= 0.f) return;  // step still unknown — nowhere to put the points
+
+    // First packet of a sweep: drop anything a lost closing packet left behind.
+    if (start_angle == kSweepStartDeg && data_position <= data_size) begin_revolution();
+
+    // Index of this packet's first point within the sweep, verbatim from the
+    // vendor driver: the angular offset from 20°, plus what the header's own
+    // counters carry. Exactly one of the two terms moves, whichever way the
+    // firmware numbers its packets — either start_angle walks the sweep while
+    // data_position stays at this packet's own count, or start_angle stays at
+    // 20° while data_position accumulates — so the sum is the packet's true
+    // start index in both cases.
+    const int32_t begin =
+        static_cast<int32_t>(std::lround(
+            (static_cast<float>(start_angle) - static_cast<float>(kSweepStartDeg)) / angle_inc_deg_))
         + static_cast<int32_t>(data_position) - static_cast<int32_t>(data_size);
 
+    // Integer wire angles make 320°/step land a point or two short of what the
+    // device actually streams; grow rather than clip the tail (the vendor
+    // driver does the same).
+    const size_t needed = static_cast<size_t>(begin > 0 ? begin : 0) + data_size;
+    if (needed > pending_ranges_.size() && needed <= kMaxPointsPerRev) {
+        pending_ranges_.resize(needed, kMissingPoint);
+        pending_intensities_.resize(needed, 0.f);
+        points_total_ = static_cast<uint32_t>(needed);
+    }
+
     const uint8_t* p = frame + kRangeHeaderSize;
     for (uint16_t i = 0; i < data_size; ++i, p += 4) {
-        int32_t idx = begin + i;
-        if (idx < 0 || idx >= static_cast<int32_t>(points_total_)) continue;
-        uint16_t dist  = le16(p + 0);
-        uint16_t inten = le16(p + 2);
+        const int32_t idx = begin + i;
+        if (idx < 0 || idx >= static_cast<int32_t>(pending_ranges_.size())) continue;
+        const uint16_t dist  = le16(p + 0);
+        const uint16_t inten = le16(p + 2);
         pending_ranges_[idx] = (dist > kMaxDistanceMm)
             ? std::numeric_limits<float>::infinity()
             : static_cast<float>(dist) * 1e-3f;  // mm -> m
@@ -203,22 +273,53 @@ void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
             * (255.f / kMaxIntensity);
     }
 
-    if (data_position >= points_total_) finish_scan();
+    // The packet that closes the sweep at 340°, with its point counter full,
+    // ends the revolution — the vendor driver's condition unchanged. It holds
+    // whichever way the firmware numbers packets: per-packet counters make
+    // data_position == measure_size true on every packet (so the 340° edge
+    // decides), cumulative ones make it true only on the sweep's last packet.
+    if (end_angle == kSweepEndDeg && data_position == measure_size) finish_scan();
 }
 
 void EspeDriver::finish_scan() {
+    if (pending_ranges_.size() < 2 || angle_inc_deg_ <= 0.f) return;
+
     LaserScan& scan = ready_result_.scan;
     scan = LaserScan{};
-    scan.timestamp_ms = pending_time_;  // header "time" field, unit unverified
+    // The header's 16-bit "time" field is a device counter of unverified unit
+    // (the vendor header calls it "not enabled" and its ROS driver never
+    // stamps a scan with it), while this field is contracted to be a device
+    // clock in ms — leave it at 0 and report timing from the rotation below.
+    scan.timestamp_ms = 0;
     scan.ranges      = std::move(pending_ranges_);
     scan.intensities = std::move(pending_intensities_);
-    scan.angle_min       = (rev_start_deg_ + cfg_.angle_offset_deg) * kDeg2Rad;
+    scan.angle_min       = (static_cast<float>(kSweepStartDeg) + cfg_.angle_offset_deg) * kDeg2Rad;
     scan.angle_increment = angle_inc_deg_ * kDeg2Rad;
     scan.angle_max       = scan.angle_min +
         scan.angle_increment * static_cast<float>(scan.ranges.size() - 1);
     scan.range_min = cfg_.range_min_m;
     scan.range_max = cfg_.range_max_m;
 
+    // Timing: revolutions complete one rotation period apart, but the points
+    // in one only span the 320° the head measures — the remaining 40° is dead
+    // time before the next sweep starts. scan_time is what a consumer
+    // subtracts from the arrival time to date the FIRST point, so it must be
+    // the sweep, not the period (the vendor ROS driver stamps with the same
+    // 320/360 factor). Consequence: a spin rate read back as 1/scan_time is
+    // 360/320 higher than the mechanical one. The first revolution has
+    // nothing to measure against — leave the fields at 0 and let the consumer
+    // fall back.
+    const auto now = std::chrono::steady_clock::now();
+    if (have_last_rev_) {
+        const float period = std::chrono::duration<float>(now - last_rev_end_).count();
+        if (period > 0.f && period < kMaxRevPeriodS) {
+            scan.scan_time      = period * (kSweepSpanDeg / kFullTurnDeg);
+            scan.time_increment = scan.scan_time / static_cast<float>(scan.ranges.size() - 1);
+        }
+    }
+    last_rev_end_  = now;
+    have_last_rev_ = true;
+
     finalize_scan(scan, cfg_, inverted_);
 
     ExtraInfo& info = ready_result_.info;
@@ -227,12 +328,9 @@ void EspeDriver::finish_scan() {
     info.espe_error_status = espe_error_status_;
 
     latest_diag_ = decode_diagnostics(info);
-    latest_diag_.device_timestamp_ms = scan.timestamp_ms;
     mark_scan_decoded();
 
-    pending_ranges_.clear();
-    pending_intensities_.clear();
-    points_total_ = 0;
+    begin_revolution();  // the vectors above were moved out — restore them
     scan_ready_ = true;
 }
 

plugins/driver_espe/espe_driver.hpp

@@ -2,6 +2,7 @@
 #pragma once
 #include "lidar_interface.hpp"
 
+#include <chrono>
 #include <cstdint>
 #include <optional>
 #include <string>
@@ -18,6 +19,11 @@ inline constexpr ModelConfig MODEL_ESPE_LGA60 { "ESPE-LGA60", -160.f, 160.f, 0.0
 // ROS driver; NOT verified on real hardware. open() sends the "RAuto" start
 // command; device parameters (spin rate, resolution, filters) are whatever
 // the vendor Windows config tool programmed — this driver does not set them.
+//
+// The device streams each 20°→340° sweep as several packets, so a scan is
+// assembled across packets and only completes when the sweep closes at 340°
+// (see handle_range_frame). Rotation timing is measured host-side: the wire
+// carries no usable clock.
 class EspeDriver : public LidarDriverInterface {
 public:
     // ip: device address; use_udp selects the transport the device is
@@ -52,6 +58,8 @@ private:
     bool fill_buffer(int timeout_ms);   // one recv() into recv_buf_
     bool parse_buffer();                 // consume frames; true when a scan completed
     void handle_range_frame(const uint8_t* frame, uint16_t data_size);
+    void set_resolution(float inc_deg);  // latch the step + (re)size the revolution
+    void begin_revolution();             // blank the buffer for a fresh sweep
     void finish_scan();
 
     ModelConfig  cfg_;
@@ -66,13 +74,19 @@ private:
     // Stream bytes carried across frame boundaries; per-instance.
     std::string recv_buf_;
 
-    // Per-revolution accumulation
+    // Per-revolution accumulation. A revolution is the device's fixed
+    // 20°→340° sweep, so it holds 320°/angle_inc_deg_ points — several
+    // packets' worth. The header's point counters describe one PACKET and say
+    // nothing about the revolution's size.
     std::vector<float> pending_ranges_;
     std::vector<float> pending_intensities_;
-    float    rev_start_deg_ = 0.f;  // device angle of the revolution's first point
-    float    angle_inc_deg_ = 0.f;
-    uint32_t points_total_  = 0;    // measure_size from the header; 0 = no rev open
-    uint16_t pending_time_  = 0;    // header "time" field, unit unverified
+    float    angle_inc_deg_ = 0.f;  // angular step; 0 = not latched yet
+    uint32_t points_total_  = 0;    // points per revolution = 320° / step
+
+    // Rotation period, measured host-side between completed revolutions: the
+    // wire carries no usable clock (see finish_scan()).
+    std::chrono::steady_clock::time_point last_rev_end_{};
+    bool                                  have_last_rev_ = false;
 
     // Latched from the newest "WSimu" area frame, if the device sends any.
     std::optional<uint16_t> espe_error_status_;

plugins/driver_olei/olei_driver.cpp

@@ -40,11 +40,41 @@ static constexpr uint16_t FRAME_ID_A = 0xFAF0;  // 2D Ethernet (VB, VF, LR-1F)
 static constexpr uint16_t FRAME_ID_B = 0xFEF0;  // LR-1BS5 / LR-1BS2 Ethernet variant
 static constexpr uint16_t FRAME_ID_C = 0xFEAC;  // Protocol V3 (GS1-5)
 
+// Models whose angle scale runs CLOCKWISE (left-handed): decoding them as CCW
+// mirrors the world left/right. Undetectable with a single lidar (the SLAM map
+// is self-consistently mirrored, and heading-vs-motion checks pass because a
+// mirror about x preserves "ahead") — it only surfaces when a second,
+// right-handed lidar disagrees, or when the map is compared against the real
+// room.
+//
+// Evidence:
+//  - LR-1BS5: field-verified 2026-07-23 (rotation-only decode produced a
+//    left-right mirrored map versus the actual room; mirrored decode matches).
+//  - The Family B azimuth protocol itself is clockwise per DF Automation's
+//    production ROS driver (github.com/dfautomation/ole2d,
+//    src/olelidar/src/decoder.cpp: walks the device array backwards with the
+//    comment "reverse, laserscan is anticlockwise"; its packet layout —
+//    azimuth x0.01 deg, invalid sentinel 0xFF00 — is exactly our Family B).
+//    LR-1FMI speaks the same Family B protocol, so it is listed too.
+//  - Family A (VB/VF/LR-1F) and Family C (GS1-5) units are unverified; they
+//    stay CCW until checked against a real room.
+static bool model_angles_clockwise(const char* name) {
+    return std::strcmp(name, "LR-1BS5") == 0 || std::strcmp(name, "LR-1FMI") == 0;
+}
+
 OleiDriver::OleiDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
                        bool inverted)
     : cfg_(cfg), ip_(ip), port_(port), inverted_(inverted)
 {
     auto_detect_ = (std::strcmp(cfg.name, "AUTO") == 0);
+    model_mirror_ = model_angles_clockwise(cfg.name);
+}
+
+// Device angle -> our CCW convention: clockwise models get their raw angle
+// negated BEFORE the model's 0°-reference offset is added (LR-1BS5: 0° at the
+// rear AND clockwise, so out = 180 − raw).
+float OleiDriver::device_deg(float raw_deg) const {
+    return (model_mirror_ ? -raw_deg : raw_deg) + cfg_.angle_offset_deg;
 }
 
 OleiDriver::~OleiDriver() { close(); }
@@ -238,7 +268,7 @@ bool OleiDriver::parse_family_a(const uint8_t* buf, int len) {
         uint8_t  intensity = blk[2];
 
         float frac  = (num_pts > 1) ? static_cast<float>(i) / (num_pts - 1) : 0.f;
-        float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start) + cfg_.angle_offset_deg);
+        float angle = to_signed_deg(device_deg(ang_start + frac * (ang_end - ang_start)));
         angle       = maybe_invert(angle, inverted_);
 
         if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
@@ -285,6 +315,7 @@ bool OleiDriver::parse_family_b(const uint8_t* buf, int len) {
                     cfg_.range_min_m      = entry.cfg->range_min_m;
                     cfg_.range_max_m      = entry.cfg->range_max_m;
                     cfg_.angle_offset_deg = entry.cfg->angle_offset_deg;
+                    model_mirror_         = model_angles_clockwise(entry.cfg->name);
                     break;
                 }
             }
@@ -308,7 +339,7 @@ bool OleiDriver::parse_family_b(const uint8_t* buf, int len) {
         }
         last_angle_ = dev_deg;
 
-        float    angle     = maybe_invert(to_signed_deg(angle_raw * 0.01f + cfg_.angle_offset_deg), inverted_);
+        float    angle     = maybe_invert(to_signed_deg(device_deg(angle_raw * 0.01f)), inverted_);
         float    dist_m    = le16(blk + 2) * scale_mm * 0.001f;
         uint8_t  intensity = static_cast<uint8_t>(le16(blk + 4) >> 2);   // 10-bit → 8-bit
 
@@ -397,7 +428,8 @@ bool OleiDriver::parse_family_c(const uint8_t* buf, int len) {
             range_mm = le16(blk + 2);
         }
 
-        float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f + cfg_.angle_offset_deg);
+        float angle = to_signed_deg(
+            device_deg(static_cast<float>(first_index + i) * angle_inc - 180.f));
         angle       = maybe_invert(angle, inverted_);
         if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
 

plugins/driver_olei/olei_driver.hpp

@@ -13,7 +13,7 @@ inline constexpr ModelConfig MODEL_VB     { "VB",      -135.f, 135.f, 0.05f, 30.
 inline constexpr ModelConfig MODEL_VF     { "VF",      -180.f, 180.f, 0.05f, 30.f };  // 2D 360°
 inline constexpr ModelConfig MODEL_LR1F   { "LR-1F",   -180.f, 180.f, 0.05f, 50.f, 180.f };  // 2D 360° 50m; device 0° = rear
 inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f };  // 2D 360° (Family B); device 0° = rear
-inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f };  // 2D 360° (Family B)
+inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f, 180.f };  // 2D 360° (Family B); device 0° = rear AND the angle scale runs CLOCKWISE — the driver mirrors it (out = 180 − raw, see model_angles_clockwise). Field-verified on OLELR-1BS5: offset 0 flipped the SLAM heading, rotation-only 180 then left-right mirrored the whole map.
 inline constexpr ModelConfig MODEL_LR16F  { "LR-16F",  -135.f, 135.f, 0.05f, 30.f };  // 3D 16 line
 inline constexpr ModelConfig MODEL_GS15   { "GS1-5",   -180.f, 180.f, 0.05f, 30.f };  // 2D 360° (Family C/V3)
 inline constexpr ModelConfig MODEL_AUTO   { "AUTO",    -180.f, 180.f, 0.05f, 30.f };
@@ -54,11 +54,18 @@ private:
 
     void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
     void flush_scan();
+    // Raw device angle -> our CCW convention (negated for clockwise-scale
+    // models, then the model's 0°-reference offset).
+    float device_deg(float raw_deg) const;
 
     ModelConfig  cfg_;
     std::string  ip_;
     uint16_t     port_;
     bool         inverted_  = false;
+    // Model's angle scale runs clockwise (e.g. LR-1BS5): device_deg() negates
+    // the raw angle so the output is right-handed. Set from the preset in the
+    // ctor and again when AUTO locks onto a detected model.
+    bool         model_mirror_ = false;
     int          sock_fd_   = -1;
     ScanCallback cb_;
 

plugins/driver_rplidar/CMakeLists.txt

@@ -1,35 +1,12 @@
-# The rplidar plugin compiles the vendor SDK straight into the plugin .so.
-# XLIDAR_RPLIDAR_SDK_DIR must point at a directory holding include/ + src/;
-# when unset, common local layouts are probed. Without an SDK the plugin is
-# skipped (the rest of the build is unaffected).
-
-if(NOT XLIDAR_RPLIDAR_SDK_DIR)
-    foreach(candidate
-            ${CMAKE_SOURCE_DIR}/third_party/rplidar_sdk
-            ${CMAKE_SOURCE_DIR}/../rplidar_sdk/sdk
-            ${CMAKE_SOURCE_DIR}/../xloc-monorepo/xlocd/deps/rplidar_sdk)
-        if(EXISTS ${candidate}/include/sl_lidar.h)
-            set(XLIDAR_RPLIDAR_SDK_DIR ${candidate})
-            break()
-        endif()
-    endforeach()
-endif()
-
-if(NOT XLIDAR_RPLIDAR_SDK_DIR OR NOT EXISTS ${XLIDAR_RPLIDAR_SDK_DIR}/include/sl_lidar.h)
-    message(WARNING "driver_rplidar: Slamtec SDK not found "
-                    "(set -DXLIDAR_RPLIDAR_SDK_DIR=...) — plugin skipped")
-    return()
-endif()
-
-message(STATUS "driver_rplidar: using SDK at ${XLIDAR_RPLIDAR_SDK_DIR}")
+# Compiles the vendored Slamtec SDK (third_party/rplidar_sdk) straight into
+# the plugin .so — no external dependency.
+set(RPLIDAR_SDK_DIR ${CMAKE_SOURCE_DIR}/third_party/rplidar_sdk)
 
 file(GLOB_RECURSE RPLIDAR_SDK_SOURCES CONFIGURE_DEPENDS
-    ${XLIDAR_RPLIDAR_SDK_DIR}/src/*.cpp)
-# The SDK ships win32/macOS arch files; keep Linux only.
-list(FILTER RPLIDAR_SDK_SOURCES EXCLUDE REGEX "arch/(win32|macOS)/")
+    ${RPLIDAR_SDK_DIR}/src/*.cpp)
 
 xlidar_add_plugin(driver_rplidar rplidar_driver.cpp ${RPLIDAR_SDK_SOURCES})
 target_include_directories(driver_rplidar SYSTEM PRIVATE
-    ${XLIDAR_RPLIDAR_SDK_DIR}/include
-    ${XLIDAR_RPLIDAR_SDK_DIR}/src
+    ${RPLIDAR_SDK_DIR}/include
+    ${RPLIDAR_SDK_DIR}/src
 )

plugins/driver_rplidar/rplidar_driver.cpp

@@ -1,11 +1,11 @@
-// Slamtec RPLIDAR over serial (C1 defaults), built on the vendor SDK
-// (sl_lidar.h). Ported from xlocd's embedded rplidar driver — the scan math
-// (angle/distance decoding, inversion, FOV window, NaN invalid points) is
-// kept identical.
+// Slamtec RPLIDAR over serial (C1 defaults), built on the vendored SDK
+// (third_party/rplidar_sdk, sl_lidar.h). Scan math: angle/distance decoding,
+// inversion, FOV window, invalid points as NaN.
 //
-// Unlike the network drivers, angles are reported in the DEVICE frame
-// [0, 2π), 0 = ahead, ascending — exactly what the SDK's ascendScanData
-// yields (and what xlocd's engine expects).
+// Unlike the network drivers, angles arrive in the DEVICE frame [0, 2π),
+// 0 = ahead, ascending (the SDK's ascendScanData order) — but Slamtec's
+// angle scale runs CLOCKWISE viewed from the top, so decode mirrors it into
+// our right-handed convention (see the handedness comment in recv_scan).
 #include "lidar_interface.hpp"
 #include "plugin_helpers.hpp"
 
@@ -200,14 +200,25 @@ public:
 
         LaserScan& scan = out.scan;
         scan = LaserScan{};
-        // Inverted mount -> mirror the angles (angle' = 2π - angle) and walk
-        // the nodes backwards to keep ascending order.
+        // Handedness: Slamtec devices count their angle CLOCKWISE viewed from
+        // the top (Interface Protocol spec; the official rplidar_ros node
+        // mirrors the angles for exactly this reason). Our output convention
+        // is right-handed (CCW, + = left), so:
+        //   - right-side-up (inverted_ == false): mirror (angle' = 2π − raw)
+        //     and walk the nodes backwards to keep ascending order;
+        //   - upside-down (inverted_ == true): the flip makes the rotation
+        //     appear CCW from above, so the raw ascending angles are already
+        //     right-handed — use them as-is.
+        // Field-verified (2026-07-23): a right-side-up C1 decoded as CCW
+        // produced a left-right mirrored sweep against a verified
+        // right-handed reference lidar; invisible with the C1 alone because
+        // a single-sensor SLAM map is self-consistently mirrored.
         if (inverted_) {
-            scan.angle_min = kTwoPi - angle_last;
-            scan.angle_max = kTwoPi - angle_first;
-        } else {
             scan.angle_min = angle_first;
             scan.angle_max = angle_last;
+        } else {
+            scan.angle_min = kTwoPi - angle_last;
+            scan.angle_max = kTwoPi - angle_first;
         }
         scan.angle_increment = (scan.angle_max - scan.angle_min) / static_cast<float>(count - 1);
         scan.scan_time       = scan_time;
@@ -222,7 +233,10 @@ public:
         scan.ranges.reserve(count);
         scan.intensities.reserve(count);
         for (std::size_t i = 0; i < count; ++i) {
-            const std::size_t node_index = inverted_ ? count - 1 - i : i;
+            // Mirrored branches walk backwards (see the handedness comment
+            // above): backwards for a right-side-up unit, forwards when the
+            // physical flip already reversed the apparent rotation.
+            const std::size_t node_index = inverted_ ? i : count - 1 - i;
             // dist = 0 is the SDK's "no return" sentinel; together with
             // out-of-range / out-of-window points it becomes NaN.
             const float distance = node_distance_m(nodes[node_index]);

third_party/rplidar_sdk/LICENSE

@@ -0,0 +1,23 @@
+Copyright (c) 2009 - 2014 RoboPeak Team (http://www.robopeak.com)
+Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. (http://www.slamtec.com)
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

third_party/rplidar_sdk/include/rplidar.h

@@ -0,0 +1,45 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include <vector>
+#include "hal/types.h"
+#include "rplidar_protocol.h"
+#include "rplidar_cmd.h"
+
+#include "rplidar_driver.h"
+
+#define RPLIDAR_SDK_VERSION "2.0.0"
+#define SLAMTEC_LIDAR_SDK_VERSION  SL_LIDAR_SDK_VERSION

third_party/rplidar_sdk/include/rplidar_cmd.h

@@ -0,0 +1,215 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014-2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "sl_lidar_cmd.h"
+#include "rplidar_protocol.h"
+
+// Commands
+//-----------------------------------------
+
+#define RPLIDAR_AUTOBAUD_MAGICBYTE     SL_LIDAR_AUTOBAUD_MAGICBYTE
+
+// Commands without payload and response
+#define RPLIDAR_CMD_STOP               SL_LIDAR_CMD_STOP
+#define RPLIDAR_CMD_SCAN               SL_LIDAR_CMD_SCAN
+#define RPLIDAR_CMD_FORCE_SCAN         SL_LIDAR_CMD_FORCE_SCAN
+#define RPLIDAR_CMD_RESET              SL_LIDAR_CMD_RESET
+
+
+// Commands without payload but have response
+#define RPLIDAR_CMD_GET_DEVICE_INFO          SL_LIDAR_CMD_GET_DEVICE_INFO
+#define RPLIDAR_CMD_GET_DEVICE_HEALTH        SL_LIDAR_CMD_GET_DEVICE_HEALTH
+
+#define RPLIDAR_CMD_GET_SAMPLERATE           SL_LIDAR_CMD_GET_SAMPLERATE //added in fw 1.17
+
+#define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL      SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL
+
+// Commands with payload but no response
+#define RPLIDAR_CMD_NEW_BAUDRATE_CONFIRM     SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM //added in fw 1.30
+
+// Commands with payload and have response
+#define RPLIDAR_CMD_EXPRESS_SCAN            SL_LIDAR_CMD_EXPRESS_SCAN //added in fw 1.17
+#define RPLIDAR_CMD_HQ_SCAN                 SL_LIDAR_CMD_HQ_SCAN //added in fw 1.24
+#define RPLIDAR_CMD_GET_LIDAR_CONF          SL_LIDAR_CMD_GET_LIDAR_CONF //added in fw 1.24
+#define RPLIDAR_CMD_SET_LIDAR_CONF          SL_LIDAR_CMD_SET_LIDAR_CONF //added in fw 1.24
+//add for A2 to set RPLIDAR motor pwm when using accessory board
+#define RPLIDAR_CMD_SET_MOTOR_PWM           SL_LIDAR_CMD_SET_MOTOR_PWM
+#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG      SL_LIDAR_CMD_GET_ACC_BOARD_FLAG
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+
+// Payloads
+// ------------------------------------------
+#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL      SL_LIDAR_EXPRESS_SCAN_MODE_NORMAL 
+#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE    SL_LIDAR_EXPRESS_SCAN_MODE_FIXANGLE  // won't been supported but keep to prevent build fail
+//for express working flag(extending express scan protocol)
+#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST                 SL_LIDAR_EXPRESS_SCAN_FLAG_BOOST 
+#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION    SL_LIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION
+
+//for ultra express working flag
+#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD                 SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_STD 
+#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY    SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY
+
+#define RPLIDAR_HQ_SCAN_FLAG_CCW            (0x1<<0)
+#define RPLIDAR_HQ_SCAN_FLAG_RAW_ENCODER    (0x1<<1)
+#define RPLIDAR_HQ_SCAN_FLAG_RAW_DISTANCE   (0x1<<2)
+
+typedef sl_lidar_payload_express_scan_t   rplidar_payload_express_scan_t;
+typedef sl_lidar_payload_hq_scan_t        rplidar_payload_hq_scan_t;
+typedef sl_lidar_payload_get_scan_conf_t  rplidar_payload_get_scan_conf_t;
+typedef sl_lidar_payload_motor_pwm_t      rplidar_payload_motor_pwm_t;
+typedef sl_lidar_payload_acc_board_flag_t rplidar_payload_acc_board_flag_t;
+typedef sl_lidar_payload_set_scan_conf_t  rplidar_payload_set_scan_conf_t;
+typedef sl_lidar_payload_new_bps_confirmation_t  rplidar_payload_new_bps_confirmation_t;
+
+// Response
+// ------------------------------------------
+#define RPLIDAR_ANS_TYPE_DEVINFO                                 SL_LIDAR_ANS_TYPE_DEVINFO
+#define RPLIDAR_ANS_TYPE_DEVHEALTH                               SL_LIDAR_ANS_TYPE_DEVHEALTH
+#define RPLIDAR_ANS_TYPE_MEASUREMENT                             SL_LIDAR_ANS_TYPE_MEASUREMENT
+// Added in FW ver 1.17
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED                    SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ                          SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ
+// Added in FW ver 1.17
+#define RPLIDAR_ANS_TYPE_SAMPLE_RATE                             SL_LIDAR_ANS_TYPE_SAMPLE_RATE
+//added in FW ver 1.23alpha
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA              SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA
+//added in FW ver 1.24
+#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF                          SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF
+#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF                          SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED              SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED        SL_LIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED
+
+#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG                          SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG
+
+#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK      SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK
+
+typedef sl_lidar_response_acc_board_flag_t rplidar_response_acc_board_flag_t;
+
+
+#define RPLIDAR_STATUS_OK                 SL_LIDAR_STATUS_OK
+#define RPLIDAR_STATUS_WARNING            SL_LIDAR_STATUS_WARNING
+#define RPLIDAR_STATUS_ERROR              SL_LIDAR_STATUS_ERROR
+
+#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT        SL_LIDAR_RESP_MEASUREMENT_SYNCBIT
+#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT  SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT
+#define RPLIDAR_RESP_HQ_FLAG_SYNCBIT            SL_LIDAR_RESP_HQ_FLAG_SYNCBIT
+#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT       SL_LIDAR_RESP_MEASUREMENT_CHECKBIT
+#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT    SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT
+
+typedef sl_lidar_response_sample_rate_t rplidar_response_sample_rate_t;
+typedef sl_lidar_response_measurement_node_t rplidar_response_measurement_node_t;
+
+//[distance_sync flags]
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK           SL_LIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK
+#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK        SL_LIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK
+
+typedef sl_lidar_response_cabin_nodes_t rplidar_response_cabin_nodes_t;
+
+
+#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1               SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1
+#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2               SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2
+#define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC                  SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC
+#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT              SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT
+
+
+typedef sl_lidar_response_capsule_measurement_nodes_t         rplidar_response_capsule_measurement_nodes_t;
+typedef sl_lidar_response_dense_cabin_nodes_t                 rplidar_response_dense_cabin_nodes_t;
+typedef sl_lidar_response_dense_capsule_measurement_nodes_t   rplidar_response_dense_capsule_measurement_nodes_t;
+typedef sl_lidar_response_ultra_dense_capsule_measurement_nodes_t rplidar_response_ultra_dense_capsule_measurement_nodes_t;
+// ext1 : x2 boost mode
+
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS     SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS   SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS
+
+typedef sl_lidar_response_ultra_cabin_nodes_t                      rplidar_response_ultra_cabin_nodes_t;
+typedef sl_lidar_response_ultra_capsule_measurement_nodes_t        rplidar_response_ultra_capsule_measurement_nodes_t;
+typedef sl_lidar_response_measurement_node_hq_t                    rplidar_response_measurement_node_hq_t;
+typedef sl_lidar_response_hq_capsule_measurement_nodes_t           rplidar_response_hq_capsule_measurement_nodes_t;
+
+
+#   define RPLIDAR_CONF_SCAN_COMMAND_STD            SL_LIDAR_CONF_SCAN_COMMAND_STD
+#   define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS        SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS
+#   define RPLIDAR_CONF_SCAN_COMMAND_HQ             SL_LIDAR_CONF_SCAN_COMMAND_HQ
+#   define RPLIDAR_CONF_SCAN_COMMAND_BOOST          SL_LIDAR_CONF_SCAN_COMMAND_BOOST
+#   define RPLIDAR_CONF_SCAN_COMMAND_STABILITY      SL_LIDAR_CONF_SCAN_COMMAND_STABILITY
+#   define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY    SL_LIDAR_CONF_SCAN_COMMAND_SENSITIVITY
+
+#define RPLIDAR_CONF_ANGLE_RANGE                    SL_LIDAR_CONF_ANGLE_RANGE
+#define RPLIDAR_CONF_DESIRED_ROT_FREQ               SL_LIDAR_CONF_DESIRED_ROT_FREQ
+#define RPLIDAR_CONF_SCAN_COMMAND_BITMAP            SL_LIDAR_CONF_SCAN_COMMAND_BITMAP
+#define RPLIDAR_CONF_MIN_ROT_FREQ                   SL_LIDAR_CONF_MIN_ROT_FREQ
+#define RPLIDAR_CONF_MAX_ROT_FREQ                   SL_LIDAR_CONF_MAX_ROT_FREQ
+#define RPLIDAR_CONF_MAX_DISTANCE                   SL_LIDAR_CONF_MAX_DISTANCE
+        
+#define RPLIDAR_CONF_SCAN_MODE_COUNT                SL_LIDAR_CONF_SCAN_MODE_COUNT
+#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE        SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE
+#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE         SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE
+#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE             SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE
+#define RPLIDAR_CONF_SCAN_MODE_TYPICAL              SL_LIDAR_CONF_SCAN_MODE_TYPICAL
+#define RPLIDAR_CONF_SCAN_MODE_NAME                 SL_LIDAR_CONF_SCAN_MODE_NAME
+#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP                 SL_LIDAR_EXPRESS_SCAN_STABILITY_BITMAP
+#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP               SL_LIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP
+#define RPLIDAR_CONF_LIDAR_STATIC_IP_ADDR           SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR
+#define RPLIDAR_CONF_LIDAR_MAC_ADDR                 SL_LIDAR_CONF_LIDAR_MAC_ADDR
+
+#define RPLIDAR_CONF_DETECTED_SERIAL_BPS            SL_LIDAR_CONF_DETECTED_SERIAL_BPS
+
+typedef sl_lidar_response_get_lidar_conf_t       rplidar_response_get_lidar_conf_t;
+typedef sl_lidar_response_set_lidar_conf_t       rplidar_response_set_lidar_conf_t;
+typedef sl_lidar_response_device_info_t          rplidar_response_device_info_t;
+typedef sl_lidar_response_device_health_t        rplidar_response_device_health_t;
+typedef sl_lidar_ip_conf_t                       rplidar_ip_conf_t;
+typedef sl_lidar_response_device_macaddr_info_t  rplidar_response_device_macaddr_info_t;
+
+// Definition of the variable bit scale encoding mechanism
+#define RPLIDAR_VARBITSCALE_X2_SRC_BIT  SL_LIDAR_VARBITSCALE_X2_SRC_BIT
+#define RPLIDAR_VARBITSCALE_X4_SRC_BIT  SL_LIDAR_VARBITSCALE_X4_SRC_BIT
+#define RPLIDAR_VARBITSCALE_X8_SRC_BIT  SL_LIDAR_VARBITSCALE_X8_SRC_BIT
+#define RPLIDAR_VARBITSCALE_X16_SRC_BIT SL_LIDAR_VARBITSCALE_X16_SRC_BIT
+
+#define RPLIDAR_VARBITSCALE_X2_DEST_VAL SL_LIDAR_VARBITSCALE_X2_DEST_VAL
+#define RPLIDAR_VARBITSCALE_X4_DEST_VAL SL_LIDAR_VARBITSCALE_X4_DEST_VAL
+#define RPLIDAR_VARBITSCALE_X8_DEST_VAL SL_LIDAR_VARBITSCALE_X8_DEST_VAL
+#define RPLIDAR_VARBITSCALE_X16_DEST_VAL SL_LIDAR_VARBITSCALE_X16_DEST_VAL
+
+#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_)   SL_LIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_)
+
+#if defined(_WIN32)
+#pragma pack()
+#endif

third_party/rplidar_sdk/include/rplidar_driver.h

@@ -0,0 +1,247 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "sl_lidar_driver.h"
+
+#ifndef __cplusplus
+#error "The RPlidar SDK requires a C++ compiler to be built"
+#endif
+
+
+namespace rp { namespace standalone{ namespace rplidar {
+    using namespace sl;
+    typedef LidarScanMode RplidarScanMode;
+
+enum {
+   DRIVER_TYPE_SERIALPORT = 0x0,
+   DRIVER_TYPE_TCP = 0x1,
+   DRIVER_TYPE_UDP = 0x2,
+};
+
+class RPlidarDriver {
+public:
+    enum {
+        DEFAULT_TIMEOUT = 2000, //2000 ms
+    };
+
+    enum {
+        MAX_SCAN_NODES = 8192,
+    };
+
+    enum {
+        LEGACY_SAMPLE_DURATION = 476,
+    };
+
+public:
+    /// Create an RPLIDAR Driver Instance
+    /// This interface should be invoked first before any other operations
+    ///
+    /// \param drivertype the connection type used by the driver. 
+    static RPlidarDriver * CreateDriver(_u32 drivertype = CHANNEL_TYPE_SERIALPORT);
+    
+
+    RPlidarDriver(sl_u32 channelType);
+
+    /// Dispose the RPLIDAR Driver Instance specified by the drv parameter
+    /// Applications should invoke this interface when the driver instance is no longer used in order to free memory
+    static void DisposeDriver(RPlidarDriver * drv);
+
+    /// Open the specified serial port and connect to a target RPLIDAR device
+    ///
+    /// \param port_path     the device path of the serial port 
+    ///        e.g. on Windows, it may be com3 or \\.\com10 
+    ///             on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc
+    ///
+    /// \param baudrate      the baudrate used
+    ///        For most RPLIDAR models, the baudrate should be set to 115200
+    ///
+    /// \param flag          other flags
+    ///        Reserved for future use, always set to Zero
+    u_result connect(const char *path, _u32 portOrBaud, _u32 flag = 0);
+    
+    /// Disconnect with the RPLIDAR and close the serial port
+    void disconnect();
+
+    /// Returns TRUE when the connection has been established
+    bool isConnected(); 
+
+    /// Ask the RPLIDAR core system to reset it self
+    /// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode.
+    ///
+    ///  \param timeout       The operation timeout value (in millisecond) for the serial port communication                     
+    u_result reset(_u32 timeout = DEFAULT_TIMEOUT);
+
+    u_result clearNetSerialRxCache() {
+        return RESULT_OK;
+    }
+    // FW1.24
+    /// Get all scan modes that supported by lidar
+    u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT);
+   
+    /// Get typical scan mode of lidar
+    u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT);
+
+    /// Start scan
+    ///
+    /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+    /// \param useTypicalScan   Use lidar's typical scan mode or use the compatibility mode (2k sps)
+    /// \param options          Scan options (please use 0)
+    /// \param outUsedScanMode  The scan mode selected by lidar
+    u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL);
+
+    /// Start scan in specific mode
+    ///
+    /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+    /// \param scanMode         The scan mode id (use getAllSupportedScanModes to get supported modes)
+    /// \param options          Scan options (please use 0)
+    /// \param outUsedScanMode  The scan mode selected by lidar
+    u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT);
+
+    /// Retrieve the health status of the RPLIDAR
+    /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode.
+    ///
+    /// \param health        The health status info returned from the RPLIDAR
+    ///
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication     
+    u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT);
+
+    /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.
+    /// 
+    /// \param info          The device information returned from the RPLIDAR
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication  
+    u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT);
+
+    /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only.
+    /// 
+    /// \param pwm           The motor pwm value would like to set 
+    u_result setMotorPWM(_u16 pwm);
+
+    /// Start RPLIDAR's motor when using accessory board
+    u_result startMotor();
+
+    /// Stop RPLIDAR's motor when using accessory board
+    u_result stopMotor();
+
+    /// Check whether the device support motor control.
+    /// Note: this API will disable grab.
+    /// 
+    /// \param support       Return the result.
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication. 
+    u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT);
+
+	///Set LPX and S2E series lidar's static IP address
+	///
+	/// \param conf             Network parameter that LPX series lidar owned
+	/// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
+	u_result  setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT);
+
+    ///Get LPX and S2E series lidar's static IP address
+    ///
+    /// \param conf             Network parameter that LPX series lidar owned
+    /// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
+    u_result  getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT);
+
+	///Get LPX and S2E series lidar's MAC address
+	///
+	/// \param macAddrArray         The device MAC information returned from the LPX series lidar
+	u_result getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs = DEFAULT_TIMEOUT);
+
+    /// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated
+    ///
+    /// \param timeout       The operation timeout value (in millisecond) for the serial port communication 
+    u_result stop(_u32 timeout = DEFAULT_TIMEOUT);
+
+    /// Wait and grab a complete 0-360 degree scan data previously received. 
+    /// The grabbed scan data returned by this interface always has the following charactistics:
+    ///
+    /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1
+    /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan
+    /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+    ///
+    /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+    ///                       Once the interface returns, this parameter will store the actual received data count.
+    ///
+    /// \param timeout        Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely.
+    ///
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 
+    ///
+    /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
+    u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT);
+
+    /// Ascending the scan data according to the angle value in the scan.
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData
+    ///
+    /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+    ///                       Once the interface returns, this parameter will store the actual received data count.
+    /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. 
+    u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count);
+
+    /// Return received scan points even if it's not complete scan
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+    ///
+    /// \param count          Once the interface returns, this parameter will store the actual received data count.
+    ///
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 
+    u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count);
+
+    /// Return received scan points even if it's not complete scan
+    ///
+    /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+    ///
+    /// \param count          Once the interface returns, this parameter will store the actual received data count.
+    ///
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 
+    u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count);
+
+
+    virtual ~RPlidarDriver();
+protected:
+    RPlidarDriver();
+
+private:
+    sl_u32 _channelType;
+    IChannel* _channel;
+    ILidarDriver* _lidarDrv;
+    
+};
+
+
+
+
+}}}

third_party/rplidar_sdk/include/rplidar_protocol.h

@@ -0,0 +1,61 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+#include "sl_lidar_protocol.h"
+// RP-Lidar Input Packets
+
+#define RPLIDAR_CMD_SYNC_BYTE        SL_LIDAR_CMD_SYNC_BYTE
+#define RPLIDAR_CMDFLAG_HAS_PAYLOAD  SL_LIDAR_CMDFLAG_HAS_PAYLOAD
+
+
+#define RPLIDAR_ANS_SYNC_BYTE1       SL_LIDAR_ANS_SYNC_BYTE1
+#define RPLIDAR_ANS_SYNC_BYTE2       SL_LIDAR_ANS_SYNC_BYTE2
+
+#define RPLIDAR_ANS_PKTFLAG_LOOP     SL_LIDAR_ANS_PKTFLAG_LOOP
+
+#define RPLIDAR_ANS_HEADER_SIZE_MASK        SL_LIDAR_ANS_HEADER_SIZE_MASK
+#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT    SL_LIDAR_ANS_HEADER_SUBTYPE_SHIFT
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+typedef sl_lidar_cmd_packet_t rplidar_cmd_packet_t;
+typedef sl_lidar_ans_header_t rplidar_ans_header_t;
+
+
+#if defined(_WIN32)
+#pragma pack()
+#endif

third_party/rplidar_sdk/include/rptypes.h

@@ -0,0 +1,116 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+
+#ifdef _WIN32
+
+//fake stdint.h for VC only
+
+typedef signed   char     int8_t;
+typedef unsigned char     uint8_t;
+
+typedef __int16           int16_t;
+typedef unsigned __int16  uint16_t;
+
+typedef __int32           int32_t;
+typedef unsigned __int32  uint32_t;
+
+typedef __int64           int64_t;
+typedef unsigned __int64  uint64_t;
+
+#else
+
+#include <stdint.h>
+
+#endif
+
+
+//based on stdint.h
+typedef int8_t         _s8;
+typedef uint8_t        _u8;
+
+typedef int16_t        _s16;
+typedef uint16_t       _u16;
+
+typedef int32_t        _s32;
+typedef uint32_t       _u32;
+
+typedef int64_t        _s64;
+typedef uint64_t       _u64;
+
+#define __small_endian
+
+#ifndef __GNUC__
+#define __attribute__(x)
+#endif
+
+
+// The _word_size_t uses actual data bus width of the current CPU
+#ifdef _AVR_
+typedef _u8            _word_size_t;
+#define THREAD_PROC    
+#elif defined (WIN64)
+typedef _u64           _word_size_t;
+#define THREAD_PROC    __stdcall
+#elif defined (WIN32)
+typedef _u32           _word_size_t;
+#define THREAD_PROC    __stdcall
+#elif defined (__GNUC__)
+typedef unsigned long  _word_size_t;
+#define THREAD_PROC   
+#elif defined (__ICCARM__)
+typedef _u32            _word_size_t;
+#define THREAD_PROC  
+#endif
+
+
+typedef uint32_t u_result;
+
+#define RESULT_OK              0
+#define RESULT_FAIL_BIT        0x80000000
+#define RESULT_ALREADY_DONE    0x20
+#define RESULT_INVALID_DATA    (0x8000 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_FAIL  (0x8001 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_TIMEOUT  (0x8002 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_STOP    (0x8003 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_NOT_SUPPORT    (0x8004 | RESULT_FAIL_BIT)
+#define RESULT_FORMAT_NOT_SUPPORT    (0x8005 | RESULT_FAIL_BIT)
+#define RESULT_INSUFFICIENT_MEMORY   (0x8006 | RESULT_FAIL_BIT)
+
+#define IS_OK(x)    ( ((x) & RESULT_FAIL_BIT) == 0 )
+#define IS_FAIL(x)  ( ((x) & RESULT_FAIL_BIT) )
+
+typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * );

third_party/rplidar_sdk/include/sl_crc.h

@@ -0,0 +1,43 @@
+/*
+* Slamtec LIDAR SDK
+*
+* sl_crc.h
+*
+* Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+*/
+
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "sl_lidar_cmd.h"
+
+namespace sl {namespace crc32 {
+    sl_u32 bitrev(sl_u32 input, sl_u16 bw);//reflect
+    void init(sl_u32 poly); // table init
+    sl_u32 cal(sl_u32 crc, void* input, sl_u16 len);
+    sl_result getResult(sl_u8 *ptr, sl_u32 len);
+}}

third_party/rplidar_sdk/include/sl_lidar.h

@@ -0,0 +1,47 @@
+/*
+* Slamtec LIDAR SDK
+*
+* sl_lidar.h
+*
+* Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+*/
+
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "sl_lidar_driver.h"
+
+#define SL_LIDAR_SDK_VERSION_MAJOR  2
+#define SL_LIDAR_SDK_VERSION_MINOR  1
+#define SL_LIDAR_SDK_VERSION_PATCH  0
+#define SL_LIDAR_SDK_VERSION_SEQ    ((SL_LIDAR_SDK_VERSION_MAJOR << 16) | (SL_LIDAR_SDK_VERSION_MINOR << 8) | SL_LIDAR_SDK_VERSION_PATCH)
+
+
+#define SL_LIDAR_SDK_VERSION_MK_STR_INDIR(x)  #x
+#define SL_LIDAR_SDK_VERSION_MK_STR(x)        SL_LIDAR_SDK_VERSION_MK_STR_INDIR(x)
+
+#define SL_LIDAR_SDK_VERSION        (SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_MAJOR) "." SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_MINOR) "." SL_LIDAR_SDK_VERSION_MK_STR(SL_LIDAR_SDK_VERSION_PATCH))

third_party/rplidar_sdk/include/sl_lidar_cmd.h

@@ -0,0 +1,388 @@
+/*
+* Slamtec LIDAR SDK
+*
+* sl_lidar_cmd.h
+*
+* Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+*/
+
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4200)
+#endif
+
+#include "sl_lidar_protocol.h"
+
+ // Commands
+ //-----------------------------------------
+
+
+#define SL_LIDAR_AUTOBAUD_MAGICBYTE         0x41
+
+ // Commands without payload and response
+#define SL_LIDAR_CMD_STOP                   0x25
+#define SL_LIDAR_CMD_SCAN                   0x20
+#define SL_LIDAR_CMD_FORCE_SCAN             0x21
+#define SL_LIDAR_CMD_RESET                  0x40
+
+// Commands with payload but no response
+#define SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM   0x90 //added in fw 1.30
+
+// Commands without payload but have response
+#define SL_LIDAR_CMD_GET_DEVICE_INFO        0x50
+#define SL_LIDAR_CMD_GET_DEVICE_HEALTH      0x52
+
+#define SL_LIDAR_CMD_GET_SAMPLERATE         0x59 //added in fw 1.17
+
+#define SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL    0xA8
+
+
+// Commands with payload and have response
+#define SL_LIDAR_CMD_EXPRESS_SCAN           0x82 //added in fw 1.17
+#define SL_LIDAR_CMD_HQ_SCAN                0x83 //added in fw 1.24
+#define SL_LIDAR_CMD_GET_LIDAR_CONF         0x84 //added in fw 1.24
+#define SL_LIDAR_CMD_SET_LIDAR_CONF         0x85 //added in fw 1.24
+//add for A2 to set RPLIDAR motor pwm when using accessory board
+#define SL_LIDAR_CMD_SET_MOTOR_PWM          0xF0
+#define SL_LIDAR_CMD_GET_ACC_BOARD_FLAG     0xFF
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+
+// Payloads
+// ------------------------------------------
+#define SL_LIDAR_EXPRESS_SCAN_MODE_NORMAL      0 
+#define SL_LIDAR_EXPRESS_SCAN_MODE_FIXANGLE    0  // won't been supported but keep to prevent build fail
+//for express working flag(extending express scan protocol)
+#define SL_LIDAR_EXPRESS_SCAN_FLAG_BOOST                 0x0001 
+#define SL_LIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION    0x0002
+
+//for ultra express working flag
+#define SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_STD                 0x0001 
+#define SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY    0x0002
+
+typedef struct _sl_lidar_payload_express_scan_t
+{
+    sl_u8   working_mode;
+    sl_u16  working_flags;
+    sl_u16  param;
+} __attribute__((packed)) sl_lidar_payload_express_scan_t;
+
+typedef struct _sl_lidar_payload_hq_scan_t
+{
+    sl_u8  flag;
+    sl_u8   reserved[32];
+} __attribute__((packed)) sl_lidar_payload_hq_scan_t;
+
+typedef struct _sl_lidar_payload_get_scan_conf_t
+{
+    sl_u32  type;
+} __attribute__((packed)) sl_lidar_payload_get_scan_conf_t;
+
+typedef struct _sl_payload_set_scan_conf_t {
+    sl_u32  type;
+} __attribute__((packed)) sl_lidar_payload_set_scan_conf_t;
+
+
+#define DEFAULT_MOTOR_SPEED         (0xFFFFu)
+
+typedef struct _sl_lidar_payload_motor_pwm_t
+{
+    sl_u16 pwm_value;
+} __attribute__((packed)) sl_lidar_payload_motor_pwm_t;
+
+typedef struct _sl_lidar_payload_acc_board_flag_t
+{
+    sl_u32 reserved;
+} __attribute__((packed)) sl_lidar_payload_acc_board_flag_t;
+
+typedef struct _sl_lidar_payload_hq_spd_ctrl_t {
+    sl_u16  rpm;
+} __attribute__((packed))sl_lidar_payload_hq_spd_ctrl_t;
+
+
+typedef struct _sl_lidar_payload_new_bps_confirmation_t {
+    sl_u16   flag; // reserved, must be 0x5F5F
+    sl_u32  required_bps;
+    sl_u16  param;
+} __attribute__((packed)) sl_lidar_payload_new_bps_confirmation_t;
+
+// Response
+// ------------------------------------------
+#define SL_LIDAR_ANS_TYPE_DEVINFO          0x4
+#define SL_LIDAR_ANS_TYPE_DEVHEALTH        0x6
+
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT                       0x81
+// Added in FW ver 1.17
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED              0x82
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ                    0x83
+//added in FW ver 1.23alpha
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA        0x84
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED        0x85
+#define SL_LIDAR_ANS_TYPE_MEASUREMENT_ULTRA_DENSE_CAPSULED  0x86
+
+
+// Added in FW ver 1.17
+#define SL_LIDAR_ANS_TYPE_SAMPLE_RATE      0x15
+
+//added in FW ver 1.24
+#define SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF     0x20
+#define SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF     0x21
+
+
+#define SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG   0xFF
+
+#define SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK      (0x1)
+typedef struct _sl_lidar_response_acc_board_flag_t
+{
+    sl_u32 support_flag;
+} __attribute__((packed)) sl_lidar_response_acc_board_flag_t;
+
+
+#define SL_LIDAR_STATUS_OK                 0x0
+#define SL_LIDAR_STATUS_WARNING            0x1
+#define SL_LIDAR_STATUS_ERROR              0x2
+
+#define SL_LIDAR_RESP_MEASUREMENT_SYNCBIT        (0x1<<0)
+#define SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT  2
+
+#define SL_LIDAR_RESP_HQ_FLAG_SYNCBIT               (0x1<<0)
+
+#define SL_LIDAR_RESP_MEASUREMENT_CHECKBIT       (0x1<<0)
+#define SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT    1
+
+typedef struct _sl_lidar_response_sample_rate_t
+{
+    sl_u16  std_sample_duration_us;
+    sl_u16  express_sample_duration_us;
+} __attribute__((packed)) sl_lidar_response_sample_rate_t;
+
+typedef struct _sl_lidar_response_measurement_node_t
+{
+    sl_u8    sync_quality;      // syncbit:1;syncbit_inverse:1;quality:6;
+    sl_u16   angle_q6_checkbit; // check_bit:1;angle_q6:15;
+    sl_u16   distance_q2;
+} __attribute__((packed)) sl_lidar_response_measurement_node_t;
+
+//[distance_sync flags]
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK           (0x3)
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK        (0xFC)
+
+typedef struct _sl_lidar_response_cabin_nodes_t
+{
+    sl_u16   distance_angle_1; // see [distance_sync flags]
+    sl_u16   distance_angle_2; // see [distance_sync flags]
+    sl_u8    offset_angles_q3;
+} __attribute__((packed)) sl_lidar_response_cabin_nodes_t;
+
+
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1               0xA
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2               0x5
+
+#define SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC                  0xA5
+
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT              (0x1<<15)
+
+typedef struct _sl_lidar_response_capsule_measurement_nodes_t
+{
+    sl_u8                             s_checksum_1; // see [s_checksum_1]
+    sl_u8                             s_checksum_2; // see [s_checksum_1]
+    sl_u16                            start_angle_sync_q6;
+    sl_lidar_response_cabin_nodes_t  cabins[16];
+} __attribute__((packed)) sl_lidar_response_capsule_measurement_nodes_t;
+
+typedef struct _sl_lidar_response_dense_cabin_nodes_t
+{
+    sl_u16   distance;
+} __attribute__((packed)) sl_lidar_response_dense_cabin_nodes_t;
+
+typedef struct _sl_lidar_response_dense_capsule_measurement_nodes_t
+{
+    sl_u8                             s_checksum_1; // see [s_checksum_1]
+    sl_u8                             s_checksum_2; // see [s_checksum_1]
+    sl_u16                            start_angle_sync_q6;
+    sl_lidar_response_dense_cabin_nodes_t  cabins[40];
+} __attribute__((packed)) sl_lidar_response_dense_capsule_measurement_nodes_t;
+
+
+typedef struct _sl_lidar_response_ultra_dense_cabin_nodes_t {
+    sl_u16  qualityl_distance_scale[2];
+    sl_u8   qualityh_array;
+} __attribute__((packed)) sl_lidar_response_ultra_dense_cabin_nodes_t;
+
+typedef struct _sl_lidar_response_ultra_dense_capsule_measurement_nodes_t {
+    sl_u8                             s_checksum_1; // see [s_checksum_1]
+    sl_u8                             s_checksum_2; // see [s_checksum_1]
+    sl_u32                            time_stamp;
+    sl_u16                            dev_status;
+    sl_u16                            start_angle_sync_q6;
+    sl_lidar_response_ultra_dense_cabin_nodes_t  cabins[32];
+} __attribute__((packed)) sl_lidar_response_ultra_dense_capsule_measurement_nodes_t;
+
+
+// ext1 : x2 boost mode
+
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS     12
+#define SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS   10
+
+typedef struct _sl_lidar_response_ultra_cabin_nodes_t
+{
+    // 31                                              0
+    // | predict2 10bit | predict1 10bit | major 12bit |
+    sl_u32 combined_x3;
+} __attribute__((packed)) sl_lidar_response_ultra_cabin_nodes_t;
+
+typedef struct _sl_lidar_response_ultra_capsule_measurement_nodes_t
+{
+    sl_u8                             s_checksum_1; // see [s_checksum_1]
+    sl_u8                             s_checksum_2; // see [s_checksum_1]
+    sl_u16                            start_angle_sync_q6;
+    sl_lidar_response_ultra_cabin_nodes_t  ultra_cabins[32];
+} __attribute__((packed)) sl_lidar_response_ultra_capsule_measurement_nodes_t;
+
+typedef struct sl_lidar_response_measurement_node_hq_t
+{
+    sl_u16   angle_z_q14;
+    sl_u32   dist_mm_q2;
+    sl_u8    quality;
+    sl_u8    flag;
+} __attribute__((packed)) sl_lidar_response_measurement_node_hq_t;
+
+typedef struct _sl_lidar_response_hq_capsule_measurement_nodes_t
+{
+    sl_u8 sync_byte;
+    sl_u64 time_stamp;
+    sl_lidar_response_measurement_node_hq_t node_hq[96];
+    sl_u32  crc32;
+}__attribute__((packed)) sl_lidar_response_hq_capsule_measurement_nodes_t;
+
+
+#   define SL_LIDAR_CONF_SCAN_COMMAND_STD            0
+#   define SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS        1
+#   define SL_LIDAR_CONF_SCAN_COMMAND_HQ             2
+#   define SL_LIDAR_CONF_SCAN_COMMAND_BOOST          3
+#   define SL_LIDAR_CONF_SCAN_COMMAND_STABILITY      4
+#   define SL_LIDAR_CONF_SCAN_COMMAND_SENSITIVITY    5
+
+#define SL_LIDAR_CONF_ANGLE_RANGE                    0x00000000
+#define SL_LIDAR_CONF_DESIRED_ROT_FREQ               0x00000001
+#define SL_LIDAR_CONF_SCAN_COMMAND_BITMAP            0x00000002
+#define SL_LIDAR_CONF_MIN_ROT_FREQ                   0x00000004
+#define SL_LIDAR_CONF_MAX_ROT_FREQ                   0x00000005
+#define SL_LIDAR_CONF_MAX_DISTANCE                   0x00000060
+
+#define SL_LIDAR_CONF_SCAN_MODE_COUNT                0x00000070
+#define SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE        0x00000071
+#define SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE         0x00000074
+#define SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE             0x00000075
+#define SL_LIDAR_CONF_LIDAR_MAC_ADDR                 0x00000079
+#define SL_LIDAR_CONF_SCAN_MODE_TYPICAL              0x0000007C
+#define SL_LIDAR_CONF_SCAN_MODE_NAME                 0x0000007F
+
+
+#define SL_LIDAR_CONF_MODEL_REVISION_ID              0x00000080
+#define SL_LIDAR_CONF_MODEL_NAME_ALIAS               0x00000081
+
+#define SL_LIDAR_CONF_DETECTED_SERIAL_BPS            0x000000A1
+
+#define SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR           0x0001CCC0
+#define SL_LIDAR_EXPRESS_SCAN_STABILITY_BITMAP                 4
+#define SL_LIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP               5
+
+typedef struct _sl_lidar_response_get_lidar_conf
+{
+    sl_u32 type;
+    sl_u8  payload[0];
+}__attribute__((packed)) sl_lidar_response_get_lidar_conf_t;
+
+typedef struct _sl_lidar_response_set_lidar_conf
+{
+    sl_u32 type;
+    sl_u32 result;
+}__attribute__((packed)) sl_lidar_response_set_lidar_conf_t;
+
+
+typedef struct _sl_lidar_response_device_info_t
+{
+    sl_u8   model;
+    sl_u16  firmware_version;
+    sl_u8   hardware_version;
+    sl_u8   serialnum[16];
+} __attribute__((packed)) sl_lidar_response_device_info_t;
+
+typedef struct _sl_lidar_response_device_health_t
+{
+    sl_u8   status;
+    sl_u16  error_code;
+} __attribute__((packed)) sl_lidar_response_device_health_t;
+
+typedef struct _sl_lidar_ip_conf_t {
+    sl_u8 ip_addr[4];
+    sl_u8 net_mask[4];
+    sl_u8 gw[4];
+}__attribute__((packed)) sl_lidar_ip_conf_t;
+
+typedef struct _sl_lidar_response_device_macaddr_info_t {
+    sl_u8   macaddr[6];
+} __attribute__((packed)) sl_lidar_response_device_macaddr_info_t;
+
+typedef struct  _sl_lidar_response_desired_rot_speed_t{
+    sl_u16 rpm;
+    sl_u16 pwm_ref;
+}__attribute__((packed)) sl_lidar_response_desired_rot_speed_t;
+
+// Definition of the variable bit scale encoding mechanism
+#define SL_LIDAR_VARBITSCALE_X2_SRC_BIT  9
+#define SL_LIDAR_VARBITSCALE_X4_SRC_BIT  11
+#define SL_LIDAR_VARBITSCALE_X8_SRC_BIT  12
+#define SL_LIDAR_VARBITSCALE_X16_SRC_BIT 14
+
+#define SL_LIDAR_VARBITSCALE_X2_DEST_VAL 512
+#define SL_LIDAR_VARBITSCALE_X4_DEST_VAL 1280
+#define SL_LIDAR_VARBITSCALE_X8_DEST_VAL 1792
+#define SL_LIDAR_VARBITSCALE_X16_DEST_VAL 3328
+
+#define SL_LIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \
+    (  (((0x1<<(_BITS_)) - SL_LIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \
+       ((SL_LIDAR_VARBITSCALE_X16_DEST_VAL - SL_LIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \
+       ((SL_LIDAR_VARBITSCALE_X8_DEST_VAL - SL_LIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \
+       ((SL_LIDAR_VARBITSCALE_X4_DEST_VAL - SL_LIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \
+       SL_LIDAR_VARBITSCALE_X2_DEST_VAL - 1)
+
+
+#if defined(_WIN32)
+#pragma pack()
+#endif
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif

third_party/rplidar_sdk/include/sl_lidar_driver.h

@@ -0,0 +1,569 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#pragma once
+
+#ifndef __cplusplus
+#error "The Slamtec LIDAR SDK requires a C++ compiler to be built"
+#endif
+
+#include <vector>
+#include <map>
+#include <string>
+
+#ifndef DEPRECATED
+    #ifdef __GNUC__
+        #define DEPRECATED(func) func __attribute__ ((deprecated))
+    #elif defined(_MSC_VER)
+        #define DEPRECATED(func) __declspec(deprecated) func
+    #else
+        #pragma message("WARNING: You need to implement DEPRECATED for this compiler")
+        #define DEPRECATED(func) func
+    #endif
+#endif
+
+
+#include "sl_lidar_cmd.h"
+
+#include <string>
+
+namespace sl {
+
+#ifdef DEPRECATED
+#define DEPRECATED_WARN(fn, replacement) do { \
+        static bool __shown__ = false; \
+        if (!__shown__) { \
+            printDeprecationWarn(fn, replacement); \
+            __shown__ = true; \
+        } \
+    } while (0)
+#endif
+
+    /**
+    * Lidar scan mode
+    */
+    struct LidarScanMode
+    {
+        // Mode id
+        sl_u16  id;
+
+        // Time cost for one measurement (in microseconds)
+        float   us_per_sample;
+
+        // Max distance in this scan mode (in meters)
+        float   max_distance;
+
+        // The answer command code for this scan mode
+        sl_u8   ans_type;
+
+        // The name of scan mode (padding with 0 if less than 64 characters)
+        char    scan_mode[64];
+    };
+
+    template <typename T>
+    struct Result
+    {
+        sl_result err;
+        T value;
+        Result(const T& value)
+            : err(SL_RESULT_OK)
+            , value(value)
+        {
+        }
+
+        Result(sl_result err)
+            : err(err)
+            , value()
+        {
+        }
+
+        operator sl_result() const
+        {
+            return err;
+        }
+
+        operator bool() const
+        {
+            return SL_IS_OK(err);
+        }
+
+        T& operator* ()
+        {
+            return value;
+        }
+
+        T* operator-> ()
+        {
+            return &value;
+        }
+    };
+
+    enum LIDARTechnologyType {
+        LIDAR_TECHNOLOGY_UNKNOWN = 0,
+        LIDAR_TECHNOLOGY_TRIANGULATION = 1,
+        LIDAR_TECHNOLOGY_DTOF = 2,
+        LIDAR_TECHNOLOGY_ETOF = 3,
+        LIDAR_TECHNOLOGY_FMCW = 4,
+    };
+
+    enum LIDARMajorType {
+        LIDAR_MAJOR_TYPE_UNKNOWN = 0,
+        LIDAR_MAJOR_TYPE_A_SERIES = 1,
+        LIDAR_MAJOR_TYPE_S_SERIES = 2,
+        LIDAR_MAJOR_TYPE_T_SERIES = 3,
+        LIDAR_MAJOR_TYPE_M_SERIES = 4,
+        LIDAR_MAJOR_TYPE_C_SERIES = 6,
+    };
+
+    enum LIDARInterfaceType {
+        LIDAR_INTERFACE_UART = 0,
+        LIDAR_INTERFACE_ETHERNET = 1,
+        LIDAR_INTERFACE_USB = 2,
+        LIDAR_INTERFACE_CANBUS = 5,
+
+
+        LIDAR_INTERFACE_UNKNOWN = 0xFFFF,
+    };
+
+    struct SlamtecLidarTimingDesc {
+
+        sl_u32  sample_duration_uS;
+        sl_u32  native_baudrate;
+        
+        sl_u32  linkage_delay_uS;
+
+        LIDARInterfaceType native_interface_type;
+
+        bool    native_timestamp_support;
+    };
+
+    /**
+    * Abstract interface of communication channel
+    */
+    class IChannel
+    {
+    public:
+        virtual ~IChannel() {}
+
+    public:
+        /**
+        * Open communication channel (return true if succeed)
+        */
+        virtual bool open() = 0;
+
+        /**
+        * Close communication channel
+        */
+        virtual void close() = 0;
+
+        /**
+        * Flush all written data to remote endpoint
+        */
+        virtual void flush() = 0;
+
+        /**
+        * Wait for some data
+        * \param size Bytes to wait
+        * \param timeoutInMs Wait timeout (in microseconds, -1 for forever)
+        * \param actualReady [out] actual ready bytes
+        * \return true for data ready
+        */
+        virtual bool waitForData(size_t size, sl_u32 timeoutInMs = -1, size_t* actualReady = nullptr) = 0;
+
+
+        /**
+        * Wait for some data
+        * \param size_hint Byte count may available to retrieve without beening blocked 
+        * \param timeoutInMs Wait timeout (in microseconds, -1 for forever)
+        * \return RESULT_OK if there is data available for receiving
+        *         RESULT_OPERATION_TIMEOUT if the given timeout duration is exceed
+        *         RESULT_OPERATION_FAIL if there is something wrong with the channel
+        */
+        virtual sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs = 1000) = 0;
+
+
+        /**
+        * Send data to remote endpoint
+        * \param data The data buffer
+        * \param size The size of data buffer (in bytes)
+        * \return Bytes written (negative for write failure)
+        */
+        virtual int write(const void* data, size_t size) = 0;
+
+        /**
+        * Read data from the chanel
+        * \param buffer The buffer to receive data
+        * \param size The size of the read buffer
+        * \return Bytes read (negative for read failure)
+        */
+        virtual int read(void* buffer, size_t size) = 0;
+
+        /**
+        * Clear read cache
+        */
+        virtual void clearReadCache() = 0;
+
+        virtual int getChannelType() = 0;
+
+    private:
+
+    };
+
+    /**
+    * Abstract interface of serial port channel
+    */
+    class ISerialPortChannel : public IChannel
+    {
+    public:
+        virtual ~ISerialPortChannel() {}
+
+    public:
+        virtual void setDTR(bool dtr) = 0;
+    };
+
+    /**
+    * Create a serial channel
+    * \param device Serial port device
+    *                   e.g. on Windows, it may be com3 or \\.\com10
+    *                   on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc
+    * \param baudrate Baudrate
+    *                   Please refer to the datasheet for the baudrate (maybe 115200 or 256000)
+    */
+    Result<IChannel*> createSerialPortChannel(const std::string& device, int baudrate);
+
+    /**
+    * Create a TCP channel
+    * \param ip IP address of the device
+    * \param port TCP port
+    */
+    Result<IChannel*> createTcpChannel(const std::string& ip, int port);
+
+    /**
+    * Create a UDP channel
+    * \param ip IP address of the device
+    * \param port UDP port
+    */
+    Result<IChannel*> createUdpChannel(const std::string& ip, int port);
+
+    enum MotorCtrlSupport
+    {
+        MotorCtrlSupportNone = 0,
+        MotorCtrlSupportPwm = 1,
+        MotorCtrlSupportRpm = 2,
+    };
+
+    enum ChannelType{
+        CHANNEL_TYPE_SERIALPORT = 0x0,
+        CHANNEL_TYPE_TCP = 0x1,
+        CHANNEL_TYPE_UDP = 0x2,
+    };
+
+        /**
+    * Lidar motor info
+    */
+    struct LidarMotorInfo
+    {
+        MotorCtrlSupport motorCtrlSupport;
+
+        // Desire speed
+        sl_u16 desired_speed;
+
+        // Max speed 
+        sl_u16 max_speed;
+
+        // Min speed
+        sl_u16 min_speed;
+    };
+
+    class ILidarDriver
+    {
+    public:
+        virtual ~ILidarDriver() {}
+
+    public:
+        /**
+        * Connect to LIDAR via channel
+        * \param channel The communication channel
+        *                    Note: you should manage the lifecycle of the channel object, make sure it is alive during lidar driver's lifecycle
+        */
+        virtual sl_result connect(IChannel* channel) = 0;
+
+        /**
+        * Disconnect from the LIDAR
+        */
+        virtual void disconnect() = 0;
+        
+        /**
+        * Check if the connection is established
+        */
+        virtual bool isConnected() = 0;
+
+    public:
+        enum
+        {
+            DEFAULT_TIMEOUT = 2000
+        };
+
+    public:
+        /// Ask the LIDAR core system to reset it self
+        /// The host system can use the Reset operation to help LIDAR escape the self-protection mode.
+        ///
+        ///  \param timeout       The operation timeout value (in millisecond)
+        virtual sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+        /// Get all scan modes that supported by lidar
+        virtual sl_result getAllSupportedScanModes(std::vector<LidarScanMode>& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+        /// Get typical scan mode of lidar
+        virtual sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+        /// Start scan
+        ///
+        /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+        /// \param useTypicalScan   Use lidar's typical scan mode or use the compatibility mode (2k sps)
+        /// \param options          Scan options (please use 0)
+        /// \param outUsedScanMode  The scan mode selected by lidar
+        virtual sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr) = 0;
+
+        /// Start scan in specific mode
+        ///
+        /// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
+        /// \param scanMode         The scan mode id (use getAllSupportedScanModes to get supported modes)
+        /// \param options          Scan options (please use 0)
+        /// \param outUsedScanMode  The scan mode selected by lidar
+        virtual sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+        /// Retrieve the health status of the RPLIDAR
+        /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode.
+        ///
+        /// \param health        The health status info returned from the RPLIDAR
+        ///
+        /// \param timeout       The operation timeout value (in millisecond) for the serial port communication     
+        virtual sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+        /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.
+        /// 
+        /// \param info          The device information returned from the RPLIDAR
+        /// \param timeout       The operation timeout value (in millisecond) for the serial port communication  
+        virtual sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+        /// Check whether the device support motor control
+        /// Note: this API will disable grab.
+        /// 
+        /// \param motorCtrlSupport Return the result.
+        /// \param timeout          The operation timeout value (in millisecond) for the serial port communication. 
+        virtual sl_result checkMotorCtrlSupport(MotorCtrlSupport& motorCtrlSupport, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+        /// Calculate LIDAR's current scanning frequency from the given scan data
+        /// Please refer to the application note doc for details
+        /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data
+        ///
+        /// \param scanMode      Lidar's current scan mode
+        /// \param nodes         Current scan's measurements
+        /// \param count         The number of sample nodes inside the given buffer
+        virtual sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency) = 0;
+
+		///Set LPX and S2E series lidar's static IP address
+		///
+		/// \param conf             Network parameter that LPX series lidar owned
+		/// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
+		virtual sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+       
+        ///Get LPX and S2E series lidar's static IP address
+        ///
+        /// \param conf             Network parameter that LPX series lidar owned
+        /// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
+        virtual sl_result getLidarIpConf( sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+  // 
+		/////Get LPX series lidar's MAC address
+		///
+		/// \param macAddrArray         The device MAC information returned from the LPX series lidar
+        ///                             Notice: the macAddrArray must point to a valid buffer with at least 6 bytes length
+        ///                                     Otherwise, buffer overwrite will occur
+		virtual sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+        /// Ask the LIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated
+        ///
+        /// \param timeout       The operation timeout value (in millisecond) for the serial port communication 
+        virtual sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+        /// Wait and grab a complete 0-360 degree scan data previously received. 
+        /// The grabbed scan data returned by this interface always has the following charactistics:
+        ///
+        /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1
+        /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan
+        /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
+        ///
+        /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+        ///
+        /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+        ///                       Once the interface returns, this parameter will store the actual received data count.
+        ///
+        /// \param timeout        Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely.
+        ///
+        /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 
+        ///
+        /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
+        virtual sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+
+
+        /// Wait and grab a complete 0-360 degree scan data previously received with timestamp support.
+        /// 
+        /// The returned timestamp belongs to the first data point of the scan data (begining of the scan).
+        /// Its value is represented based on the current machine's time domain with the unit of microseconds (uS).
+        /// 
+        /// If the currently connected LIDAR supports hardware timestamp mechanism, this timestamp will use 
+        /// the actual data emitted by the LIDAR device and remap it to the current machine's time domain. 
+        /// 
+        /// For other models that do not support hardware timestamps, this data will be deducted through estimation, 
+        /// and there may be a slight deviation from the actual situation. 
+        ///
+        /// The grabbed scan data returned by this interface always has the following charactistics:
+        ///
+        /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1
+        /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan
+        /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer.
+        ///
+        /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+        ///
+        /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+        ///                       Once the interface returns, this parameter will store the actual received data count.
+        ///
+        /// \param timestamp_uS   The reference used to store the timestamp value.
+        /// \param timeout        Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely.
+        ///
+        /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. 
+        ///
+        /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation.
+        virtual sl_result grabScanDataHqWithTimeStamp(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u64 & timestamp_uS, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+
+        /// Ascending the scan data according to the angle value in the scan.
+        ///
+        /// \param nodebuffer     Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData
+        ///
+        /// \param count          The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t).
+        ///                       Once the interface returns, this parameter will store the actual received data count.
+        /// The interface will return SL_RESULT_OPERATION_FAIL when all the scan data is invalid. 
+        virtual sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t count) = 0;
+
+        /// Return received scan points even if it's not complete scan
+        ///
+        /// \param nodebuffer     Buffer provided by the caller application to store the scan data
+        ///
+        /// \param count          Once the interface returns, this parameter will store the actual received data count.
+        ///
+        /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 
+        virtual sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count) = 0;
+        /// Set lidar motor speed
+        /// The host system can use this operation to set lidar motor speed.
+        ///
+        /// \param speed        The speed value set to lidar
+        ///
+        ///Note: The function will stop scan if speed is DEFAULT_MOTOR_SPEED.
+        virtual sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_SPEED) = 0;
+        
+        /// Get the motor information of the RPLIDAR include the max speed, min speed, desired speed.
+        /// 
+        /// \param motorInfo          The motor information returned from the RPLIDAR
+        virtual sl_result getMotorInfo(LidarMotorInfo &motorInfo, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+    
+
+        /// Ask the LIDAR to use a new baudrate for serial communication
+        /// The target LIDAR system must support such feature to work.
+        /// This function does NOT check whether the target LIDAR works with the requiredBaudRate or not.
+        /// In order to verifiy the result, use getDeviceInfo or other getXXXX functions instead.
+        /// 
+        /// \param requiredBaudRate   The new baudrate required to be used. It MUST matches with the baudrate of the binded channel.
+        /// \param baudRateDetected   The actual baudrate detected by the LIDAR system
+        virtual sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected = NULL) = 0;
+
+
+
+        /// Get the technology of the LIDAR's measurement system
+        /// 
+        /// 
+        /// \param devInfo   The device info used to deduct the result
+        ///                  If NULL is specified, a driver cached version of the connected LIDAR will be used
+        virtual LIDARTechnologyType getLIDARTechnologyType(const sl_lidar_response_device_info_t* devInfo = nullptr) = 0;
+        
+        
+        /// Get the Major Type (Series Info) of the LIDAR
+        /// 
+        /// 
+        /// \param devInfo   The device info used to deduct the result
+        ///                  If NULL is specified, a driver cached version of the connected LIDAR will be used
+        virtual LIDARMajorType getLIDARMajorType(const sl_lidar_response_device_info_t* devInfo = nullptr) = 0;
+
+
+        /// Get the Model Name of the LIDAR
+        /// The result will be somthing like: "A1M8" or "S1M1" or "A3M1-R1"
+        /// 
+        /// \param out_description   The output string that contains the generated model name
+        ///                          
+        /// \param fetchAliasName    If set to true, a communication will be taken to ask if there is any Alias name availabe
+        /// \param devInfo           The device info used to deduct the result
+        ///                          If NULL is specified, a driver cached version of the connected LIDAR will be used
+        /// \param timeout           The timeout value used by potential data communication
+        virtual sl_result getModelNameDescriptionString(std::string& out_description, bool fetchAliasName = true, const sl_lidar_response_device_info_t* devInfo = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+};
+
+    /**
+    * Create a LIDAR driver instance
+    *
+    * Example
+    * Result<ISerialChannel*> channel = createSerialPortChannel("/dev/ttyUSB0", 115200);
+    * assert((bool)channel);
+    * assert(*channel);
+    *
+    * auto lidar = createLidarDriver();
+    * assert((bool)lidar);
+    * assert(*lidar);
+    *
+    * auto res = (*lidar)->connect(*channel);
+    * assert(SL_IS_OK(res));
+    *
+    * sl_lidar_response_device_info_t deviceInfo;
+    * res = (*lidar)->getDeviceInfo(deviceInfo);
+    * assert(SL_IS_OK(res));
+    *
+    * printf("Model: %d, Firmware Version: %d.%d, Hardware Version: %d\n",
+    *        deviceInfo.model,
+    *        deviceInfo.firmware_version >> 8, deviceInfo.firmware_version & 0xffu,
+    *        deviceInfo.hardware_version);
+    *
+    * delete *lidar;
+    * delete *channel;
+    */
+    Result<ILidarDriver*> createLidarDriver();
+}

third_party/rplidar_sdk/include/sl_lidar_driver_impl.h

@@ -0,0 +1,157 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#pragma once
+#include "sl_lidar_driver.h"
+
+namespace sl {
+	class SL_LidarDriver :public ILidarDriver
+	{
+		public:
+			enum {
+				LEGACY_SAMPLE_DURATION = 476,
+			};
+
+			enum
+			{
+					NORMAL_CAPSULE = 0,
+					DENSE_CAPSULE = 1,
+			};
+
+			enum {
+				A2A3_LIDAR_MINUM_MAJOR_ID  = 2,
+				TOF_LIDAR_MINUM_MAJOR_ID = 6,
+			};
+		public:
+			SL_LidarDriver()
+				:_channel(NULL)
+				, _isConnected(false)
+				, _isScanning(false)
+				, _isSupportingMotorCtrl(MotorCtrlSupportNone)
+				, _cached_sampleduration_std(LEGACY_SAMPLE_DURATION)
+				,_cached_sampleduration_express(LEGACY_SAMPLE_DURATION)
+				, _cached_scan_node_hq_count(0)
+				, _cached_scan_node_hq_count_for_interval_retrieve(0)
+			{}
+
+			sl_result connect(IChannel* channel);
+			void disconnect();
+			bool isConnected();
+			sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getAllSupportedScanModes(std::vector<LidarScanMode>& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr);
+			sl_result startScanNormal(bool force, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT);
+			DEPRECATED(sl_result grabScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT));
+			sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result checkMotorCtrlSupport(MotorCtrlSupport & support, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency);
+			sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout);
+			sl_result getLidarIpConf(sl_lidar_ip_conf_t& conf, sl_u32 timeout);
+			sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs);
+			sl_result ascendScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t count);
+			sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t count);
+			sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t & count);
+			sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_PWM);//
+			sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected = NULL);
+	
+	protected:
+			sl_result startMotor();
+			sl_result checkSupportConfigCommands(bool& outSupport, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getScanModeCount(sl_u16& modeCount, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result setLidarConf(sl_u32 type, const void* payload, size_t payloadSize, sl_u32 timeout);
+			sl_result getLidarConf(sl_u32 type, std::vector<sl_u8> &outputBuf, const std::vector<sl_u8> &reserve = std::vector<sl_u8>(), sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result getLidarSampleDuration(float& sampleDurationRes, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getMaxDistance(float &maxDistance, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getScanModeAnsType(sl_u8 &ansType, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			sl_result getScanModeName(char* modeName, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT);
+			//DEPRECATED(sl_result getSampleDuration_uS(sl_lidar_response_sample_rate_t & rateInfo, sl_u32 timeout = DEFAULT_TIMEOUT));
+			//DEPRECATED (sl_result checkExpressScanSupported(bool & support, sl_u32 timeout = DEFAULT_TIMEOUT));
+			//DEPRECATED(sl_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode));
+		private:
+			sl_result  _sendCommand(sl_u16 cmd, const void * payload = NULL, size_t payloadsize = 0 );
+			sl_result _waitResponseHeader(sl_lidar_ans_header_t * header, sl_u32 timeout = DEFAULT_TIMEOUT);
+			template <typename T>
+			sl_result _waitResponse(T &payload ,sl_u8 ansType, sl_u32 timeout = DEFAULT_TIMEOUT);
+			void _disableDataGrabbing();
+			sl_result _waitNode(sl_lidar_response_measurement_node_t * node, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result _waitScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t & count, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result _cacheScanData();
+			void _ultraCapsuleToNormal(const sl_lidar_response_ultra_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
+			sl_result _waitCapsuledNode(sl_lidar_response_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT);
+			void _capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
+			void _dense_capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
+			sl_result _cacheCapsuledScanData();
+
+			void _ultra_dense_capsuleToNormal(const sl_lidar_response_ultra_dense_capsule_measurement_nodes_t& capslue, sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& nodeCount);
+			sl_result _waitUltraDenseCapsuledNode(sl_lidar_response_ultra_dense_capsule_measurement_nodes_t& node, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result _cacheUltraDenseCapsuledScanData();
+			
+
+			sl_result _waitHqNode(sl_lidar_response_hq_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT);
+			void _HqToNormal(const sl_lidar_response_hq_capsule_measurement_nodes_t & node_hq, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
+			sl_result _cacheHqScanData();
+			sl_result _waitUltraCapsuledNode(sl_lidar_response_ultra_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT);
+			sl_result _cacheUltraCapsuledScanData();
+			sl_result _clearRxDataCache();
+
+		private:
+			IChannel *_channel;
+			bool _isConnected;
+			bool _isScanning;
+			MotorCtrlSupport _isSupportingMotorCtrl;
+
+			rp::hal::Locker         _lock;
+			rp::hal::Event          _dataEvt;
+			rp::hal::Thread         _cachethread;
+			sl_u16                    _cached_sampleduration_std;
+			sl_u16                    _cached_sampleduration_express;
+
+			sl_lidar_response_measurement_node_hq_t   _cached_scan_node_hq_buf[8192];
+			size_t                                   _cached_scan_node_hq_count;
+			sl_u8                                    _cached_capsule_flag;
+
+			sl_lidar_response_measurement_node_hq_t   _cached_scan_node_hq_buf_for_interval_retrieve[8192];
+			size_t                                   _cached_scan_node_hq_count_for_interval_retrieve;
+
+			sl_lidar_response_capsule_measurement_nodes_t       _cached_previous_capsuledata;
+			sl_lidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata;
+			sl_lidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata;
+			sl_lidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata;
+			bool                                         _is_previous_capsuledataRdy;
+			bool                                         _is_previous_HqdataRdy;
+	};
+
+}

third_party/rplidar_sdk/include/sl_lidar_protocol.h

@@ -0,0 +1,85 @@
+/*
+* Slamtec LIDAR SDK
+*
+* sl_lidar_protocol.h
+*
+* Copyright (c) 2020 Shanghai Slamtec Co., Ltd.
+*/
+
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4200)
+#endif
+
+#include "sl_types.h"
+
+#define SL_LIDAR_CMD_SYNC_BYTE              0xA5
+#define SL_LIDAR_CMDFLAG_HAS_PAYLOAD        0x80
+
+#define SL_LIDAR_ANS_SYNC_BYTE1             0xA5
+#define SL_LIDAR_ANS_SYNC_BYTE2             0x5A
+
+#define SL_LIDAR_ANS_PKTFLAG_LOOP           0x1
+
+#define SL_LIDAR_ANS_HEADER_SIZE_MASK       0x3FFFFFFF
+#define SL_LIDAR_ANS_HEADER_SUBTYPE_SHIFT   (30)
+
+#if defined(_WIN32)
+#pragma pack(1)
+#endif
+
+
+
+typedef struct sl_lidar_cmd_packet_t
+{
+    sl_u8 syncByte; //must be SL_LIDAR_CMD_SYNC_BYTE
+    sl_u8 cmd_flag;
+    sl_u8 size;
+    sl_u8 data[0];
+} __attribute__((packed)) sl_lidar_cmd_packet_t;
+
+
+typedef struct sl_lidar_ans_header_t
+{
+    sl_u8  syncByte1; // must be SL_LIDAR_ANS_SYNC_BYTE1
+    sl_u8  syncByte2; // must be SL_LIDAR_ANS_SYNC_BYTE2
+    sl_u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2;
+    sl_u8  type;
+} __attribute__((packed)) sl_lidar_ans_header_t;
+
+#if defined(_WIN32)
+#pragma pack()
+#endif
+
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif

third_party/rplidar_sdk/include/sl_types.h

@@ -0,0 +1,83 @@
+/*
+* Slamtec LIDAR SDK
+*
+* sl_types.h
+*
+* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. 
+*/
+
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+#include <cstdint>
+
+#define SL_DEFINE_TYPE(IntType, NewType)    typedef std::IntType NewType
+#else
+#include <stdint.h>
+
+#define SL_DEFINE_TYPE(IntType, NewType)    typedef IntType NewType
+#endif
+
+#define SL_DEFINE_INT_TYPE(Bits) \
+    SL_DEFINE_TYPE(int ## Bits ## _t, sl_s ## Bits); \
+    SL_DEFINE_TYPE(uint ## Bits ## _t, sl_u ## Bits); \
+
+SL_DEFINE_INT_TYPE(8)
+SL_DEFINE_INT_TYPE(16)
+SL_DEFINE_INT_TYPE(32)
+SL_DEFINE_INT_TYPE(64)
+
+#if !defined(__GNUC__) && !defined(__attribute__)
+#   define __attribute__(x)
+#endif
+
+#ifdef WIN64
+typedef sl_u64          sl_word_size_t;
+#elif defined(WIN32)
+typedef sl_u32          sl_word_size_t;
+#elif defined(__GNUC__)
+typedef unsigned long   sl_word_size_t;
+#elif defined(__ICCARM__)
+typedef sl_u32          sl_word_size_t;
+#endif
+
+typedef uint32_t sl_result;
+
+#define SL_RESULT_OK                     (sl_result)0
+#define SL_RESULT_FAIL_BIT               (sl_result)0x80000000
+#define SL_RESULT_ALREADY_DONE           (sl_result)0x20
+#define SL_RESULT_INVALID_DATA           (sl_result)(0x8000 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_OPERATION_FAIL         (sl_result)(0x8001 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_OPERATION_TIMEOUT      (sl_result)(0x8002 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_OPERATION_STOP         (sl_result)(0x8003 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_OPERATION_NOT_SUPPORT  (sl_result)(0x8004 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_FORMAT_NOT_SUPPORT     (sl_result)(0x8005 | SL_RESULT_FAIL_BIT)
+#define SL_RESULT_INSUFFICIENT_MEMORY    (sl_result)(0x8006 | SL_RESULT_FAIL_BIT)
+
+#define SL_IS_OK(x)    ( ((x) & SL_RESULT_FAIL_BIT) == 0 )
+#define SL_IS_FAIL(x)  ( ((x) & SL_RESULT_FAIL_BIT) )

third_party/rplidar_sdk/src/arch/linux/arch_linux.h

@@ -0,0 +1,64 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+// libc dep
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <math.h>
+#include <time.h>
+#include <stdarg.h>
+
+// libc++ dep
+#include <iostream>
+#include <string>
+
+// linux specific
+#include <unistd.h>
+#include <errno.h>
+#include <pthread.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <sys/select.h>
+#include <time.h>
+
+#include "timer.h"
+

third_party/rplidar_sdk/src/arch/linux/net_serial.cpp

@@ -0,0 +1,475 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <assert.h>
+// linux specific
+
+#include <errno.h>
+#include <fcntl.h>
+
+#include <time.h>
+#include "hal/types.h"
+#include "arch/linux/net_serial.h"
+#include <sys/select.h>
+
+#include <algorithm>
+//__GNUC__
+#if defined(__GNUC__)
+// for Linux extension
+#include <asm/ioctls.h>
+#include <asm/termbits.h>
+#include <sys/ioctl.h>
+extern "C" int tcflush(int fildes, int queue_selector);
+#else
+// for other standard UNIX
+#include <termios.h>
+#include <sys/ioctl.h>
+
+#endif
+
+
+namespace rp{ namespace arch{ namespace net{
+
+raw_serial::raw_serial()
+    : rp::hal::serial_rxtx()
+    , _baudrate(0)
+    , _flags(0)
+    , serial_fd(-1)
+{
+    _init();
+}
+
+raw_serial::~raw_serial()
+{
+    close();
+
+}
+
+bool raw_serial::open()
+{
+    return open(_portName, _baudrate, _flags);
+}
+
+bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags)
+{   
+    strncpy(_portName, portname, sizeof(_portName));
+    _baudrate = baudrate;
+    _flags    = flags;
+    return true;
+}
+
+bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags)
+{
+    if (isOpened()) close();
+    
+    serial_fd = ::open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
+
+    if (serial_fd == -1) return false;
+
+    
+
+#if !defined(__GNUC__)
+    // for standard UNIX
+    struct termios options, oldopt;
+    tcgetattr(serial_fd, &oldopt);
+    bzero(&options,sizeof(struct termios));
+
+    // enable rx and tx
+    options.c_cflag |= (CLOCAL | CREAD);
+
+    _u32 termbaud = getTermBaudBitmap(baudrate);
+
+    if (termbaud == (_u32)-1) {
+        close();
+        return false;
+    }
+    cfsetispeed(&options, termbaud);
+    cfsetospeed(&options, termbaud);
+
+    options.c_cflag &= ~PARENB; //no checkbit
+    options.c_cflag &= ~CSTOPB; //1bit stop bit
+    options.c_cflag &= ~CRTSCTS; //no flow control
+
+    options.c_cflag &= ~CSIZE;
+    options.c_cflag |= CS8; /* Select 8 data bits */
+
+#ifdef CNEW_RTSCTS
+    options.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
+#endif
+
+    options.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
+
+    // raw input mode   
+    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    // raw output mode   
+    options.c_oflag &= ~OPOST;
+
+
+
+    if (tcsetattr(serial_fd, TCSANOW, &options))
+    {
+        close();
+        return false;
+    }
+
+#else
+
+    // using Linux extension ...
+    struct termios2 tio;
+
+    ioctl(serial_fd, TCGETS2, &tio);
+    bzero(&tio, sizeof(struct termios2));
+
+    tio.c_cflag = BOTHER;
+    tio.c_cflag |= (CLOCAL | CREAD | CS8); //8 bit no hardware handshake
+
+    tio.c_cflag &= ~CSTOPB;   //1 stop bit
+    tio.c_cflag &= ~CRTSCTS;  //No CTS
+    tio.c_cflag &= ~PARENB;   //No Parity
+
+#ifdef CNEW_RTSCTS
+    tio.c_cflag &= ~CNEW_RTSCTS; // no hw flow control
+#endif
+
+    tio.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control
+
+
+    tio.c_cc[VMIN] = 0;         //min chars to read
+    tio.c_cc[VTIME] = 0;        //time in 1/10th sec wait
+
+    tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+    // raw output mode   
+    tio.c_oflag &= ~OPOST;
+
+    tio.c_ispeed = baudrate;
+    tio.c_ospeed = baudrate;
+
+
+    ioctl(serial_fd, TCSETS2, &tio);
+
+#endif
+
+
+    tcflush(serial_fd, TCIFLUSH);
+
+    if (fcntl(serial_fd, F_SETFL, FNDELAY))
+    {
+        close();
+        return false;
+    }
+
+
+    _is_serial_opened = true;
+    _operation_aborted = false;
+
+    //Clear the DTR bit to let the motor spin
+    clearDTR();
+    do {
+        // create self pipeline for wait cancellation
+        if (pipe(_selfpipe) == -1) break;
+
+        int flags = fcntl(_selfpipe[0], F_GETFL);
+        if (flags == -1)
+            break;
+
+        flags |= O_NONBLOCK;                /* Make read end nonblocking */
+        if (fcntl(_selfpipe[0], F_SETFL, flags) == -1)
+            break;
+
+        flags = fcntl(_selfpipe[1], F_GETFL);
+        if (flags == -1)
+            break;
+
+        flags |= O_NONBLOCK;                /* Make write end nonblocking */
+        if (fcntl(_selfpipe[1], F_SETFL, flags) == -1)
+            break;
+
+    } while (0);
+    
+    return true;
+}
+
+void raw_serial::close()
+{
+    if (serial_fd != -1)
+        ::close(serial_fd);
+    serial_fd = -1;
+    
+    if (_selfpipe[0] != -1)
+        ::close(_selfpipe[0]);
+
+    if (_selfpipe[1] != -1)
+        ::close(_selfpipe[1]);
+
+    _selfpipe[0] = _selfpipe[1] = -1;
+
+    _operation_aborted = false;
+    _is_serial_opened = false;
+}
+
+int raw_serial::senddata(const unsigned char * data, size_t size)
+{
+// FIXME: non-block io should be used
+    if (!isOpened()) return 0;
+
+    if (data == NULL || size ==0) return 0;
+    
+    size_t tx_len = 0;
+    required_tx_cnt = 0;
+    do {
+        int ans = ::write(serial_fd, data + tx_len, size-tx_len);
+        
+        if (ans == -1) return tx_len;
+        
+        tx_len += ans;
+        required_tx_cnt = tx_len;
+    }while (tx_len<size);
+    
+    
+    return tx_len;
+}
+
+
+int raw_serial::recvdata(unsigned char * data, size_t size)
+{
+    if (!isOpened()) return 0;
+    
+    int ans = ::read(serial_fd, data, size);
+    
+    if (ans == -1) ans=0;
+    required_rx_cnt = ans;
+    return ans;
+}
+
+
+void raw_serial::flush( _u32 flags)
+{
+    tcflush(serial_fd,TCIFLUSH); 
+}
+
+int raw_serial::waitforsent(_u32 timeout, size_t * returned_size)
+{
+    if (returned_size) *returned_size = required_tx_cnt;
+    return 0;
+}
+
+int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size)
+{
+    if (!isOpened() ) return -1;
+   
+    if (returned_size) *returned_size = required_rx_cnt;
+    return 0;
+}
+
+int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size)
+{
+    size_t length = 0;
+    if (returned_size==NULL) returned_size=(size_t *)&length;
+    *returned_size = 0;
+
+    int max_fd;
+    fd_set input_set;
+    struct timeval timeout_val;
+
+    /* Initialize the input set */
+    FD_ZERO(&input_set);
+    FD_SET(serial_fd, &input_set);
+
+    if (_selfpipe[0] != -1)
+        FD_SET(_selfpipe[0], &input_set);
+
+    max_fd =  std::max<int>(serial_fd, _selfpipe[0]) + 1;
+
+    /* Initialize the timeout structure */
+    timeout_val.tv_sec = timeout / 1000;
+    timeout_val.tv_usec = (timeout % 1000) * 1000;
+
+    if ( isOpened() )
+    {
+        int nread;
+
+        if ( ioctl(serial_fd, FIONREAD, &nread) == -1) return ANS_DEV_ERR;
+
+        *returned_size = nread;
+
+        if (*returned_size >= data_count)
+        {
+            return 0;
+        }
+    }
+
+    while ( isOpened() )
+    {
+        /* Do the select */
+        int n = ::select(max_fd, &input_set, NULL, NULL, &timeout_val);
+
+        if (n < 0)
+        {
+            // select error
+            *returned_size =  0;
+            return ANS_DEV_ERR;
+        }
+        else if (n == 0)
+        {
+            // time out
+            *returned_size =0;
+            return ANS_TIMEOUT;
+        }
+        else
+        {
+            if (FD_ISSET(_selfpipe[0], &input_set)) {   
+                // require aborting the current operation
+                int ch;
+                for (;;) {                    
+                    if (::read(_selfpipe[0], &ch, 1) == -1) {
+                        break;
+                    }
+                    
+                }
+
+                // treat as  timeout
+                *returned_size = 0;
+                return ANS_TIMEOUT;
+            }
+
+            // data avaliable
+            assert (FD_ISSET(serial_fd, &input_set));
+
+
+            if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR;
+            if (*returned_size >= data_count)
+            {
+                return 0;
+            }
+        }
+        
+    }
+
+    *returned_size=0;
+    return ANS_DEV_ERR;
+}
+
+size_t raw_serial::rxqueue_count()
+{
+    if  ( !isOpened() ) return 0;
+    size_t remaining;
+    
+    if (::ioctl(serial_fd, FIONREAD, &remaining) == -1) return 0;
+    return remaining;
+}
+
+void raw_serial::setDTR()
+{
+    if ( !isOpened() ) return;
+
+    uint32_t dtr_bit = TIOCM_DTR;
+    ioctl(serial_fd, TIOCMBIS, &dtr_bit);
+}
+
+void raw_serial::clearDTR()
+{
+    if ( !isOpened() ) return;
+
+    uint32_t dtr_bit = TIOCM_DTR;
+    ioctl(serial_fd, TIOCMBIC, &dtr_bit);
+}
+
+void raw_serial::_init()
+{
+    serial_fd = -1;  
+    _portName[0] = 0;
+    required_tx_cnt = required_rx_cnt = 0;
+    _operation_aborted = false;
+    _selfpipe[0] = _selfpipe[1] = -1;
+}
+
+void raw_serial::cancelOperation()
+{
+    _operation_aborted = true;
+    if (_selfpipe[1] == -1) return;
+
+    (int)::write(_selfpipe[1], "x", 1);
+}
+
+_u32 raw_serial::getTermBaudBitmap(_u32 baud)
+{
+#define BAUD_CONV( _baud_) case _baud_:  return B##_baud_ 
+switch (baud) {
+        BAUD_CONV(1200);
+        BAUD_CONV(1800);
+        BAUD_CONV(2400);
+        BAUD_CONV(4800);
+        BAUD_CONV(9600);
+        BAUD_CONV(19200);
+        BAUD_CONV(38400);
+        BAUD_CONV(57600);
+        BAUD_CONV(115200);
+        BAUD_CONV(230400);
+        BAUD_CONV(460800);
+        BAUD_CONV(500000);
+        BAUD_CONV(576000);
+        BAUD_CONV(921600);
+        BAUD_CONV(1000000);
+        BAUD_CONV(1152000);
+        BAUD_CONV(1500000);
+        BAUD_CONV(2000000);
+        BAUD_CONV(2500000);
+        BAUD_CONV(3000000);
+        BAUD_CONV(3500000);
+        BAUD_CONV(4000000);
+    }
+    return -1;
+}
+
+}}} //end rp::arch::net
+
+//begin rp::hal
+namespace rp{ namespace hal{
+
+serial_rxtx * serial_rxtx::CreateRxTx()
+{
+    return new rp::arch::net::raw_serial();
+}
+
+void serial_rxtx::ReleaseRxTx(serial_rxtx *rxtx)
+{
+    delete rxtx;
+}
+
+}} //end rp::hal

third_party/rplidar_sdk/src/arch/linux/net_serial.h

@@ -0,0 +1,90 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "hal/abs_rxtx.h"
+
+namespace rp{ namespace arch{ namespace net{
+
+class raw_serial : public rp::hal::serial_rxtx
+{
+public:
+    enum{
+        SERIAL_RX_BUFFER_SIZE = 512,
+        SERIAL_TX_BUFFER_SIZE = 128,
+    };
+
+    raw_serial();
+    virtual ~raw_serial();
+    virtual bool bind(const char * portname, uint32_t baudrate, uint32_t flags = 0);
+    virtual bool open();
+    virtual void close();
+    virtual void flush( _u32 flags);
+    
+    virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual int senddata(const unsigned char * data, size_t size);
+    virtual int recvdata(unsigned char * data, size_t size);
+
+    virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL);
+    virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL);
+
+    virtual size_t rxqueue_count();
+
+    virtual void setDTR();
+    virtual void clearDTR();
+
+    _u32 getTermBaudBitmap(_u32 baud);
+
+    virtual void cancelOperation();
+
+protected:
+    bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0);
+    void _init();
+
+    char _portName[200];
+    uint32_t _baudrate;
+    uint32_t _flags;
+
+    int serial_fd;
+
+    size_t required_tx_cnt;
+    size_t required_rx_cnt;
+
+    int    _selfpipe[2];
+    bool   _operation_aborted;
+};
+
+}}}

third_party/rplidar_sdk/src/arch/linux/net_socket.cpp

@@ -0,0 +1,893 @@
+/*
+ *  RoboPeak Project
+ *  HAL Layer - Socket Interface
+ *  Copyright 2009 - 2013 RoboPeak Project
+ *
+ *  POXIS Implementation
+ */
+
+
+#include "sdkcommon.h"
+#include "../../hal/socket.h"
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
+
+
+namespace rp{ namespace net {
+
+
+static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) 
+{
+    switch (type) {
+        case SocketAddress::ADDRESS_TYPE_INET:
+            return AF_INET;
+        case SocketAddress::ADDRESS_TYPE_INET6:
+            return AF_INET6;
+        case SocketAddress::ADDRESS_TYPE_UNSPEC:
+            return AF_UNSPEC;
+
+        default:
+            assert(!"should not reach here");
+            return AF_UNSPEC;
+    }
+}
+
+
+SocketAddress::SocketAddress() 
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+}
+
+SocketAddress::SocketAddress(const SocketAddress & src)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+}
+
+
+
+SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type)
+{
+    _platform_data = reinterpret_cast<void *>(new sockaddr_storage);
+    memset(_platform_data, 0, sizeof(sockaddr_storage));
+    
+    // default to ipv4 in case the following operation fails
+    reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+
+    setAddressFromString(addrString, type);
+    setPort(port);
+}
+
+SocketAddress::SocketAddress(void * platform_data)
+ : _platform_data(platform_data)
+{}
+
+SocketAddress & SocketAddress::operator = (const SocketAddress &src)
+{
+    memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage));
+    return *this;
+}
+
+
+SocketAddress::~SocketAddress()
+{
+    delete reinterpret_cast<sockaddr_storage *>(_platform_data);
+}
+
+SocketAddress::address_type_t SocketAddress::getAddressType() const
+{
+    switch(reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family) {
+        case AF_INET:
+            return ADDRESS_TYPE_INET;
+        case AF_INET6:
+            return ADDRESS_TYPE_INET6;
+        default:
+            assert(!"should not reach here");
+            return ADDRESS_TYPE_INET;
+    }
+}
+
+int SocketAddress::getPort() const
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_port);
+        case ADDRESS_TYPE_INET6:
+            return (int)ntohs(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_port);
+        default:
+            return 0;
+    }
+}
+
+u_result SocketAddress::setPort(int port)
+{
+    switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_in *>(_platform_data)->sin_port = htons((short)port);
+            break;
+        case ADDRESS_TYPE_INET6:
+            reinterpret_cast<sockaddr_in6 *>(_platform_data)->sin6_port = htons((short)port);
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+u_result SocketAddress::setAddressFromString(const char * address_string,  SocketAddress::address_type_t type)
+{
+    int ans = 0;
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET;
+            ans = inet_pton(AF_INET, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in *>(_platform_data)->sin_addr);
+        break;
+
+
+        case ADDRESS_TYPE_INET6:
+            
+            reinterpret_cast<sockaddr_storage *>(_platform_data)->ss_family = AF_INET6;
+            ans = inet_pton(AF_INET6, 
+                            address_string, 
+                            &reinterpret_cast<sockaddr_in6  *>(_platform_data)->sin6_addr);
+        break;
+
+        default:
+            return RESULT_INVALID_DATA;
+
+    }
+    setPort(prevPort);
+
+    return ans<=0?RESULT_INVALID_DATA:RESULT_OK;
+}
+
+
+u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const
+{
+    int net_family = reinterpret_cast<const sockaddr_storage *>(_platform_data)->ss_family;
+    const char *ans = NULL;
+    switch (net_family) {
+        case AF_INET:
+            ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr,
+                            buffer, buffersize);
+        break;
+
+        case AF_INET6:
+            ans = inet_ntop(net_family, &reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr,
+                            buffer, buffersize);
+
+        break;
+    }
+    return ans==NULL?RESULT_OPERATION_FAIL:RESULT_OK;
+}
+
+
+
+size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS, SocketAddress::address_type_t type)
+{
+    struct addrinfo hints;
+    struct addrinfo *result;
+    int ans;
+
+    memset(&hints, 0, sizeof(struct addrinfo));
+    hints.ai_family = _halAddrTypeToOSType(type);
+    hints.ai_flags = AI_PASSIVE;
+
+    if (!performDNS) {
+        hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST;
+    
+    }
+
+    ans = getaddrinfo(hostname, sevicename, &hints, &result);
+
+    addresspool.clear();
+
+    if (ans != 0) {
+        // hostname loopup failed
+        return 0;
+    }
+
+    
+    for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) {
+        if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) {
+            sockaddr_storage * storagebuffer = new sockaddr_storage;
+            assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen);
+            memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen);
+            addresspool.push_back(SocketAddress(storagebuffer));
+        }
+    }
+
+    
+    freeaddrinfo(result);
+
+    return addresspool.size();
+}
+
+
+u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const
+{
+     switch (getAddressType()) {
+        case ADDRESS_TYPE_INET:
+            if (bufferSize < sizeof(in_addr::s_addr)) return RESULT_INSUFFICIENT_MEMORY;
+
+            memcpy(buffer, &reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast<const sockaddr_in *>(_platform_data)->sin_addr.s_addr));
+
+            
+            break;
+        case ADDRESS_TYPE_INET6:
+            if (bufferSize < sizeof(in6_addr::s6_addr)) return RESULT_INSUFFICIENT_MEMORY;
+            memcpy(buffer, reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast<const sockaddr_in6 *>(_platform_data)->sin6_addr.s6_addr));
+
+            break;
+        default:
+            return RESULT_OPERATION_FAIL;
+    }
+    return RESULT_OK;
+}
+
+
+void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type)
+{
+
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_loopback;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+}
+
+void SocketAddress::setBroadcastAddressIPv4()
+{
+
+    int prevPort = getPort();
+    sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+    addrv4->sin_family = AF_INET;
+    addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST);
+    setPort(prevPort);
+
+}
+
+void SocketAddress::setAnyAddress(SocketAddress::address_type_t type)
+{
+    int prevPort = getPort();
+    switch (type) {
+        case ADDRESS_TYPE_INET:
+            {
+                sockaddr_in * addrv4 = reinterpret_cast<sockaddr_in *>(_platform_data);
+                addrv4->sin_family = AF_INET;
+                addrv4->sin_addr.s_addr = htonl(INADDR_ANY);
+            }
+            break;
+        case ADDRESS_TYPE_INET6:
+            {
+                sockaddr_in6  * addrv6 = reinterpret_cast<sockaddr_in6  *>(_platform_data);
+                addrv6->sin6_family = AF_INET6;
+                addrv6->sin6_addr = in6addr_any;
+
+            }
+            break;
+        default:
+            return;
+    }
+
+    setPort(prevPort);
+
+
+}
+
+
+}}
+
+
+
+///--------------------------------
+
+
+namespace rp { namespace arch { namespace net{ 
+
+using namespace rp::net;
+
+class _single_thread StreamSocketImpl : public StreamSocket
+{
+public:
+
+    StreamSocketImpl(int fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, sizeof(bool_true) );
+
+        enableNoDelay(true);
+        this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~StreamSocketImpl() 
+    {
+        close(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>( const_cast<void *>(localaddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+    virtual u_result connect(const SocketAddress & pairAddress)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(pairAddress.getPlatformData());
+        int ans = ::connect(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (!ans) return RESULT_OK;
+
+
+        switch (errno) {
+            case EAFNOSUPPORT:
+                return RESULT_OPERATION_NOT_SUPPORT;
+#if 0
+            case EINPROGRESS:
+                return RESULT_OK; //treat async connection as good status
+#endif
+            case ETIMEDOUT:
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+      
+    virtual u_result listen(int backlog)
+    {
+        int ans = ::listen( _socket_fd,   backlog);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual StreamSocket * accept(SocketAddress * pairAddress) 
+    {
+        size_t addrsize;
+        addrsize = sizeof(sockaddr_storage);
+        int pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast<struct sockaddr *>(const_cast<void *>(pairAddress->getPlatformData())):NULL
+            , (socklen_t*)&addrsize);
+
+        if (pair_socket>=0) {
+            return new StreamSocketImpl(pair_socket);
+        } else {
+            return NULL;
+        }
+    }
+
+    virtual u_result waitforIncomingConnection(_u32 timeout)
+    {
+        return waitforData(timeout);
+    }
+
+    virtual u_result send(const void * buffer, size_t len) 
+    {
+        size_t ans = ::send( _socket_fd, buffer, len, MSG_NOSIGNAL);
+        if (ans == len) {
+            return RESULT_OK;
+        } else {
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+        }
+        
+    }
+
+
+    virtual u_result recv(void *buf, size_t len, size_t & recv_len)
+    {
+        size_t ans = ::recv( _socket_fd, buf, len, 0);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+
+            
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+    }
+
+#if 0
+    virtual u_result recvNoWait(void *buf, size_t len, size_t & recv_len)
+    {
+        size_t ans = ::recv( _socket_fd, buf, len, MSG_DONTWAIT);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return RESULT_OK; 
+            } else {
+                return RESULT_OPERATION_FAIL;
+            }
+
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+#endif
+
+    virtual u_result getPeerAddress(SocketAddress & peerAddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>(peerAddr.getPlatformData())); //donnot do this at home...
+        assert(addr);
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getpeername(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <=  sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+
+    }
+
+    virtual u_result shutdown(socket_direction_mask mask)
+    {
+        int shutdw_opt ;
+
+        switch (mask) {
+            case SOCKET_DIR_RD:
+                shutdw_opt = SHUT_RD;
+                break;
+            case SOCKET_DIR_WR:
+                shutdw_opt = SHUT_WR;
+                break;
+            case SOCKET_DIR_BOTH:
+            default:
+                shutdw_opt = SHUT_RDWR;
+        }
+
+        int ans = ::shutdown(_socket_fd, shutdw_opt);
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableKeepAlive(bool enable)
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , &bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result enableNoDelay(bool enable ) 
+    {
+        int bool_true = enable?1:0;
+        return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY,&bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+protected:
+    int  _socket_fd;
+
+
+};
+
+
+class _single_thread DGramSocketImpl : public DGramSocket
+{
+public:
+
+    DGramSocketImpl(int fd)
+        : _socket_fd(fd)
+    {
+        assert(fd>=0);
+        int bool_true = 1;
+        ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, sizeof(bool_true) );
+        setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH);
+    }
+
+    virtual ~DGramSocketImpl() 
+    {
+        close(_socket_fd);
+    }
+
+    virtual void dispose()
+    {
+        delete this;
+    }
+
+
+    virtual u_result bind(const SocketAddress & localaddr)
+    {
+        const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(localaddr.getPlatformData());
+        assert(addr);
+        int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage));
+        if (ans) {
+            return RESULT_OPERATION_FAIL;
+        } else {
+            return RESULT_OK;
+        }
+    }
+
+    virtual u_result getLocalAddress(SocketAddress & localaddr)
+    {
+        struct sockaddr * addr = reinterpret_cast<struct sockaddr *>(const_cast<void *>((localaddr.getPlatformData()))); //donnot do this at home...
+        assert(addr);
+
+        size_t actualsize =  sizeof(sockaddr_storage);
+        int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize);
+
+        assert(actualsize <= sizeof(sockaddr_storage));
+        assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6);
+
+        return ans?RESULT_OPERATION_FAIL:RESULT_OK;
+    }
+
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk)
+    {
+        int ans;
+        timeval tv;
+        tv.tv_sec = timeout / 1000; 
+        tv.tv_usec = (timeout % 1000) * 1000; 
+
+        if (msk & SOCKET_DIR_RD) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        if (msk & SOCKET_DIR_WR) {
+             ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) );
+             if (ans) return RESULT_OPERATION_FAIL;
+        }
+
+        return RESULT_OK;
+    }
+  
+
+    virtual u_result waitforSent(_u32 timeout ) 
+    {
+        fd_set wrset;
+        FD_ZERO(&wrset);
+        FD_SET(_socket_fd, &wrset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result waitforData(_u32 timeout )
+    {
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        timeval tv;
+        tv.tv_sec = timeout / 1000;
+        tv.tv_usec = (timeout % 1000) * 1000;
+        int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv);
+
+        switch (ans) {
+            case 1:
+                // fired
+                return RESULT_OK;
+            case 0:
+                // timeout
+                return RESULT_OPERATION_TIMEOUT;
+            default:
+                delay(0); //relax cpu
+                return RESULT_OPERATION_FAIL;
+        }
+    }
+
+    virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len)
+    {
+        const struct sockaddr * addr = target ? reinterpret_cast<const struct sockaddr *>(target->getPlatformData()) : NULL;
+        int dest_addr_size = (target ? sizeof(sockaddr_storage) : 0);
+        int ans = ::sendto(_socket_fd, (const char *)buffer, (int)len, 0, addr, dest_addr_size);
+        if (ans != -1) {
+            assert(ans == len);
+            return RESULT_OK;
+        } else {
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+
+                case EMSGSIZE:
+                    return RESULT_INVALID_DATA;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+        
+        }
+
+    }
+
+    virtual u_result setPairAddress(const SocketAddress* pairAddress)
+    {
+        sockaddr_storage unspecAddr;
+        unspecAddr.ss_family = AF_UNSPEC;
+
+        const struct sockaddr* addr = pairAddress ? reinterpret_cast<const struct sockaddr*>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr*>(&unspecAddr);
+        int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage));
+        return ans ? RESULT_OPERATION_FAIL : RESULT_OK;
+
+    }
+    
+    virtual u_result clearRxCache()
+    {
+        timeval tv;
+        tv.tv_sec = 0;
+        tv.tv_usec = 0;
+        fd_set rdset;
+        FD_ZERO(&rdset);
+        FD_SET(_socket_fd, &rdset);
+
+        int res = -1;
+        char recv_data[2];
+        memset(recv_data, 0, sizeof(recv_data));
+        while (true) {
+            res = select(FD_SETSIZE, &rdset, nullptr, nullptr, &tv);
+            if (res == 0) break;
+            recv(_socket_fd, recv_data, 1, 0);
+        }
+        return RESULT_OK;
+    }
+
+    virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+        size_t ans = ::recvfrom( _socket_fd, buf, len, 0, addr, (socklen_t*)&source_addr_size);
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            switch (errno) {
+                case EAGAIN:
+#if EWOULDBLOCK!=EAGAIN
+                case EWOULDBLOCK:
+#endif
+                    return RESULT_OPERATION_TIMEOUT;
+                default:
+                    return RESULT_OPERATION_FAIL;
+            }
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+
+#if 0
+    virtual u_result recvFromNoWait(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr)
+    {
+        struct sockaddr * addr = (sourceAddr?reinterpret_cast<struct sockaddr *>(const_cast<void *>(sourceAddr->getPlatformData())):NULL);
+        size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0);
+
+
+        size_t ans = ::recvfrom( _socket_fd, buf, len, MSG_DONTWAIT, addr, &source_addr_size);
+
+        if (ans == (size_t)-1) {
+            recv_len = 0;  
+            if (errno == EAGAIN || errno == EWOULDBLOCK) {
+                return RESULT_OK; 
+            } else {
+                return RESULT_OPERATION_FAIL;
+            }
+
+
+        } else {
+            recv_len = ans;
+            return RESULT_OK;
+        }
+
+    }
+#endif
+    
+protected:
+    int  _socket_fd;
+
+};
+
+
+}}}
+
+
+namespace rp { namespace net{ 
+
+
+static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family)
+{
+    switch (family) {
+        case SocketBase::SOCKET_FAMILY_INET:
+            return AF_INET;
+        case SocketBase::SOCKET_FAMILY_INET6:
+            return AF_INET6;
+        case SocketBase::SOCKET_FAMILY_RAW:
+            return AF_PACKET;
+        default:
+            assert(!"should not reach here");
+            return AF_INET; // force treating as IPv4 in release mode
+    }
+
+}
+
+StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    if (family == SOCKET_FAMILY_RAW) return NULL;
+
+
+    int socket_family = _socketHalFamilyToOSFamily(family);
+    int socket_fd = ::socket(socket_family, SOCK_STREAM, 0);
+    if (socket_fd == -1) return NULL;
+
+    StreamSocket * newborn = static_cast<StreamSocket *>(new rp::arch::net::StreamSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family)
+{
+    int socket_family = _socketHalFamilyToOSFamily(family);
+
+
+    int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0);
+    if (socket_fd == -1) return NULL;
+
+    DGramSocket * newborn = static_cast<DGramSocket *>(new rp::arch::net::DGramSocketImpl(socket_fd));
+    return newborn;
+
+}
+
+
+}}
+

third_party/rplidar_sdk/src/arch/linux/thread.hpp

@@ -0,0 +1,185 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+
+#include <sched.h>
+#include <sys/types.h>
+#include <sys/syscall.h>
+#include <sys/time.h>
+#include <sys/resource.h>
+
+namespace rp{ namespace hal{
+
+Thread Thread::create(thread_proc_t proc, void * data)
+{
+    Thread newborn(proc, data);
+    
+    // tricky code, we assume pthread_t is not a structure but a word size value
+    assert( sizeof(newborn._handle) >= sizeof(pthread_t));
+
+    pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data);
+
+    return newborn;
+}
+
+u_result Thread::terminate()
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL;
+}
+
+u_result Thread::SetSelfPriority( priority_val_t p)
+{
+
+    pid_t selfTid = syscall(SYS_gettid);
+
+        // check whether current schedule policy supports priority levels
+    int current_policy = SCHED_OTHER;
+    struct sched_param current_param;
+    int nice = 0;
+    int ans;
+
+    if (sched_getparam(selfTid, &current_param))
+    {
+        // cannot retreieve values
+        return RESULT_OPERATION_FAIL;
+    }   
+
+    int pthread_priority_min;
+
+#if 1
+    pthread_priority_min = sched_get_priority_min(SCHED_RR);
+#else
+    pthread_priority_min = 1;
+#endif
+	int pthread_priority = 0 ;
+
+	switch(p)
+	{
+	case PRIORITY_REALTIME:
+		//pthread_priority = pthread_priority_max;
+        current_policy = SCHED_RR;
+        pthread_priority = pthread_priority_min + 1;
+        nice = 0;
+		break;
+	case PRIORITY_HIGH:
+		//pthread_priority = (pthread_priority_max + pthread_priority_min)/2;
+        current_policy = SCHED_RR;
+        pthread_priority = pthread_priority_min;
+        nice = 0;
+		break;
+	case PRIORITY_NORMAL:
+        pthread_priority = 0;
+        current_policy = SCHED_OTHER;
+        nice = 0;
+        break;
+	case PRIORITY_LOW:
+        pthread_priority = 0;
+        current_policy = SCHED_OTHER;
+        nice = 10;
+        break;
+	case PRIORITY_IDLE:
+		pthread_priority = 0;
+        current_policy = SCHED_IDLE;
+        nice = 0;
+		break;
+	}
+    // change the inhertiable behavior
+    current_policy |= SCHED_RESET_ON_FORK;
+
+    current_param.__sched_priority = pthread_priority;
+
+  
+
+    
+    // do not use pthread version as it will make the priority be inherited by a thread child
+	if ( (ans = sched_setscheduler(selfTid, current_policy , &current_param)) )
+	{
+        if (ans == EPERM)
+        {
+            //DBG_PRINT("warning, current process hasn't the right permission to set threads priority\n");
+        }
+		return RESULT_OPERATION_FAIL;
+	}
+
+
+    if ((current_policy == SCHED_OTHER) || (current_policy == SCHED_BATCH))
+    {
+        if (setpriority(PRIO_PROCESS, selfTid, nice)) {
+            return RESULT_OPERATION_FAIL;
+        }
+    }
+    
+
+	return  RESULT_OK;
+}
+
+Thread::priority_val_t Thread::getPriority()
+{
+	if (!this->_handle) return PRIORITY_NORMAL;
+
+    int current_policy;
+    struct sched_param current_param;
+    if (pthread_getschedparam( (pthread_t) this->_handle, &current_policy, &current_param))
+    {
+        // cannot retreieve values
+        return PRIORITY_NORMAL;
+    }   
+
+    int pthread_priority_max = sched_get_priority_max(SCHED_RR);
+    int pthread_priority_min = sched_get_priority_min(SCHED_RR);
+
+    if (current_param.__sched_priority ==(pthread_priority_max ))
+	{
+		return PRIORITY_REALTIME;
+	}
+	if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2)
+	{
+		return PRIORITY_HIGH;
+	}
+	return PRIORITY_NORMAL;
+}
+
+u_result Thread::join(unsigned long timeout)
+{
+    if (!this->_handle) return RESULT_OK;
+    
+    pthread_join((pthread_t)(this->_handle), NULL);
+    this->_handle = 0;
+    return RESULT_OK;
+}
+
+}}

third_party/rplidar_sdk/src/arch/linux/timer.cpp

@@ -0,0 +1,52 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "arch/linux/arch_linux.h"
+
+namespace rp{ namespace arch{
+_u64 rp_getus()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return t.tv_sec*1000000LL + t.tv_nsec/1000;
+}
+_u64 rp_getms()
+{
+    struct timespec t;
+    t.tv_sec = t.tv_nsec = 0;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    return t.tv_sec*1000L + t.tv_nsec/1000000L;
+}
+}}

third_party/rplidar_sdk/src/arch/linux/timer.h

@@ -0,0 +1,59 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "hal/types.h"
+
+#include <unistd.h>
+static inline void delay(_word_size_t ms){
+    while (ms>=1000){
+        usleep(1000*1000);
+        ms-=1000;
+    };
+    if (ms!=0)
+        usleep(ms*1000);
+}
+
+// TODO: the highest timer interface should be clock_gettime
+namespace rp{ namespace arch{
+
+_u64 rp_getus();
+_u64 rp_getms();
+
+}}
+
+#define getms() rp::arch::rp_getms()
+#define getus() rp::arch::rp_getus()
+

third_party/rplidar_sdk/src/dataunpacker/dataunnpacker_commondef.h

@@ -0,0 +1,60 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  External Reference and dependencies
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#pragma once
+
+#include "sdkcommon.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+#include "hal/waiter.h"
+#include "hal/byteorder.h"
+#include "sl_lidar_driver.h"
+#include "sl_crc.h" 
+#include <algorithm>
+#include <memory>
+
+#define CONF_NO_BOOST_CRC_SUPPORT
+
+#include "dataupacker_namespace.h"
+
+

third_party/rplidar_sdk/src/dataunpacker/dataunnpacker_internal.h

@@ -0,0 +1,74 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Internal Definition
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#pragma once
+
+BEGIN_DATAUNPACKER_NS()
+
+
+class LIDARSampleDataUnpackerInner: public LIDARSampleDataUnpacker
+{
+public:
+	LIDARSampleDataUnpackerInner(LIDARSampleDataListener& l): LIDARSampleDataUnpacker(l){}
+
+	virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0;
+	virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size) = 0;
+	virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size) = 0;
+	virtual void publishNewScanReset() = 0;
+
+
+	virtual _u64 getCurrentTimestamp_uS() = 0;
+
+};
+
+class IDataUnpackerHandler
+{
+public:
+	IDataUnpackerHandler() {}
+	virtual ~IDataUnpackerHandler() {}
+
+
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size) = 0;
+
+	virtual _u8 getSampleAnswerType() const = 0;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size) = 0;
+	virtual void reset() = 0;
+
+};
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/dataunpacker.cpp

@@ -0,0 +1,259 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#include "dataunnpacker_commondef.h"
+#include "dataunpacker.h"
+#include "dataunnpacker_internal.h"
+
+
+#include <map>
+
+
+#define REGISTER_HANDLER(_c_) {     \
+		auto newBorn = new unpacker::_c_();   \
+		if (!newBorn) return false; \
+		handlerList.push_back(newBorn); \
+	} 
+
+// How to include new handlers?
+// 1. add extra include line below if a new handle is to be included
+// 2. update the code in function _registerDataUnpackerHandlers
+#include "unpacker/handler_capsules.h"
+#include "unpacker/handler_hqnode.h"
+#include "unpacker/handler_normalnode.h"
+
+
+#define  DEF_REGISTER_HANDLER_LIST
+
+
+BEGIN_DATAUNPACKER_NS()
+
+
+static bool _registerDataUnpackerHandlers(std::vector<IDataUnpackerHandler *> & handlerList)
+{
+	REGISTER_HANDLER(UnpackerHandler_NormalNode);
+	REGISTER_HANDLER(UnpackerHandler_HQNode);
+	REGISTER_HANDLER(UnpackerHandler_CapsuleNode);
+	REGISTER_HANDLER(UnpackerHandler_UltraCapsuleNode);
+	REGISTER_HANDLER(UnpackerHandler_DenseCapsuleNode);
+	REGISTER_HANDLER(UnpackerHandler_UltraDenseCapsuleNode);
+	return true;
+}
+
+
+class LIDARSampleDataUnpackerImpl : public LIDARSampleDataUnpackerInner
+{
+public:
+
+	void registerHandler(_u8 ansType, IDataUnpackerHandler* handler)
+	{
+		_handlerMap[ansType] = handler;
+	}
+
+
+	void unregisterAllHandlers()
+	{
+		for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr)
+		{
+			delete itr->second;
+		}
+		_handlerMap.clear();
+	}
+
+	LIDARSampleDataUnpackerImpl(LIDARSampleDataListener& l)
+		: LIDARSampleDataUnpackerInner(l)
+		, _enabled(false)
+		, _lastActiveAnsType(0)
+		, _lastActiveHandler(nullptr)
+	{
+
+	}
+
+	virtual ~LIDARSampleDataUnpackerImpl()
+	{
+		unregisterAllHandlers();
+	}
+
+
+	virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size)
+	{
+	
+		// notify the handlers ...
+		for (auto itr = _handlerMap.begin(); itr != _handlerMap.end(); ++itr)
+		{
+			itr->second->onUnpackerContextSet(type, data, size);
+		}
+	}
+
+	virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) {
+		if (!_enabled) return false;
+
+
+		if (_lastActiveAnsType != ansType) {
+			onDeselectHandler();
+
+			auto itr = _handlerMap.find(ansType);
+			if (itr != _handlerMap.end()) {
+				onSelectHandler(ansType, itr->second);
+			}
+			else {
+				onSelectHandler(ansType, nullptr);
+			}
+			
+		}
+
+		if (_lastActiveHandler) {
+			_lastActiveHandler->onData(this, reinterpret_cast<const _u8 *>(buffer), size);
+			return true;
+		}
+		else {
+			return false;
+		}
+	}
+
+	virtual void reset()
+	{
+		clearCache();
+		_lastActiveHandler = nullptr;
+		_lastActiveAnsType = 0;
+
+	}
+
+	virtual void enable()
+	{
+		_enabled = true;
+		reset();
+	}
+
+	virtual void disable()
+	{
+		_enabled = false;
+		reset();
+
+	}
+
+	virtual void clearCache()
+	{
+		if (_lastActiveHandler) {
+			_lastActiveHandler->reset();
+		}
+	}
+
+	virtual _u64 getCurrentTimestamp_uS() {
+		return getus();
+	}
+
+	virtual void publishHQNode(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node)
+	{
+		_listener.onHQNodeDecoded(timestamp_uS, node);
+	}
+
+
+	virtual void publishDecodingErrorMsg(int errorType, _u8 ansType, const void* payload, size_t size)
+	{
+		_listener.onDecodingError(errorType, ansType, payload, size);
+
+	}
+
+	virtual void publishCustomData(_u8 ansType, _u32 customCode, const void* payload, size_t size)
+	{
+		_listener.onCustomSampleDataDecoded(ansType, customCode, payload, size);
+	}
+
+
+	virtual void publishNewScanReset()
+	{
+		_listener.onHQNodeScanResetReq();
+	}
+protected:
+
+	void onSelectHandler(_u8 ansType, IDataUnpackerHandler* handler)
+	{
+		_lastActiveHandler = handler;
+		_lastActiveAnsType = ansType;
+	}
+
+	void onDeselectHandler()
+	{
+		reset();
+	}
+
+
+protected:
+	bool _enabled;
+	std::map<_u8, IDataUnpackerHandler*> _handlerMap;
+
+	_u8 _lastActiveAnsType;
+	IDataUnpackerHandler* _lastActiveHandler;
+};
+
+LIDARSampleDataUnpacker* LIDARSampleDataUnpacker::CreateInstance(LIDARSampleDataListener& listener)
+{
+	LIDARSampleDataUnpackerImpl* impl = new LIDARSampleDataUnpackerImpl(listener);
+	
+	std::vector<IDataUnpackerHandler*> list;
+	if (!_registerDataUnpackerHandlers(list)) {
+		delete  impl;
+		for (auto itr = list.begin(); itr != list.end(); ++itr) {
+			delete* itr;
+		}
+		impl = nullptr;
+	}
+
+	for (auto itr = list.begin(); itr != list.end(); ++itr) {
+		impl->registerHandler((*itr)->getSampleAnswerType(), (*itr));
+	}
+	return impl;
+}
+
+void LIDARSampleDataUnpacker::ReleaseInstance(LIDARSampleDataUnpacker* unpacker) {
+	delete unpacker;
+}
+
+LIDARSampleDataUnpacker::~LIDARSampleDataUnpacker() {
+
+}
+
+LIDARSampleDataUnpacker::LIDARSampleDataUnpacker(LIDARSampleDataListener& l) 
+	: _listener(l)
+{
+
+}
+
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/dataunpacker.h

@@ -0,0 +1,93 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+
+#pragma once
+
+#include "dataupacker_namespace.h"
+
+BEGIN_DATAUNPACKER_NS()
+
+
+class LIDARSampleDataListener
+{
+
+
+public:
+	virtual void onHQNodeScanResetReq() = 0;
+	virtual void onHQNodeDecoded(_u64 timestamp_uS, const rplidar_response_measurement_node_hq_t* node) = 0;
+	virtual void onCustomSampleDataDecoded(_u8 ansType, _u32 customCode, const void* data, size_t size) {}
+
+	virtual void onDecodingError(int errMsg, _u8 ansType, const void* payload, size_t size) {}
+};
+
+class LIDARSampleDataUnpacker
+{
+public:
+	enum {
+		ERR_EVENT_ON_EXP_ENCODER_RESET = 0x8001,
+		ERR_EVENT_ON_EXP_CHECKSUM_ERR = 0x8002,
+	};
+
+	enum UnpackerContextType {
+		UNPACKER_CONTEXT_TYPE_LIDAR_UNKNOWN = 0,
+		UNPACKER_CONTEXT_TYPE_LIDAR_TIMING = 1,
+		UNPACKER_CONTEXT_TYPE_TRIANGULATION_OPTICAL_FACTOR = 2,
+
+	};
+
+	virtual ~LIDARSampleDataUnpacker();
+	static LIDARSampleDataUnpacker* CreateInstance(LIDARSampleDataListener& listener);
+	static void ReleaseInstance(LIDARSampleDataUnpacker*);
+
+	virtual void updateUnpackerContext(UnpackerContextType type, const void* data, size_t size) = 0;
+
+	virtual void enable() = 0;
+	virtual void disable() = 0;
+
+	virtual bool onSampleData(_u8 ansType, const void* buffer, size_t size) = 0;
+	virtual void reset() = 0;
+	virtual void clearCache() = 0;
+
+protected:
+	LIDARSampleDataUnpacker(LIDARSampleDataListener&);
+	LIDARSampleDataListener& _listener;
+
+};
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/dataupacker_namespace.h

@@ -0,0 +1,5 @@
+#pragma once
+
+
+#define BEGIN_DATAUNPACKER_NS()  namespace sl{ namespace internal{
+#define END_DATAUNPACKER_NS()  }}

third_party/rplidar_sdk/src/dataunpacker/unpacker/handler_capsules.h

@@ -0,0 +1,149 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Capsule Style Sample Node Handlers
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#pragma once
+
+BEGIN_DATAUNPACKER_NS()
+
+namespace unpacker {
+
+class UnpackerHandler_CapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_CapsuleNode();
+	virtual ~UnpackerHandler_CapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+
+	void _onScanNodeCapsuleData(rplidar_response_capsule_measurement_nodes_t &, LIDARSampleDataUnpackerInner* engine);
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+};
+
+class UnpackerHandler_UltraCapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_UltraCapsuleNode();
+	virtual ~UnpackerHandler_UltraCapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	void _onScanNodeUltraCapsuleData(rplidar_response_ultra_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
+
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+
+};
+
+
+
+class UnpackerHandler_DenseCapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_DenseCapsuleNode();
+	virtual ~UnpackerHandler_DenseCapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	void _onScanNodeDenseCapsuleData(rplidar_response_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
+
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+
+};
+
+
+class UnpackerHandler_UltraDenseCapsuleNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_UltraDenseCapsuleNode();
+	virtual ~UnpackerHandler_UltraDenseCapsuleNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	void _onScanNodeUltraDenseCapsuleData(rplidar_response_ultra_dense_capsule_measurement_nodes_t&, LIDARSampleDataUnpackerInner* engine);
+
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+	bool             _is_previous_capsuledataRdy;
+
+	rplidar_response_ultra_dense_capsule_measurement_nodes_t _cached_previous_ultra_dense_capsuledata;
+	_u64             _cached_last_data_timestamp_us;
+
+
+
+	int              _last_node_sync_bit;
+	int              _last_dist_q2;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+};
+
+
+}
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/unpacker/handler_hqnode.cpp

@@ -0,0 +1,192 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  HQNode Sample Node Handler
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#include "../dataunnpacker_commondef.h"
+#include "../dataunpacker.h"
+#include "../dataunnpacker_internal.h"
+
+#ifdef CONF_NO_BOOST_CRC_SUPPORT
+#include "sl_crc.h" 
+#endif
+
+#include "handler_hqnode.h"
+
+BEGIN_DATAUNPACKER_NS()
+	
+namespace unpacker{
+
+
+static _u64 _getSampleDelayOffsetInHQMode(const SlamtecLidarTimingDesc& timing)
+{
+    // FIXME: to eval
+    // 
+    // guess channel baudrate by LIDAR model ....
+    const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:1000000;
+
+    _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_hq_t) * 10 / channelBaudRate;
+
+    if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
+    {
+        tranmissionDelay = 100; //dummy value
+    }
+
+    // center of the sample duration
+    const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
+    const _u64 sampleFilterDelay = timing.sample_duration_uS;
+
+    return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS;
+}
+
+UnpackerHandler_HQNode::UnpackerHandler_HQNode()
+    : _cached_scan_node_buf_pos(0)
+{
+    _cached_scan_node_buf.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t));
+    memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
+}
+
+UnpackerHandler_HQNode::~UnpackerHandler_HQNode()
+{
+
+}
+
+_u8 UnpackerHandler_HQNode::getSampleAnswerType() const
+{
+	return RPLIDAR_ANS_TYPE_MEASUREMENT_HQ;
+}
+
+void UnpackerHandler_HQNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
+{
+
+    for (size_t pos = 0; pos < cnt; ++pos)
+    {
+        _u8 current_data = data[pos];
+
+        switch (_cached_scan_node_buf_pos)
+        {
+        case 0: // expect the sync byte
+        {
+            if (current_data == RPLIDAR_RESP_MEASUREMENT_HQ_SYNC) {
+                // pass
+            }
+            else {
+                continue;
+            }
+        }
+        break;
+
+        case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1 - 4:    // get bytes to calculate crc ready
+        {
+           
+        }
+        break;
+
+        case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1: // new data ready
+        {
+            _cached_scan_node_buf[sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1] = current_data;
+            _cached_scan_node_buf_pos = 0;
+            rplidar_response_hq_capsule_measurement_nodes_t* nodesData = reinterpret_cast<rplidar_response_hq_capsule_measurement_nodes_t*>(&_cached_scan_node_buf[0]);
+
+#ifdef CONF_NO_BOOST_CRC_SUPPORT
+            _u32 crcCalc = crc32::getResult(&_cached_scan_node_buf[0], sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 4);
+
+
+#else
+            // calculate crc with boost crc method
+            boost::crc_optimal<32, 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true> mycrc;
+            std::vector<_u8> crcInputData;
+            crcInputData.resize(sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4);
+            memcpy(&crcInputData[0], nodesData, sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4);
+            //supplement crcInputData to mutiples of 4
+            int leftBytes = 4 - (crcInputData.size() & 3);
+            for (int i = 0; i < leftBytes; i++)
+                crcInputData.push_back(0);
+            mycrc.process_bytes(&crcInputData[0], crcInputData.size());
+            _u32 crcCalc = mycrc.checksum();
+            
+#endif
+
+            _u32 recvCRC = nodesData->crc32;
+#ifdef _CPU_ENDIAN_BIG
+            recvCRC = le32_to_cpu(recvCRC);
+            nodesData->time_stamp = le64_to_cpu(nodesData->time_stamp);
+#endif
+            if (recvCRC == crcCalc)
+            {
+                for (size_t pos = 0; pos < _countof(nodesData->node_hq); ++pos)
+                {
+                    rplidar_response_measurement_node_hq_t hqNode = nodesData->node_hq[pos];
+#ifdef _CPU_ENDIAN_BIG
+                    hqNode.angle_z_q14 = le16_to_cpu(hqNode.angle_z_q14);
+                    hqNode.dist_mm_q2 = le32_to_cpu(hqNode.dist_mm_q2);
+#endif
+                    engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInHQMode(_cachedTimingDesc), &hqNode);
+                }
+            }
+            else  //crc check not passed 
+            {
+                engine->publishDecodingErrorMsg(LIDARSampleDataUnpacker::ERR_EVENT_ON_EXP_CHECKSUM_ERR
+                    , RPLIDAR_ANS_TYPE_MEASUREMENT_HQ, nodesData, sizeof(*nodesData));
+            }
+            continue;
+        }
+        break;
+
+
+        }
+        _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
+    }
+
+}
+
+
+void UnpackerHandler_HQNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
+{
+    if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
+        assert(size == sizeof(_cachedTimingDesc));
+        _cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
+    }
+}
+
+void UnpackerHandler_HQNode::reset()
+{
+    _cached_scan_node_buf_pos = 0;
+}
+}
+
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/unpacker/handler_hqnode.h

@@ -0,0 +1,63 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  HQNode Sample Node Handler
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#pragma once
+
+BEGIN_DATAUNPACKER_NS()
+
+namespace unpacker {
+
+	class UnpackerHandler_HQNode : public IDataUnpackerHandler {
+	public:
+		UnpackerHandler_HQNode();
+		virtual ~UnpackerHandler_HQNode();
+
+		virtual _u8 getSampleAnswerType() const;
+		virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+		virtual void reset();
+		virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+
+	protected:
+		std::vector<_u8> _cached_scan_node_buf;
+		int              _cached_scan_node_buf_pos;
+		SlamtecLidarTimingDesc _cachedTimingDesc;
+	};
+
+}
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/unpacker/handler_normalnode.cpp

@@ -0,0 +1,159 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Normal Sample Node Handler
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#include "../dataunnpacker_commondef.h"
+#include "../dataunpacker.h"
+#include "../dataunnpacker_internal.h"
+
+
+#include "handler_normalnode.h"
+
+BEGIN_DATAUNPACKER_NS()
+	
+namespace unpacker{
+
+
+static _u64 _getSampleDelayOffsetInLegacyMode(const SlamtecLidarTimingDesc& timing)
+{
+    // guess channel baudrate by LIDAR model ....
+    const _u64 channelBaudRate = timing.native_baudrate? timing.native_baudrate:115200;
+
+    _u64 tranmissionDelay = 1000000ULL * sizeof(rplidar_response_measurement_node_t) * 10 / channelBaudRate;
+
+    if (timing.native_interface_type == LIDARInterfaceType::LIDAR_INTERFACE_ETHERNET)
+    {
+        tranmissionDelay = 100; //dummy value
+    }
+
+    // center of the sample duration
+    const _u64 sampleDelay = (timing.sample_duration_uS >> 1);
+    const _u64 sampleFilterDelay = timing.sample_duration_uS;
+
+    return sampleFilterDelay + sampleDelay + tranmissionDelay + timing.linkage_delay_uS;
+}
+
+UnpackerHandler_NormalNode::UnpackerHandler_NormalNode()
+    : _cached_scan_node_buf_pos(0)
+{
+    _cached_scan_node_buf.resize(sizeof(rplidar_response_measurement_node_t));
+    memset(&_cachedTimingDesc, 0, sizeof(_cachedTimingDesc));
+;}
+
+UnpackerHandler_NormalNode::~UnpackerHandler_NormalNode()
+{
+
+}
+
+_u8 UnpackerHandler_NormalNode::getSampleAnswerType() const
+{
+	return RPLIDAR_ANS_TYPE_MEASUREMENT;
+}
+
+void UnpackerHandler_NormalNode::onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t cnt)
+{
+    for (size_t pos = 0; pos < cnt; ++pos) {
+        _u8 current_data = data[pos];
+        switch (_cached_scan_node_buf_pos) {
+        case 0: // expect the sync bit and its reverse in this byte
+        {
+            _u8 tmp = (current_data >> 1);
+            if ((tmp ^ current_data) & 0x1) {
+                // pass
+            }
+            else {
+                continue;
+            }
+
+        }
+        break;
+        case 1: // expect the highest bit to be 1
+        {
+            if (current_data & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
+                // pass
+            }
+            else {
+                _cached_scan_node_buf_pos = 0;
+                continue;
+            }
+        }
+        break;
+        case sizeof(rplidar_response_measurement_node_t) - 1: // new data ready
+        {
+            _cached_scan_node_buf[sizeof(rplidar_response_measurement_node_t) - 1] = current_data;
+            _cached_scan_node_buf_pos = 0;
+
+            rplidar_response_measurement_node_t* node = reinterpret_cast<rplidar_response_measurement_node_t*>(&_cached_scan_node_buf[0]);
+#ifdef _CPU_ENDIAN_BIG
+            node->angle_q6_checkbit = le16_to_cpu(node->angle_q6_checkbit);
+            node->distance_q2 = le16_to_cpu(node->distance_q2);
+#endif
+            //cast node to rplidar_response_measurement_node_hq_t
+            rplidar_response_measurement_node_hq_t hqNode;
+            hqNode.angle_z_q14 = (((node->angle_q6_checkbit) >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90;  //transfer to q14 Z-angle
+            hqNode.dist_mm_q2 = node->distance_q2;
+            hqNode.flag = (node->sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT);  // trasfer syncbit to HQ flag field
+            hqNode.quality = (node->sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT;  //remove the last two bits and then make quality from 0-63 to 0-255
+            
+            
+            engine->publishHQNode(engine->getCurrentTimestamp_uS() - _getSampleDelayOffsetInLegacyMode(_cachedTimingDesc), &hqNode);
+            continue;
+
+        }
+        break;
+        }
+        _cached_scan_node_buf[_cached_scan_node_buf_pos++] = current_data;
+    }
+}
+
+
+void UnpackerHandler_NormalNode::onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size)
+{
+    if (type == LIDARSampleDataUnpacker::UNPACKER_CONTEXT_TYPE_LIDAR_TIMING) {
+        assert(size == sizeof(_cachedTimingDesc));
+        _cachedTimingDesc = *reinterpret_cast<const SlamtecLidarTimingDesc*>(data);
+    }
+}
+
+void UnpackerHandler_NormalNode::reset()
+{
+    _cached_scan_node_buf_pos = 0;
+}
+}
+
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/dataunpacker/unpacker/handler_normalnode.h

@@ -0,0 +1,63 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+
+ /*
+  *  Sample Data Unpacker System
+  *  Normal Sample Node Handler
+  */
+
+  /*
+	* Redistribution and use in source and binary forms, with or without
+	* modification, are permitted provided that the following conditions are met:
+	*
+	* 1. Redistributions of source code must retain the above copyright notice,
+	*    this list of conditions and the following disclaimer.
+	*
+	* 2. Redistributions in binary form must reproduce the above copyright notice,
+	*    this list of conditions and the following disclaimer in the documentation
+	*    and/or other materials provided with the distribution.
+	*
+	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+	* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+	* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+	* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+	* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+	* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+	* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+	* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+	* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+	* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+	*
+	*/
+
+#pragma once
+
+BEGIN_DATAUNPACKER_NS()
+	
+namespace unpacker{
+
+class UnpackerHandler_NormalNode : public IDataUnpackerHandler {
+public:
+	UnpackerHandler_NormalNode();
+	virtual ~UnpackerHandler_NormalNode();
+
+	virtual _u8 getSampleAnswerType() const;
+	virtual void onData(LIDARSampleDataUnpackerInner* engine, const _u8* data, size_t size);
+	virtual void reset();
+	virtual void onUnpackerContextSet(LIDARSampleDataUnpacker::UnpackerContextType type, const void* data, size_t size);
+protected:
+	std::vector<_u8> _cached_scan_node_buf;
+	int              _cached_scan_node_buf_pos;
+
+	SlamtecLidarTimingDesc _cachedTimingDesc;
+};
+
+}
+
+END_DATAUNPACKER_NS()

third_party/rplidar_sdk/src/hal/abs_rxtx.h

@@ -0,0 +1,88 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once 
+
+#include "hal/types.h"
+
+namespace rp{ namespace hal{
+
+class serial_rxtx
+{
+public:
+    enum{
+        ANS_OK      = 0,
+        ANS_TIMEOUT = -1,
+        ANS_DEV_ERR = -2,
+    };
+
+    static serial_rxtx * CreateRxTx();
+    static void ReleaseRxTx( serial_rxtx * );
+
+    serial_rxtx():_is_serial_opened(false){}
+    virtual ~serial_rxtx(){}
+
+    virtual void flush( _u32 flags) = 0;
+
+    virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0) = 0;
+    virtual bool open() = 0;
+    virtual void close()  = 0;
+    
+    virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0;
+
+    virtual int senddata(const unsigned char * data, size_t size) = 0;
+    virtual int recvdata(unsigned char * data, size_t size) = 0;
+
+    virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL) = 0;
+    virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL) = 0;
+
+    virtual size_t rxqueue_count() = 0;
+
+    virtual void setDTR() = 0;
+    virtual void clearDTR() = 0;
+    virtual void cancelOperation() {}
+
+    virtual bool isOpened()
+    {
+        return _is_serial_opened;
+    }
+
+protected:
+    volatile bool   _is_serial_opened;
+};
+
+}}
+
+
+

third_party/rplidar_sdk/src/hal/assert.h

@@ -0,0 +1,18 @@
+#ifndef _INFRA_HAL_ASSERT_H
+#define _INFRA_HAL_ASSERT_H
+
+#ifdef WIN32
+#include <crtdbg.h>
+#ifndef assert
+#define assert(x) _ASSERT(x)
+#endif
+#elif defined(_AVR_)
+#define assert(x) 
+#elif defined(__GNUC__)
+#ifndef assert
+#define assert(x) 
+#endif
+#else
+#error assert.h cannot identify your platform
+#endif 
+#endif

third_party/rplidar_sdk/src/hal/byteops.h

@@ -0,0 +1,94 @@
+/*
+ *  RoboPeak Project
+ *  Copyright 2009 - 2013
+ *  
+ *  RPOS - Byte Operations
+ *
+ */
+
+#pragma once
+
+// byte swapping operations for compiling time
+
+#define __static_byteswap_16(x)  ((_u16)(				\
+	(((_u16)(x) & (_u16)0x00FFU) << 8) |			\
+	(((_u16)(x) & (_u16)0xFF00U) >> 8)))
+
+#define __static_byteswap_32(x) ((_u32)(				\
+	(((_u32)(x) & (_u32)0x000000FFUL) << 24) |		\
+	(((_u32)(x) & (_u32)0x0000FF00UL) <<  8) |		\
+	(((_u32)(x) & (_u32)0x00FF0000UL) >>  8) |		\
+	(((_u32)(x) & (_u32)0xFF000000UL) >> 24)))
+
+#define __static_byteswap_64(x) ((_u64)(				\
+	(((_u64)(x) & (_u64)0x00000000000000ffULL) << 56) |	\
+	(((_u64)(x) & (_u64)0x000000000000ff00ULL) << 40) |	\
+	(((_u64)(x) & (_u64)0x0000000000ff0000ULL) << 24) |	\
+	(((_u64)(x) & (_u64)0x00000000ff000000ULL) <<  8) |	\
+	(((_u64)(x) & (_u64)0x000000ff00000000ULL) >>  8) |	\
+	(((_u64)(x) & (_u64)0x0000ff0000000000ULL) >> 24) |	\
+	(((_u64)(x) & (_u64)0x00ff000000000000ULL) >> 40) |	\
+	(((_u64)(x) & (_u64)0xff00000000000000ULL) >> 56)))
+
+
+#define __fast_swap(a, b) do { (a) ^= (b); (b) ^= (a); (a) ^= (b); } while(0)
+
+
+static inline _u16 __byteswap_16(_u16 x)
+{
+#ifdef __arch_byteswap_16
+    return __arch_byteswap_16(x);
+#else
+    return __static_byteswap_16(x);
+#endif
+}
+
+static inline _u32 __byteswap_32(_u32 x)
+{
+#ifdef __arch_byteswap_32
+    return __arch_byteswap_32(x);
+#else
+    return __static_byteswap_32(x);
+#endif
+}
+
+static inline _u64 __byteswap_64(_u64 x)
+{
+#ifdef __arch_byteswap_64
+    return __arch_byteswap_64(x);
+#else
+    return __static_byteswap_64(x);
+#endif
+}
+
+
+#ifdef float
+static inline float __byteswap_float(float x)
+{
+#ifdef __arch_byteswap_float
+    return __arch_byteswap_float(x);
+#else
+    _u8 * raw = (_u8 *)&x;
+    __fast_swap(raw[0], raw[3]);
+    __fast_swap(raw[1], raw[2]);
+    return x;
+#endif
+}
+#endif
+
+
+#ifdef double
+static inline double __byteswap_double(double x)
+{
+#ifdef __arch_byteswap_double
+    return __arch_byteswap_double(x);
+#else
+    _u8 * raw = (_u8 *)&x;
+    __fast_swap(raw[0], raw[7]);
+    __fast_swap(raw[1], raw[6]);
+    __fast_swap(raw[2], raw[5]);
+    __fast_swap(raw[3], raw[4]);
+    return x;
+#endif
+}
+#endif

third_party/rplidar_sdk/src/hal/byteorder.h

@@ -0,0 +1,112 @@
+/*
+ *  RoboPeak Project
+ *  Copyright 2009 - 2013
+ *  
+ *  RPOS - Endianness Helper
+ *
+ */
+
+#pragma once
+
+
+#if !defined(_CPU_ENDIAN_BIG) && !defined(_CPU_ENDIAN_SMALL)
+// CPU Endianness is not specified, assume little endian.
+#define _CPU_ENDIAN_SMALL
+#endif
+
+#if defined(_CPU_ENDIAN_BIG) && defined(_CPU_ENDIAN_SMALL)
+#error "_CPU_ENDIAN_BIG and _CPU_ENDIAN_SMALL cannot be defined at the same time."
+#endif
+
+#include "hal/byteops.h"
+
+#if defined(_CPU_ENDIAN_SMALL)
+
+
+// we don't want to conflict with the Linux kernel...
+#ifndef __KERNEL__ 
+#define constant_cpu_to_le64(x) ((_u64)(x))
+#define constant_le64_to_cpu(x) ((_u64)(x))
+#define constant_cpu_to_le32(x) ((_u32)(x))
+#define constant_le32_to_cpu(x) ((_u32)(x))
+#define constant_cpu_to_le16(x) ((_u16)(x))
+#define constant_le16_to_cpu(x) ((_u16)(x))
+#define constant_cpu_to_be64(x) (__static_byteswap_64((x)))
+#define constant_be64_to_cpu(x) __static_byteswap_64((_u64)(x))
+#define constant_cpu_to_be32(x) (__static_byteswap_32((x)))
+#define constant_be32_to_cpu(x) __static_byteswap_32((_u32)(x))
+#define constant_cpu_to_be16(x) (__static_byteswap_16((x)))
+#define constant_be16_to_cpu(x) __static_byteswap_16((_u16)(x))
+
+#define cpu_to_le64(x) ((_u64)(x))
+#define le64_to_cpu(x) ((_u64)(x))
+#define cpu_to_le32(x) ((_u32)(x))
+#define le32_to_cpu(x) ((_u32)(x))
+#define cpu_to_le16(x) ((_u16)(x))
+#define le16_to_cpu(x) ((_u16)(x))
+#define cpu_to_be64(x) (__byteswap_64((x)))
+#define be64_to_cpu(x) __byteswap_64((_u64)(x))
+#define cpu_to_be32(x) (__byteswap_32((x)))
+#define be32_to_cpu(x) __byteswap_32((_u32)(x))
+#define cpu_to_be16(x) (__byteswap_16((x)))
+#define be16_to_cpu(x) __byteswap_16((_u16)(x))
+#endif
+
+#define cpu_to_float_le(x) ((float)x)
+#define float_le_to_cpu(x) ((float)x)
+
+#define cpu_to_float_be(x) __byteswap_float(x)
+#define float_be_to_cpu(x) __byteswap_float(x)
+
+#define cpu_to_double_le(x) ((double)x)
+#define double_le_to_cpu(x) ((double)x)
+
+#define cpu_to_double_be(x) __byteswap_double(x)
+#define double_be_to_cpu(x) __byteswap_double(x)
+
+#else
+
+// we don't want to conflict with the Linux kernel...
+#ifndef __KERNEL__ 
+#define constant_cpu_to_le64(x) (__static_byteswap_64((x)))
+#define constant_le64_to_cpu(x) __static_byteswap_64((_u64)(x))
+#define constant_cpu_to_le32(x) (__static_byteswap_32((x)))
+#define constant_le32_to_cpu(x) __static_byteswap_32((_u32)(x))
+#define constant_cpu_to_le16(x) (__static_byteswap_16((x)))
+#define constant_le16_to_cpu(x) __static_byteswap_16((_u16)(x))
+#define constant_cpu_to_be64(x) ((_u64)(x))
+#define constant_be64_to_cpu(x) ((_u64)(x))
+#define constant_cpu_to_be32(x) ((_u32)(x))
+#define constant_be32_to_cpu(x) ((_u32)(x))
+#define constant_cpu_to_be16(x) ((_u16)(x))
+#define constant_be16_to_cpu(x) ((_u16)(x))
+
+#define cpu_to_le64(x) (__byteswap_64((x)))
+#define le64_to_cpu(x) __byteswap_64((_u64)(x))
+#define cpu_to_le32(x) (__byteswap_32((x)))
+#define le32_to_cpu(x) __byteswap_32((_u32)(x))
+#define cpu_to_le16(x) (__byteswap_16((x)))
+#define le16_to_cpu(x) __byteswap_16((_u16)(x))
+#define cpu_to_be64(x) ((_u64)(x))
+#define be64_to_cpu(x) ((_u64)(x))
+#define cpu_to_be32(x) ((_u32)(x))
+#define be32_to_cpu(x) ((_u32)(x))
+#define cpu_to_be16(x) ((_u16)(x))
+#define be16_to_cpu(x) ((_u16)(x))
+#endif
+
+
+#define cpu_to_float_le(x) __byteswap_float(x)
+#define float_le_to_cpu(x) __byteswap_float(x)
+
+#define cpu_to_float_be(x) ((float)x)
+#define float_be_to_cpu(x) ((float)x)
+
+
+#define cpu_to_double_le(x) __byteswap_double(x)
+#define double_le_to_cpu(x) __byteswap_double(x)
+
+#define cpu_to_double_be(x) ((double)x)
+#define double_be_to_cpu(x) ((double)x)
+
+#endif

third_party/rplidar_sdk/src/hal/event.h

@@ -0,0 +1,206 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+namespace rp{ namespace hal{
+
+class Event
+{
+public:
+    
+    enum
+    {
+        EVENT_OK = 1,
+        EVENT_TIMEOUT = 0xFFFFFFFF,
+        EVENT_FAILED = 0,
+    };
+    
+    Event(bool isAutoReset = true, bool isSignal = false)
+#ifdef _WIN32
+        : _event(NULL)
+#else
+        : _is_signalled(isSignal)
+        , _isAutoReset(isAutoReset)
+#endif
+    {
+#ifdef _WIN32
+        _event = CreateEvent(NULL, isAutoReset?FALSE:TRUE, isSignal?TRUE:FALSE, NULL); 
+#else
+        pthread_mutex_init(&_cond_locker, NULL);
+        pthread_condattr_init(&_cond_attr);
+#ifdef _MACOS
+        // sadly, there is no monotonic clock support for pthread cond variable on MACOS
+        // if time slew is a big issue, try to reimplement it using kqueue/kevent
+#else
+        pthread_condattr_setclock(&_cond_attr, CLOCK_MONOTONIC);
+#endif
+        pthread_cond_init(&_cond_var, &_cond_attr);
+       
+#endif
+    }
+
+    ~ Event()
+    {
+        release();
+    }
+
+    void set( bool isSignal = true )
+    {
+        if (isSignal){
+#ifdef _WIN32
+            SetEvent(_event);
+#else
+            pthread_mutex_lock(&_cond_locker);
+               
+            if ( _is_signalled == false )
+            {
+                _is_signalled = true;
+                pthread_cond_signal(&_cond_var);
+            }
+            pthread_mutex_unlock(&_cond_locker);
+#endif
+        }
+        else
+        {
+#ifdef _WIN32
+            ResetEvent(_event);
+#else
+            pthread_mutex_lock(&_cond_locker);
+            _is_signalled = false;
+            pthread_mutex_unlock(&_cond_locker);
+#endif
+        }
+    }
+    
+    unsigned long wait( unsigned long timeout = 0xFFFFFFFF )
+    {
+#ifdef _WIN32
+        switch (WaitForSingleObject(_event, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout))
+        {
+        case WAIT_FAILED:
+            return EVENT_FAILED;
+        case WAIT_OBJECT_0:
+            return EVENT_OK;
+        case WAIT_TIMEOUT:
+            return EVENT_TIMEOUT;
+        }
+        return EVENT_OK;
+#else
+        unsigned long ans = EVENT_OK;
+        pthread_mutex_lock( &_cond_locker );
+
+        if ( !_is_signalled )
+        {
+            
+                if (timeout == 0xFFFFFFFF){
+                    pthread_cond_wait(&_cond_var,&_cond_locker);
+                }else
+                {
+                    int timewaitresult = 0;
+#ifdef _MACOS
+                    timespec wait_time;
+                    
+                    wait_time.tv_sec = timeout / 1000;
+                    wait_time.tv_nsec = (timeout%1000)*1000000ULL;
+                
+                    timewaitresult = pthread_cond_timedwait_relative_np(&_cond_var,&_cond_locker,&wait_time);
+#else
+                    timespec wait_time;
+                    clock_gettime(CLOCK_MONOTONIC, &wait_time);
+
+                    wait_time.tv_sec += timeout / 1000;
+                    wait_time.tv_nsec += (timeout%1000)*1000000ULL;
+                
+                    if (wait_time.tv_nsec >= 1000000000)
+                    {
+                       ++wait_time.tv_sec;
+                       wait_time.tv_nsec -= 1000000000;
+                    }
+                    timewaitresult = pthread_cond_timedwait(&_cond_var,&_cond_locker,&wait_time);
+#endif
+
+                    switch (timewaitresult)
+                    {
+                    case 0:
+                        // signalled
+                        break;
+                    case ETIMEDOUT:
+                        // time up
+                        ans = EVENT_TIMEOUT;
+                        goto _final;
+                        break;
+                    default:
+                        ans = EVENT_FAILED;
+                        goto _final;
+                    }
+       
+            }
+        }
+          
+        //assert(_is_signalled);
+
+        if ( _isAutoReset )
+        {
+            _is_signalled = false;
+        }
+_final:
+        pthread_mutex_unlock( &_cond_locker );
+
+        return ans;
+#endif
+        
+    }
+protected:
+
+    void release()
+    {
+#ifdef _WIN32
+        CloseHandle(_event);
+#else
+        pthread_mutex_destroy(&_cond_locker);
+        pthread_cond_destroy(&_cond_var);
+#endif
+    }
+
+#ifdef _WIN32
+        HANDLE _event;
+#else
+        pthread_cond_t         _cond_var;
+        pthread_mutex_t        _cond_locker;
+        pthread_condattr_t     _cond_attr;
+        bool                   _is_signalled;
+        bool                   _isAutoReset;
+#endif
+};
+}}

third_party/rplidar_sdk/src/hal/locker.h

@@ -0,0 +1,205 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+namespace rp{ namespace hal{ 
+
+class Locker
+{
+public:
+    enum LOCK_STATUS
+    {
+        LOCK_OK = 1,
+        LOCK_TIMEOUT = -1,
+        LOCK_FAILED = 0
+    };
+
+    Locker(bool recusive = false){
+#ifdef _WIN32
+        _lock = NULL;
+#endif
+        init(recusive);
+    }
+
+    ~Locker()
+    {
+        release();
+    }
+
+    Locker::LOCK_STATUS lock(unsigned long timeout = 0xFFFFFFFF)
+    {
+#ifdef _WIN32
+        switch (WaitForSingleObject(_lock, timeout==0xFFFFFFF?INFINITE:(DWORD)timeout))
+        {
+        case WAIT_ABANDONED:
+            return LOCK_FAILED;
+        case WAIT_OBJECT_0:
+            return LOCK_OK;
+        case WAIT_TIMEOUT:
+            return LOCK_TIMEOUT;
+        }
+
+#else
+#ifdef _MACOS
+        if (timeout !=0 ) {
+            if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK;
+        }
+#else
+        if (timeout == 0xFFFFFFFF){
+            if (pthread_mutex_lock(&_lock) == 0) return LOCK_OK;
+        }
+#endif
+        else if (timeout == 0)
+        {
+            if (pthread_mutex_trylock(&_lock) == 0) return LOCK_OK;
+        }
+#ifndef _MACOS
+        else
+        {
+            timespec wait_time;
+            timeval now;
+            gettimeofday(&now,NULL);
+
+            wait_time.tv_sec = timeout/1000 + now.tv_sec;
+            wait_time.tv_nsec = (timeout%1000)*1000000 + now.tv_usec*1000;
+        
+            if (wait_time.tv_nsec >= 1000000000)
+            {
+               ++wait_time.tv_sec;
+               wait_time.tv_nsec -= 1000000000;
+            }
+            switch (pthread_mutex_timedlock(&_lock,&wait_time))
+            {
+            case 0:
+                return LOCK_OK;
+            case ETIMEDOUT:
+                return LOCK_TIMEOUT;
+            }
+        }
+#endif
+#endif
+
+        return LOCK_FAILED;
+    }
+
+
+    void unlock()
+    {
+#ifdef _WIN32
+        if (_recusive) {
+            ReleaseMutex(_lock);
+        } else {
+            ReleaseSemaphore(_lock, 1, NULL);
+        }
+#else
+        pthread_mutex_unlock(&_lock);
+#endif
+    }
+
+#ifdef _WIN32
+    HANDLE getLockHandle()
+    {
+        return _lock;
+    }
+#else
+    pthread_mutex_t *getLockHandle()
+    {
+        return &_lock;
+    }
+#endif
+
+
+
+protected:
+    void    init(bool recusive)
+    {
+        
+#ifdef _WIN32
+        if (_recusive = recusive) {
+            _lock = CreateMutex(NULL, FALSE, NULL);
+        } else {
+            _lock = CreateSemaphore(NULL, 1, 1, NULL);
+        }
+#else
+        
+        if (recusive) {
+            pthread_mutexattr_t attr;
+            pthread_mutexattr_init(&attr);
+            pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+            pthread_mutex_init(&_lock, &attr);
+        } else {
+            pthread_mutex_init(&_lock, NULL);
+        }
+#endif
+    }
+
+    void    release()
+    {
+        unlock(); //force unlock before release
+#ifdef _WIN32
+
+        if (_lock) CloseHandle(_lock);
+        _lock = NULL;
+#else
+        pthread_mutex_destroy(&_lock);
+#endif
+    }
+
+#ifdef _WIN32
+    HANDLE  _lock;
+    bool _recusive;
+#else
+    pthread_mutex_t _lock;
+#endif
+    
+};
+
+class AutoLocker
+{
+public :
+    AutoLocker(Locker &l): _binded(l)
+    {
+        _binded.lock();
+    }
+
+    void forceUnlock() {
+        _binded.unlock();
+    }
+    ~AutoLocker() {_binded.unlock();}
+    Locker & _binded;
+};
+
+
+}}
+

third_party/rplidar_sdk/src/hal/socket.h

@@ -0,0 +1,149 @@
+/*
+ *  RoboPeak Project
+ *  HAL Layer - Socket Interface
+ *  Copyright 2009 - 2013 RoboPeak Project
+ */
+
+#pragma once
+
+#include <vector>
+
+namespace rp{ namespace net {
+
+class _single_thread SocketAddress 
+{
+
+public:
+    enum address_type_t {
+        ADDRESS_TYPE_UNSPEC = 0,
+        ADDRESS_TYPE_INET   = 1,
+        ADDRESS_TYPE_INET6  = 2,
+    };
+
+public:
+
+
+
+    SocketAddress();
+    SocketAddress(const char * addrString, int port, address_type_t = ADDRESS_TYPE_INET);
+    // do not use this function, internal usage
+    SocketAddress(void * platform_data);
+    SocketAddress(const SocketAddress &);
+    
+    SocketAddress & operator = (const SocketAddress &);
+
+    virtual ~SocketAddress();
+    
+    virtual int getPort() const;
+    virtual u_result setPort(int port);
+
+    virtual u_result setAddressFromString(const char * address_string,  address_type_t = ADDRESS_TYPE_INET);
+    virtual u_result getAddressAsString(char * buffer, size_t buffersize) const;
+    
+    virtual address_type_t getAddressType() const; 
+
+    virtual u_result getRawAddress(_u8 * buffer, size_t bufferSize) const;
+
+    const void * getPlatformData() const {
+        return _platform_data;
+    }
+
+    virtual void setLoopbackAddress(address_type_t = ADDRESS_TYPE_INET);
+    virtual void setBroadcastAddressIPv4();
+    virtual void setAnyAddress(address_type_t = ADDRESS_TYPE_INET);
+
+public:
+    static size_t LoopUpHostName(const char * hostname, const char * sevicename, std::vector<SocketAddress> &addresspool , bool performDNS = true, address_type_t = ADDRESS_TYPE_INET);
+
+protected:
+    void * _platform_data;
+};
+
+
+
+class SocketBase 
+{
+public:
+    enum socket_family_t {
+        SOCKET_FAMILY_INET  = 0,
+        SOCKET_FAMILY_INET6 = 1,
+        SOCKET_FAMILY_RAW   = 2,
+    };
+
+
+    enum socket_direction_mask {
+        SOCKET_DIR_RD = 0x1,
+        SOCKET_DIR_WR = 0x2,
+        SOCKET_DIR_BOTH = (SOCKET_DIR_RD | SOCKET_DIR_WR),
+    };
+
+    enum {
+        DEFAULT_SOCKET_TIMEOUT = 10000, //10sec
+    };
+
+    virtual ~SocketBase() {}
+    virtual void dispose() = 0;
+    virtual u_result bind(const SocketAddress & ) = 0;
+    
+    virtual u_result getLocalAddress(SocketAddress & ) = 0;
+    virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk = SOCKET_DIR_BOTH) = 0;
+
+    virtual u_result waitforSent(_u32 timeout  = DEFAULT_SOCKET_TIMEOUT) = 0;
+    virtual u_result waitforData(_u32 timeout  = DEFAULT_SOCKET_TIMEOUT)  = 0;
+protected:
+    SocketBase() {} 
+};
+
+
+class _single_thread StreamSocket : public SocketBase
+{
+public:
+
+    enum {
+        MAX_BACKLOG = 128,
+    };
+
+    static StreamSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET);
+    
+    virtual u_result connect(const SocketAddress & pairAddress) = 0;
+    
+    virtual u_result listen(int backlog = MAX_BACKLOG) = 0;
+    virtual StreamSocket * accept(SocketAddress * pairAddress = NULL) = 0;
+    virtual u_result waitforIncomingConnection(_u32 timeout  = DEFAULT_SOCKET_TIMEOUT) = 0;
+
+    virtual u_result send(const void * buffer, size_t len) = 0;
+    
+    virtual u_result recv(void *buf, size_t len, size_t & recv_len) = 0;
+    
+    virtual u_result getPeerAddress(SocketAddress & ) = 0;
+
+    virtual u_result shutdown(socket_direction_mask mask) = 0;
+
+    virtual u_result enableKeepAlive(bool enable = true) = 0;
+    
+    virtual u_result enableNoDelay(bool enable = true) = 0;
+
+protected:
+    virtual ~StreamSocket() {} // use dispose();
+    StreamSocket() {}
+};
+
+class _single_thread DGramSocket: public SocketBase
+{
+
+public:
+
+    static DGramSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET);
+       
+    virtual u_result setPairAddress(const SocketAddress * pairAddress) = 0;
+    virtual u_result sendTo(const SocketAddress * target, const void * buffer, size_t len) = 0;
+    virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr = NULL) = 0;
+    virtual u_result clearRxCache() = 0;
+    
+protected:
+    virtual ~DGramSocket() {} // use dispose();
+
+    DGramSocket() {}
+};
+
+}}

third_party/rplidar_sdk/src/hal/thread.cpp

@@ -0,0 +1,48 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "sdkcommon.h"
+#include "hal/thread.h"
+
+#if defined(_WIN32)
+#include "arch/win32/winthread.hpp"
+#elif defined(_MACOS)
+#include "arch/macOS/thread.hpp"
+#elif defined(__GNUC__)
+#include "arch/linux/thread.hpp"
+#else
+#error no threading implemention found for this platform.
+#endif
+
+

third_party/rplidar_sdk/src/hal/thread.h

@@ -0,0 +1,94 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+#include "hal/types.h"
+#define CLASS_THREAD(c , x ) \
+	rp::hal::Thread::create_member<c, &c::x>(this )
+
+namespace rp{ namespace hal{
+
+class Thread
+{
+public:
+    enum priority_val_t
+	{
+		PRIORITY_REALTIME = 0,
+		PRIORITY_HIGH     = 1,
+		PRIORITY_NORMAL   = 2,
+		PRIORITY_LOW      = 3,
+		PRIORITY_IDLE     = 4,
+	};
+
+    template <class T, u_result (T::*PROC)(void)>
+    static Thread create_member(T * pthis)
+    {
+		return create(_thread_thunk<T,PROC>, pthis);
+	}
+
+	template <class T, u_result (T::*PROC)(void) >
+	static _word_size_t THREAD_PROC _thread_thunk(void * data)
+	{
+		return (static_cast<T *>(data)->*PROC)();
+	}
+	static Thread create(thread_proc_t proc, void * data = NULL );
+
+public:
+    ~Thread() { }
+    Thread():  _data(NULL),_func(NULL),_handle(0)  {}
+    _word_size_t getHandle(){ return _handle;}
+    u_result terminate();
+    void *getData() { return _data;}
+    u_result join(unsigned long timeout = -1);
+	
+    // disabled as on platforms like Linux, the priority will be inherited by the child thread
+    // which may caused unexpected behavior.
+    // Please using Thread::SetSelfPriority instead
+    // u_result setPriority( priority_val_t p);
+	priority_val_t getPriority();
+
+    static u_result SetSelfPriority(priority_val_t p);
+
+
+    bool operator== ( const Thread & right) { return this->_handle == right._handle; }
+protected:
+    Thread( thread_proc_t proc, void * data ): _data(data),_func(proc), _handle(0)  {}
+    void * _data;
+    thread_proc_t _func;
+    _word_size_t _handle;
+};
+
+}}
+

third_party/rplidar_sdk/src/hal/types.h

@@ -0,0 +1,119 @@
+/*
+ *      Common Data Types for RP
+ */
+
+#ifndef _INFRA_HAL_TYPES_H_
+#define _INFRA_HAL_TYPES_H_
+
+//Basic types
+//
+#ifdef WIN32
+
+//fake stdint.h for VC only
+
+typedef signed   char     int8_t;
+typedef unsigned char     uint8_t;
+
+typedef __int16           int16_t;
+typedef unsigned __int16  uint16_t;
+
+typedef __int32           int32_t;
+typedef unsigned __int32  uint32_t;
+
+typedef __int64           int64_t;
+typedef unsigned __int64  uint64_t;
+
+
+#define RPMODULE_EXPORT  __declspec(dllexport)
+#define RPMODULE_IMPORT  __declspec(dllimport)
+
+#else
+
+#include <stdint.h>
+
+#define RPMODULE_EXPORT
+#define RPMODULE_IMPORT
+
+#endif
+
+
+//based on stdint.h
+typedef int8_t         _s8;
+typedef uint8_t        _u8;
+
+typedef int16_t        _s16;
+typedef uint16_t       _u16;
+
+typedef int32_t        _s32;
+typedef uint32_t       _u32;
+
+typedef int64_t        _s64;
+typedef uint64_t       _u64;
+
+#define __small_endian
+
+#ifndef __GNUC__
+#define __attribute__(x)
+#endif
+
+
+// The _word_size_t uses actual data bus width of the current CPU
+#ifdef _AVR_
+typedef _u8            _word_size_t;
+#define THREAD_PROC    
+#elif defined (WIN64)
+typedef _u64           _word_size_t;
+#define THREAD_PROC    __stdcall
+#elif defined (WIN32)
+typedef size_t         _word_size_t;
+#define THREAD_PROC    __stdcall
+#elif defined (__GNUC__)
+typedef unsigned long  _word_size_t;
+#define THREAD_PROC   
+#elif defined (__ICCARM__)
+typedef _u32            _word_size_t;
+#define THREAD_PROC  
+#endif
+
+
+
+//#define __le 
+//#define __be
+
+#define _multi_thread
+#define _single_thread
+
+typedef uint32_t u_result;
+
+#define RESULT_OK                       0
+#define RESULT_FAIL_BIT                 0x80000000
+#define RESULT_ALREADY_DONE             0x20
+#define RESULT_INVALID_DATA             (0x8000 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_FAIL           (0x8001 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_TIMEOUT        (0x8002 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_STOP           (0x8003 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_NOT_SUPPORT    (0x8004 | RESULT_FAIL_BIT)
+#define RESULT_FORMAT_NOT_SUPPORT       (0x8005 | RESULT_FAIL_BIT)
+#define RESULT_INSUFFICIENT_MEMORY      (0x8006 | RESULT_FAIL_BIT)
+#define RESULT_OPERATION_ABORTED        (0x8007 | RESULT_FAIL_BIT)
+#define RESULT_NOT_FOUND                (0x8008 | RESULT_FAIL_BIT)
+#define RESULT_RECONNECTING             (0x8009 | RESULT_FAIL_BIT)
+
+#define IS_OK(x)    ( ((x) & RESULT_FAIL_BIT) == 0 )
+#define IS_FAIL(x)  ( ((x) & RESULT_FAIL_BIT) )
+
+
+typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * );
+
+
+#if defined (_BUILD_AS_DLL)
+#if defined (_BUILD_DLL_EXPORT)
+#define RPMODULE_IMPEXP  RPMODULE_EXPORT
+#else
+#define RPMODULE_IMPEXP  RPMODULE_IMPORT
+#endif
+#else
+#define RPMODULE_IMPEXP 
+#endif
+
+#endif

third_party/rplidar_sdk/src/hal/util.h

@@ -0,0 +1,67 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#pragma once
+
+
+//------
+/* _countof helper */
+#if !defined(_countof)
+#if !defined(__cplusplus)
+#define _countof(_Array) (sizeof(_Array) / sizeof(_Array[0]))
+#else
+extern "C++"
+{
+template <typename _CountofType, size_t _SizeOfArray>
+char (*__countof_helper( _CountofType (&_Array)[_SizeOfArray]))[_SizeOfArray];
+#define _countof(_Array) sizeof(*__countof_helper(_Array))
+}
+#endif
+#endif
+
+/* _offsetof helper */
+#if !defined(offsetof)
+#define offsetof(_structure, _field) ((_word_size_t)&(((_structure *)0x0)->_field))
+#endif
+
+
+#define BEGIN_STATIC_CODE( _blockname_ ) \
+    static class _static_code_##_blockname_ {   \
+    public:     \
+        _static_code_##_blockname_ () 
+
+
+#define END_STATIC_CODE( _blockname_ ) \
+    }   _instance_##_blockname_;
+

third_party/rplidar_sdk/src/hal/waiter.h

@@ -0,0 +1,44 @@
+/*
+ * For synchronize asynchrous operations
+ *
+ * Copyright 2010 Robopeak Team
+ */
+#pragma once
+
+#ifdef _AVR_
+#error there is no implementation for waiter.h on AVR platforms
+#else
+
+#include "hal/event.h"
+
+namespace rp{ namespace hal{
+
+	template<typename ResultT>
+	class Waiter : public Event
+	{
+	public:
+		Waiter() 
+			: Event()
+		{
+		}
+
+		~Waiter() 
+		{}
+
+		ResultT waitForResult()
+		{
+			wait();
+			return result;
+		}
+
+		void setResult(ResultT result)
+		{
+			this->result = result;
+			set();
+		}
+
+		volatile ResultT result;
+	};
+}}
+
+#endif

third_party/rplidar_sdk/src/rplidar_driver.cpp

@@ -0,0 +1,199 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sdkcommon.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+#include "rplidar_driver.h"
+#include "sl_crc.h" 
+#include <algorithm>
+
+namespace rp { namespace standalone{ namespace rplidar {
+
+    RPlidarDriver::RPlidarDriver(){}
+
+    RPlidarDriver::RPlidarDriver(sl_u32 channelType) 
+        :_channelType(channelType)
+    {
+    }
+
+    RPlidarDriver::~RPlidarDriver() {}
+
+    RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype)
+    {
+        //_channelType = drivertype;
+        return  new RPlidarDriver(drivertype);
+    }
+
+    void RPlidarDriver::DisposeDriver(RPlidarDriver * drv)
+    {
+        delete drv;
+    }
+
+    u_result RPlidarDriver::connect(const char *path, _u32 portOrBaud, _u32 flag)
+    {
+        switch (_channelType)
+        {
+        case CHANNEL_TYPE_SERIALPORT:
+            _channel = (*createSerialPortChannel(path, portOrBaud));
+            break;
+        case CHANNEL_TYPE_TCP:
+            _channel = *createTcpChannel(path, portOrBaud);
+            break;
+        case CHANNEL_TYPE_UDP:
+            _channel = *createUdpChannel(path, portOrBaud);
+            break;
+        }
+        if (!(bool)_channel) return SL_RESULT_OPERATION_FAIL;
+        
+        _lidarDrv = *createLidarDriver();
+
+        if (!(bool)_lidarDrv) return SL_RESULT_OPERATION_FAIL;
+
+        sl_result ans =(_lidarDrv)->connect(_channel);
+        return ans;
+    }
+
+    void RPlidarDriver::disconnect()
+    {
+        (_lidarDrv)->disconnect();
+    }
+
+    bool RPlidarDriver::isConnected() 
+    { 
+        return (_lidarDrv)->isConnected();
+    }
+     
+    u_result RPlidarDriver::reset(_u32 timeout)
+    {
+        return (_lidarDrv)->reset();
+    }
+
+    u_result RPlidarDriver::getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs)
+    {
+        return (_lidarDrv)->getAllSupportedScanModes(outModes, timeoutInMs);
+    }
+
+    u_result RPlidarDriver::getTypicalScanMode(_u16& outMode, _u32 timeoutInMs)
+    {
+        return (_lidarDrv)->getTypicalScanMode(outMode, timeoutInMs);
+    }
+
+    u_result RPlidarDriver::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode)
+    {
+        return (_lidarDrv)->startScan(force, useTypicalScan, options, outUsedScanMode);
+    }
+
+    u_result RPlidarDriver::startScanExpress(bool force, _u16 scanMode, _u32 options, RplidarScanMode* outUsedScanMode, _u32 timeout)
+    {
+        return (_lidarDrv)->startScanExpress(force, scanMode, options, outUsedScanMode, timeout);
+    }
+    
+    u_result RPlidarDriver::getHealth(rplidar_response_device_health_t & health, _u32 timeout)
+    {
+        return (_lidarDrv)->getHealth(health, timeout);
+    }
+
+    u_result RPlidarDriver::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout)
+    {
+        return (_lidarDrv)->getDeviceInfo(info, timeout);
+    }
+
+    u_result RPlidarDriver::setMotorPWM(_u16 pwm)
+    {
+        return (_lidarDrv)->setMotorSpeed(pwm);
+    }   
+    
+    u_result RPlidarDriver::checkMotorCtrlSupport(bool & support, _u32 timeout)
+    {
+        MotorCtrlSupport motorSupport;
+        u_result ans = (_lidarDrv)->checkMotorCtrlSupport(motorSupport, timeout);
+        if (motorSupport == MotorCtrlSupportNone)
+            support = false;
+        return ans;
+    }
+
+    u_result RPlidarDriver::setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout)
+	{
+		return (_lidarDrv)->setLidarIpConf(conf, timeout);
+	}
+
+    u_result RPlidarDriver::getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout)
+    {
+        return (_lidarDrv)->getLidarIpConf(conf, timeout);
+    }
+
+    u_result RPlidarDriver::getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs)
+	{
+		return (_lidarDrv)->getDeviceMacAddr(macAddrArray, timeoutInMs);
+	}
+
+    u_result RPlidarDriver::stop(_u32 timeout) 
+    { 
+        return (_lidarDrv)->stop(timeout);
+    }
+
+    u_result RPlidarDriver::grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout)
+    {
+        return (_lidarDrv)->grabScanDataHq(nodebuffer, count, timeout);
+    }
+
+    u_result RPlidarDriver::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count)
+    {
+        return (_lidarDrv)->ascendScanData(nodebuffer, count);
+    }
+    
+    u_result RPlidarDriver::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)
+    {
+        return RESULT_OPERATION_NOT_SUPPORT;
+    }
+
+    u_result RPlidarDriver::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count)
+    {
+        return (_lidarDrv)->getScanDataWithIntervalHq(nodebuffer, count);
+    }
+
+    u_result RPlidarDriver::startMotor()
+    {
+        return (_lidarDrv)->setMotorSpeed(DEFAULT_MOTOR_SPEED);
+    }
+    u_result RPlidarDriver::stopMotor()
+    {
+        return (_lidarDrv)->setMotorSpeed(0);
+    }
+
+}}}

third_party/rplidar_sdk/src/sdkcommon.h

@@ -0,0 +1,51 @@
+/*
+ *  RPLIDAR SDK
+ *
+ *  Copyright (c) 2009 - 2014 RoboPeak Team
+ *  http://www.robopeak.com
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+/*
+ * Redistribution and use in source and binary forms, with or without 
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice, 
+ *    this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice, 
+ *    this list of conditions and the following disclaimer in the documentation 
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, 
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
+ * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#if defined(_WIN32)
+
+#include "arch/win32/arch_win32.h"
+#elif defined(_MACOS)
+#include "arch/macOS/arch_macOS.h"
+#elif defined(__GNUC__)
+#include "arch/linux/arch_linux.h"
+#else
+#error "unsupported target"
+#endif
+
+#include "hal/types.h"
+#include "hal/assert.h"
+
+#include "rplidar.h"
+
+#include "hal/util.h"

third_party/rplidar_sdk/src/sl_async_transceiver.cpp

@@ -0,0 +1,416 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+
+
+#include "sdkcommon.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+
+#include "sl_async_transceiver.h"
+
+
+
+namespace sl { namespace internal {
+
+
+
+
+ProtocolMessage::ProtocolMessage()
+        : len(0)
+        , cmd(0)
+        , data(NULL)
+        , _databufsize(0)
+		, _usingOutterData(false)
+{
+    _changeBufSize();
+}
+
+ProtocolMessage::ProtocolMessage(_u8 cmd, const void* buffer, size_t size)
+	: len(size)
+	, cmd(cmd)
+	, data(NULL)
+    , _databufsize(0)
+	, _usingOutterData(false)
+{
+    _changeBufSize();
+	if (buffer)
+	{
+		memcpy(data, buffer, size);
+	}
+}
+
+ProtocolMessage::ProtocolMessage(const ProtocolMessage& srcMsg)
+	: len(srcMsg.len)
+	, cmd(srcMsg.cmd)
+	, data(NULL)
+    , _databufsize(0)
+	, _usingOutterData(false)
+{
+    _changeBufSize( true );
+	if (srcMsg.data && len)
+	{
+		memcpy(data, srcMsg.data, len);
+	}
+}
+
+ProtocolMessage::~ProtocolMessage()
+{
+	this->cleanData();
+}
+
+ProtocolMessage& ProtocolMessage::operator =(const ProtocolMessage& srcMessage)
+{
+	this->cleanData();
+
+
+	this->len = srcMessage.len;
+	this->cmd = srcMessage.cmd;
+
+    _changeBufSize( true );
+	if (srcMessage.data && len)
+	{
+		memcpy(data, srcMessage.data, len);
+	}
+
+	return *this;
+}
+
+void ProtocolMessage::setDataBuf(_u8 *buffer, size_t size)
+{
+	this->cleanData();
+
+	len = size;
+	data = buffer;
+    _databufsize = size;
+	_usingOutterData = true;
+}
+
+void ProtocolMessage::fillData(const void * buffer, size_t size)
+{
+	len = size;
+    _changeBufSize();
+    if (buffer)
+	    memcpy(data, buffer, size);
+}
+
+void ProtocolMessage::cleanData()
+{
+	if (data) 
+	{
+		if (!_usingOutterData)
+		{
+			delete [] data;
+		}
+		data = NULL;
+		len = 1;
+        _databufsize = 0;
+	}
+}
+
+void ProtocolMessage::_changeBufSize( bool force_compact)
+{
+    size_t actual_size  = getPayloadSize();
+
+    size_t new_buf_size = actual_size;
+
+
+    if (!_usingOutterData)
+    {
+        // nothing to do
+        if ( new_buf_size == _databufsize ) return;
+
+        if ( new_buf_size < _databufsize){
+
+            if ( (_databufsize >> 1) < new_buf_size)
+            {
+                // reuse the current buffer
+                if (!force_compact) return;
+            }else
+            {
+                // the current buffer size is much bigger, we need to release it to save memory
+            }
+        }
+    }
+
+    // we need to change the buffer
+    cleanData();
+    // the cleanData() will reset the length info, so we need to restore it
+    len = actual_size;
+    data = new _u8[new_buf_size];
+    _databufsize = new_buf_size;
+}
+
+
+AsyncTransceiver::AsyncTransceiver(IAsyncProtocolCodec& codec)
+	: _bindedChannel(NULL)
+	, _codec(codec)
+	, _isWorking(false)
+    , _workingFlag(0)
+{
+
+}
+
+AsyncTransceiver::~AsyncTransceiver()
+{
+    unbindAndClose();
+}
+
+u_result AsyncTransceiver::openChannelAndBind(IChannel* channel)
+{
+    if (!channel) return RESULT_INVALID_DATA;
+
+    unbindAndClose();
+	u_result ans = RESULT_OK;
+	do 
+	{
+		rp::hal::AutoLocker l(_opLocker);
+
+        // try to open the channel ...
+        Result<nullptr_t> ans = SL_RESULT_OK;
+
+        if (!channel->open()) {
+            ans= RESULT_OPERATION_FAIL;
+            break;
+        }
+
+
+        // force a flush to clear any pending data
+        channel->flush();
+
+		_dataEvt.set(false);
+
+		_isWorking = true;
+        _workingFlag = 0;
+        _bindedChannel = channel;
+
+
+		_decoderThread = CLASS_THREAD(AsyncTransceiver, _proc_decoderThread);
+		_rxThread = CLASS_THREAD(AsyncTransceiver, _proc_rxThread);
+
+        
+
+
+	} while (0);
+
+	return ans;
+}
+
+void AsyncTransceiver::unbindAndClose()
+{
+	rp::hal::AutoLocker l(_opLocker);
+	if (!_isWorking) return;
+
+    assert(_bindedChannel);
+
+    
+	_isWorking = false;
+	_dataEvt.set(); // set signal to wake up threads
+
+	_decoderThread.join();
+	_rxThread.join();
+
+
+    _bindedChannel->close();
+
+    _bindedChannel = NULL;
+
+
+    for (std::list< Buffer* >::iterator itr = _rxQueue.begin(); itr != _rxQueue.end(); ++itr)
+    {
+        // Fixed vs upstream (`delete [] *itr;`): the queue holds scalar
+        // `new Buffer()` allocations, so array delete is UB — glibc aborts
+        // with "double free or corruption" when a buffer is still queued at
+        // teardown, i.e. whenever the lidar is closed while scanning.
+        delete *itr;
+    }
+    _rxQueue.clear();
+
+}
+
+u_result AsyncTransceiver::sendMessage(message_autoptr_t& msg)
+{
+    assert(msg);
+
+    if (!_isWorking) return RESULT_OPERATION_NOT_SUPPORT;
+
+    rp::hal::AutoLocker l(_opLocker);
+
+    size_t requiredBufferSize = _codec.estimateLength(msg);
+
+    if (requiredBufferSize == 0) {
+        // nothing to send
+        return RESULT_OK;
+    }
+
+    u_result ans = RESULT_OK;
+
+    _u8* txBuffer = new _u8[requiredBufferSize];
+
+    do {
+  
+        if (!txBuffer) {
+            return RESULT_INSUFFICIENT_MEMORY;
+        }
+
+        _codec.onEncodeData(msg, txBuffer, &requiredBufferSize);
+
+        int txSize = _bindedChannel->write(txBuffer, requiredBufferSize);
+
+        if (txSize < 0) ans = RESULT_OPERATION_FAIL;
+
+    } while (0);
+
+
+    delete[] txBuffer;
+    return ans;
+}
+
+sl_result AsyncTransceiver::_proc_rxThread()
+{
+    assert(_bindedChannel);
+
+    rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH);
+
+    u_result result;
+    size_t hintedSize = 0;
+    while (_isWorking)
+    {
+        result = _bindedChannel->waitForDataExt(hintedSize, 1000);
+
+        if (IS_FAIL(result))
+        {
+            // timeout is allowed
+            if (result == RESULT_OPERATION_TIMEOUT) {
+                continue;
+            }
+            if (_isWorking) {
+                _workingFlag |= WORKING_FLAG_ERROR;
+                _codec.onChannelError(result);
+                break;
+            }
+        }
+
+        // no data in buffer, sleep and wait for the next round
+        if (!hintedSize)
+        {
+            continue;
+        }
+
+
+        Buffer* decodeBuffer = new Buffer();
+        
+        decodeBuffer->data = new _u8[hintedSize];
+
+        decodeBuffer->size = _bindedChannel->read(decodeBuffer->data, hintedSize);
+#ifdef _DEBUG_DUMP_PACKET
+        printf("Revc: %d\n", decodeBuffer->size);
+#endif
+         
+        if  (!decodeBuffer->size) {
+            delete decodeBuffer;
+
+            
+            _workingFlag |= WORKING_FLAG_ERROR;
+            _codec.onChannelError(RESULT_OPERATION_ABORTED);
+            break;
+        }
+
+        assert(hintedSize >= decodeBuffer->size);
+
+
+#ifdef _DEBUG_DUMP_PACKET
+        printf("=== Dump RX Packet, size = %d ===\n", decodeBuffer->size);
+        for (int pos = 0; pos < decodeBuffer->size; pos++)
+        {
+            printf("%02x ", decodeBuffer->data[pos]);
+        }
+        printf("\n=== END ===\n");
+#endif
+
+        _rxLocker.lock();
+        _rxQueue.push_back(decodeBuffer);
+        _dataEvt.set();
+        _rxLocker.unlock();
+
+
+    }
+    _workingFlag |= WORKING_FLAG_RX_DISABLED;
+    return RESULT_OK;
+}
+
+sl_result AsyncTransceiver::_proc_decoderThread()
+{
+
+    assert(_bindedChannel);
+    rp::hal::Thread::SetSelfPriority(rp::hal::Thread::PRIORITY_HIGH);
+    _codec.onDecodeReset();
+    
+
+    while (_isWorking)
+    {
+        _rxLocker.lock();
+
+        if (_rxQueue.empty())
+        {
+            _rxLocker.unlock();
+
+            if (_dataEvt.wait(1000))
+                continue;
+
+            _rxLocker.lock();
+        }
+        assert(!_rxQueue.empty());
+
+        Buffer * bufferToDecode = _rxQueue.front();
+        _rxQueue.pop_front();
+
+        _rxLocker.unlock();
+
+        //cout<<"decoding "<< bufferToDecode->size <<" bytes of data"<<endl;
+        _codec.onDecodeData(bufferToDecode->data, bufferToDecode->size);
+
+
+        delete bufferToDecode;
+    }
+
+    return RESULT_OK;
+
+}
+
+
+}}

third_party/rplidar_sdk/src/sl_async_transceiver.h

@@ -0,0 +1,167 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#pragma once
+
+#include <list>
+#include <memory>
+
+namespace sl { namespace internal {
+
+
+class _single_thread ProtocolMessage {
+
+public:
+	size_t len;			
+	_u8 cmd;
+protected:
+	_u8* data;
+	size_t _databufsize;
+
+public:
+	ProtocolMessage();
+	ProtocolMessage(_u8 cmd, const void* buffer, size_t size);
+	ProtocolMessage(const ProtocolMessage& srcMsg);
+	virtual ~ProtocolMessage();
+
+	ProtocolMessage& operator=(const ProtocolMessage& srcMessage);
+
+	// avoid use this method, pls. use fillData instead
+	void setDataBuf(_u8* buffer, size_t size);
+
+	_u8* getDataBuf() { return data; }
+
+	void fillData(const void* buffer, size_t size);
+	void cleanData();
+
+	size_t getPayloadSize() const
+	{
+		return len;
+	}
+
+protected:
+
+	// change the data buffer to fix the new payload size
+	// the existing buffer will be reused if possible.
+	// all the existing payload data will lose
+	void _changeBufSize(bool force_compact = false);
+	bool _usingOutterData;
+};
+
+
+
+typedef std::shared_ptr<ProtocolMessage> message_autoptr_t;
+
+
+class IAsyncProtocolCodec {
+public:
+	IAsyncProtocolCodec() {}
+	virtual ~IAsyncProtocolCodec()  {}
+
+	virtual void   onChannelError(u_result errCode) {}
+
+	virtual void   onDecodeReset() {}
+	virtual void   onDecodeData(const void* buffer, size_t size) = 0;
+
+
+	virtual size_t estimateLength(message_autoptr_t& message) = 0;
+	virtual void   onEncodeData(message_autoptr_t& message, _u8* txbuffer, size_t* size) = 0;
+
+};
+
+class AsyncTransceiver {
+public:
+
+	enum working_flag_t
+	{
+		WORKING_FLAG_RX_DISABLED = 0x1L << 0,
+		WORKING_FLAG_TX_DISABLED = 0x1L << 1,
+
+		WORKING_FLAG_ERROR = 0x1L << 31,
+	};
+
+
+	AsyncTransceiver(IAsyncProtocolCodec& codec);
+	~AsyncTransceiver();
+
+
+
+	u_result openChannelAndBind(IChannel* channel);
+	void     unbindAndClose();
+
+	IChannel* getBindedChannel() const {
+		return _bindedChannel;
+	}
+	
+	u_result sendMessage(message_autoptr_t& msg);
+
+protected:
+
+	sl_result _proc_rxThread();
+	sl_result _proc_decoderThread();
+
+protected:
+
+
+	rp::hal::Locker _opLocker;
+	rp::hal::Locker _rxLocker;
+	rp::hal::Event  _dataEvt;
+
+	IChannel* _bindedChannel;
+	IAsyncProtocolCodec& _codec;
+
+
+	bool _isWorking;
+	_u32 _workingFlag;
+
+	rp::hal::Thread _rxThread;
+	rp::hal::Thread _decoderThread;
+
+	struct Buffer {
+		size_t size;
+		_u8* data;
+
+
+		Buffer() : size(0), data(NULL){}
+
+		~Buffer() {
+			if (data) {
+				delete[] data;
+				data = NULL;
+			}
+		}
+	};
+	std::list< Buffer * > _rxQueue;
+};
+
+
+}}

third_party/rplidar_sdk/src/sl_crc.cpp

@@ -0,0 +1,102 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_crc.h"  
+
+namespace sl {namespace crc32 {
+    
+    static sl_u32 table[256];//crc32_table
+    sl_u32 bitrev(sl_u32 input, sl_u16 bw)
+    {
+        sl_u16 i;
+        sl_u32 var;
+        var = 0;
+        for (i = 0; i < bw; i++) {
+            if (input & 0x01) {
+                var |= 1 << (bw - 1 - i);
+            }
+            input >>= 1;
+        }
+        return var;
+    }
+
+    void init(sl_u32 poly)
+    {
+        sl_u16 i;
+        sl_u16 j;
+        sl_u32 c;
+
+        poly = bitrev(poly, 32);
+        for (i = 0; i < 256; i++) {
+            c = i;
+            for (j = 0; j < 8; j++) {
+                if (c & 1)
+                    c = poly ^ (c >> 1);
+                else
+                    c = c >> 1;
+            }
+            table[i] = c;
+        }
+    }
+
+    sl_u32 cal(sl_u32 crc, void* input, sl_u16 len)
+    {
+        sl_u16 i;
+        sl_u8 index;
+        sl_u8* pch;
+        pch = (unsigned char*)input;
+        sl_u8 leftBytes = 4 - (len & 0x3);
+
+        for (i = 0; i < len; i++) {
+            index = (unsigned char)(crc^*pch);
+            crc = (crc >> 8) ^ table[index];
+            pch++;
+        }
+
+        for (i = 0; i < leftBytes; i++) {//zero padding
+            index = (unsigned char)(crc ^ 0);
+            crc = (crc >> 8) ^ table[index];
+        }
+        return crc ^ 0xffffffff;
+    }
+
+    sl_result getResult(sl_u8 *ptr, sl_u32 len) 
+    {
+        static sl_u8 tmp;
+        if (tmp != 1) {
+            init(0x4C11DB7);
+            tmp = 1;
+        }
+
+        return cal(0xFFFFFFFF, ptr, len);
+    }
+}}

third_party/rplidar_sdk/src/sl_lidarprotocol_codec.cpp

@@ -0,0 +1,238 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+
+
+#include "sdkcommon.h"
+#include "hal/byteorder.h"
+#include "hal/abs_rxtx.h"
+#include "hal/thread.h"
+#include "hal/types.h"
+#include "hal/assert.h"
+#include "hal/locker.h"
+#include "hal/socket.h"
+#include "hal/event.h"
+
+#include "sl_lidar_driver.h"
+#include "sl_crc.h" 
+#include <algorithm>
+
+#include "sl_async_transceiver.h"
+#include "sl_lidarprotocol_codec.h"
+
+
+
+namespace sl { namespace internal {
+
+
+
+RPLidarProtocolCodec::RPLidarProtocolCodec()
+    : IAsyncProtocolCodec()
+    , _listener(NULL)
+    , _op_locker(true)
+{
+    onDecodeReset();
+}
+
+void RPLidarProtocolCodec::exitLoopMode() {
+    onDecodeReset();
+}
+
+
+
+void RPLidarProtocolCodec::setMessageListener(IProtocolMessageListener* listener)
+{
+    rp::hal::AutoLocker l(_op_locker);
+    _listener = listener;
+}
+
+size_t RPLidarProtocolCodec::estimateLength(message_autoptr_t& message)
+{
+    size_t actualSize = 2; //1-byte's sync byte, 1-byte's cmd byte
+
+    if (message->cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
+        actualSize += (message->getPayloadSize() & 0xFF);
+        actualSize += 2; //1-byte for size field, 1-byte for checksum
+    }
+
+    return actualSize;
+}
+
+
+void RPLidarProtocolCodec::onEncodeData(message_autoptr_t& message, _u8* buffer, size_t* size)
+{
+    _u8 checksum = 0;
+    size_t writeSize = std::min<size_t>(*size, estimateLength(message));
+    size_t currentPos = 0;
+
+    while (currentPos < writeSize) {
+        _u8 currentTxByte;
+        switch (currentPos) {
+        case 0: // sync byte
+            currentTxByte = RPLIDAR_CMD_SYNC_BYTE;
+            break;
+        case 1: // cmd byte
+            currentTxByte = message->cmd;
+            break;
+        case 2: // size byte
+            currentTxByte = (_u8)message->getPayloadSize();
+            break;
+        default:
+        {
+            size_t payloadPos = currentPos - 3;
+            if (payloadPos == message->getPayloadSize()) {
+                // checksum byte
+                currentTxByte = checksum;
+                assert(currentPos + 1 == writeSize);
+            }
+            else {
+                // payload
+                currentTxByte = message->getDataBuf()[payloadPos];
+            }
+        }
+        }
+
+
+        checksum ^= currentTxByte;
+        buffer[currentPos++] = currentTxByte;
+    } while (0);
+
+    *size = currentPos;
+}
+
+void   RPLidarProtocolCodec::onDecodeReset() {
+    rp::hal::AutoLocker autolock(_op_locker);
+    // flush the pending data
+    _decodingMessage.cleanData();
+    // reset to initial state
+    _rx_pos = 0;
+    _working_states = STATUS_WAIT_SYNC1;
+}
+
+
+void RPLidarProtocolCodec::onDecodeData(const void* buffer, size_t size)
+{
+    rp::hal::AutoLocker autolock(_op_locker);
+
+    const _u8* data = reinterpret_cast<const _u8*>(buffer);
+    const _u8* dataEnd = data + size;
+
+
+    while (data != dataEnd) {
+        _u8 currentByte = *data;
+        ++data;
+
+        switch (_working_states & ((_u32)STATUS_LOOP_MODE_FLAG - 1)) {
+        case STATUS_WAIT_SYNC1:
+            if (currentByte == RPLIDAR_ANS_SYNC_BYTE1) {
+                _working_states = STATUS_WAIT_SYNC2;
+            }
+            break;
+        case STATUS_WAIT_SYNC2:
+            if (currentByte == RPLIDAR_ANS_SYNC_BYTE2) {
+                _working_states = STATUS_WAIT_SIZE_FLAG;
+                _rx_pos = 0; // init rx pos for recv size and flag
+            }
+            else {
+                // reset to the initial state
+                _working_states = STATUS_WAIT_SYNC1;
+            }
+            break;
+        case STATUS_WAIT_SIZE_FLAG:
+        {
+            assert(sizeof(_decodingMessage.len) >= 4);
+            _u8* byteArr = reinterpret_cast<_u8*>(&_decodingMessage.len);
+            byteArr[_rx_pos++] = currentByte;
+
+            if (_rx_pos == 4) {
+                _working_states = STATUS_WAIT_TYPE;
+                _decodingMessage.len = le32_to_cpu(_decodingMessage.len);
+
+                // 30bit size + 2bit flag has been received
+                _u32 flagbits = (_u32)(_decodingMessage.len >> RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT);
+                if (flagbits & RPLIDAR_ANS_PKTFLAG_LOOP) {
+                    _working_states |= STATUS_LOOP_MODE_FLAG;
+                }
+                _decodingMessage.len = (_decodingMessage.len & RPLIDAR_ANS_HEADER_SIZE_MASK);
+                // alloc buffer
+                _decodingMessage.fillData(NULL, _decodingMessage.getPayloadSize());
+                _rx_pos = 0;
+            }
+        }
+        break;
+        case STATUS_WAIT_TYPE:
+            // save the type field as a cmd 
+            _decodingMessage.cmd = currentByte;
+
+            // recv payload...
+            _working_states = (_working_states & STATUS_LOOP_MODE_FLAG)
+                | STATUS_RECV_PAYLOAD;
+
+            if (!_decodingMessage.getPayloadSize()) {
+                // zero payload packet? 
+                _working_states = STATUS_WAIT_SYNC1;
+            }
+            break;
+        case STATUS_RECV_PAYLOAD:
+            _decodingMessage.getDataBuf()[_rx_pos++] = currentByte;
+
+            if ((size_t)_rx_pos == _decodingMessage.getPayloadSize()) {
+                if (_working_states & STATUS_LOOP_MODE_FLAG) {
+                    // rewind to the payload recv status in loop mode
+                    _rx_pos = 0;
+                }
+                else {
+                    // reset the decoder
+                    _working_states = STATUS_WAIT_SYNC1;
+                }
+
+                IProtocolMessageListener* cachedLister = _listener;
+
+                autolock.forceUnlock(); //unlock the oplock to prevent deadlock
+
+
+                if (cachedLister) {
+                    cachedLister->onProtocolMessageDecoded(_decodingMessage);
+                }
+
+                _op_locker.lock(); // relock it
+            }
+            break;
+        }
+
+    }
+}
+
+
+
+
+}}

third_party/rplidar_sdk/src/sl_lidarprotocol_codec.h

@@ -0,0 +1,88 @@
+/*
+ *  Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2023 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+
+#pragma once
+
+#include "sl_async_transceiver.h"
+
+namespace sl { namespace internal {
+
+
+class IProtocolMessageListener {
+public:
+    virtual void onProtocolMessageDecoded(const ProtocolMessage&) = 0;
+};
+
+
+class RPLidarProtocolCodec : public IAsyncProtocolCodec
+{
+public:
+
+    enum {
+        STATUS_WAIT_SYNC1 = 0x0,
+        STATUS_WAIT_SYNC2 = 0x1,
+        STATUS_WAIT_SIZE_FLAG = 0x2,
+        STATUS_WAIT_TYPE = 0x3,
+        STATUS_RECV_PAYLOAD = 0x4,
+        STATUS_LOOP_MODE_FLAG = 0x80000000,
+    };
+
+    RPLidarProtocolCodec();
+
+    void exitLoopMode();
+
+
+    virtual size_t estimateLength(message_autoptr_t& message);
+
+
+    virtual void onEncodeData(message_autoptr_t& message, _u8* txbuffer, size_t* size);
+
+    virtual void   onDecodeReset();
+    virtual void   onDecodeData(const void* buffer, size_t size);
+    
+    void setMessageListener(IProtocolMessageListener* l);
+
+protected:
+
+    IProtocolMessageListener* _listener;
+    ProtocolMessage          _decodingMessage;
+    rp::hal::Locker          _op_locker;
+                            
+    _u32                     _working_states;
+    int                      _rx_pos;
+};
+
+}}
+
+
+

third_party/rplidar_sdk/src/sl_serial_channel.cpp

@@ -0,0 +1,146 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_lidar_driver.h"
+#include "hal/abs_rxtx.h"
+#include "hal/socket.h"
+
+
+namespace sl {
+    
+    class SerialPortChannel : public ISerialPortChannel
+    {
+    public:
+        SerialPortChannel(const std::string& device, int baudrate) :_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx())
+        {
+            _device = device;
+            _baudrate = baudrate;
+        }
+
+        ~SerialPortChannel()
+        {
+            if (_rxtxSerial)
+                delete _rxtxSerial;
+        }
+
+        bool bind(const std::string& device, sl_s32 baudrate)
+        {
+            _closePending = false;
+            return _rxtxSerial->bind(device.c_str(), baudrate);
+        }
+
+        bool open()
+        {
+            if(!bind(_device, _baudrate))
+                return false;
+            return _rxtxSerial->open();
+        }
+
+        void close()
+        {
+            _closePending = true;
+            _rxtxSerial->cancelOperation();
+            _rxtxSerial->close();
+        }
+        void flush()
+        {
+            _rxtxSerial->flush(0);
+        }
+
+        sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs)
+        {
+            _word_size_t result;
+            size_t size_holder;
+            size_hint = 0;
+
+            if (_closePending) return  RESULT_OPERATION_TIMEOUT;
+
+            if (!_rxtxSerial->isOpened()) {
+                return RESULT_OPERATION_FAIL;
+            }
+
+            result = _rxtxSerial->waitfordata(1, timeoutInMs, &size_holder);
+            size_hint = size_holder;
+            if (result == (_word_size_t)rp::hal::serial_rxtx::ANS_DEV_ERR)
+                return RESULT_OPERATION_FAIL;
+            if (result == (_word_size_t)rp::hal::serial_rxtx::ANS_TIMEOUT)
+                return RESULT_OPERATION_TIMEOUT;
+
+            return RESULT_OK;
+        }
+
+        bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady)
+        {
+            if (_closePending) return false;
+            return (_rxtxSerial->waitfordata(size, timeoutInMs, actualReady) == rp::hal::serial_rxtx::ANS_OK);
+        }
+
+        int write(const void* data, size_t size)
+        {
+           return _rxtxSerial->senddata((const sl_u8 * )data, size);
+        }
+
+        int read(void* buffer, size_t size)
+        {
+            size_t lenRec = 0;
+            lenRec = _rxtxSerial->recvdata((sl_u8 *)buffer, size);
+            return (int)lenRec;
+        }
+
+        void clearReadCache()
+        {
+           
+        }
+
+        void setDTR(bool dtr)
+        {
+            dtr ? _rxtxSerial->setDTR() : _rxtxSerial->clearDTR();
+        }
+
+        int getChannelType() {
+            return CHANNEL_TYPE_SERIALPORT;
+        }
+
+    private:
+        rp::hal::serial_rxtx  * _rxtxSerial;
+        bool _closePending;
+        std::string _device;
+        int _baudrate;
+
+    };
+
+    Result<IChannel*> createSerialPortChannel(const std::string& device, int baudrate)
+    {
+        return new  SerialPortChannel(device, baudrate);
+    }
+
+}

third_party/rplidar_sdk/src/sl_tcp_channel.cpp

@@ -0,0 +1,124 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_lidar_driver.h"
+#include "hal/abs_rxtx.h"
+#include "hal/socket.h"
+
+
+namespace sl {
+    
+    class TcpChannel : public IChannel
+    {
+    public:
+        TcpChannel(const std::string& ip, int port) : _binded_socket(rp::net::StreamSocket::CreateSocket()) {
+            _ip = ip;
+            _port = port;
+        }
+
+        bool bind(const std::string & ip, sl_s32 port)
+        {
+            _socket = rp::net::SocketAddress(ip.c_str(), port);
+            return true;
+        }
+
+        bool open()
+        {
+            if(!bind(_ip, _port))
+                return false;
+            return IS_OK(_binded_socket->connect(_socket));
+            
+        }
+
+        void close()
+        {
+            _binded_socket->dispose();
+            _binded_socket = NULL;
+        }
+        void flush()
+        {
+        
+        }
+
+        sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs)
+        {
+            u_result ans;
+            size_hint = 0;
+            ans = _binded_socket->waitforData(timeoutInMs);
+
+            switch (ans) {
+            case RESULT_OK:
+                size_hint = 1024; //dummy value
+                break;
+            }
+
+            return ans;
+        }
+
+        bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady)
+        {
+            if (actualReady)
+                *actualReady = size;
+            return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK);
+
+        }
+
+        int write(const void* data, size_t size)
+        {
+            return _binded_socket->send(data, size);
+        }
+
+        int read(void* buffer, size_t size)
+        {
+            size_t lenRec = 0;
+            _binded_socket->recv(buffer, size, lenRec);
+            return (int)lenRec;
+        }
+
+        void clearReadCache() {}
+
+        void setStatus(_u32 flag){}
+
+        int getChannelType() {
+            return CHANNEL_TYPE_TCP;
+        }
+    private:
+        rp::net::StreamSocket * _binded_socket;
+        rp::net::SocketAddress _socket;
+        std::string _ip;
+        int _port;
+    };
+    Result<IChannel*> createTcpChannel(const std::string& ip, int port)
+    {
+        return new  TcpChannel(ip, port);
+    }
+}

third_party/rplidar_sdk/src/sl_udp_channel.cpp

@@ -0,0 +1,129 @@
+/*
+ * Slamtec LIDAR SDK
+ *
+ *  Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd.
+ *  http://www.slamtec.com
+ *
+ */
+ /*
+  * Redistribution and use in source and binary forms, with or without
+  * modification, are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  *    this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  *    this list of conditions and the following disclaimer in the documentation
+  *    and/or other materials provided with the distribution.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  *
+  */
+
+#include "sl_lidar_driver.h"
+#include "hal/abs_rxtx.h"
+#include "hal/socket.h"
+
+
+namespace sl {
+	class UdpChannel : public IChannel
+	{
+	public:
+		UdpChannel(const std::string& ip, int port) : _binded_socket(rp::net::DGramSocket::CreateSocket()) {
+            _ip = ip;
+            _port = port;
+        }
+
+		bool bind(const std::string & ip, sl_s32 port)
+        {
+            _socket = rp::net::SocketAddress(ip.c_str(), port);
+            return true;
+        }
+
+        bool open()
+        {
+            if(!bind(_ip, _port))
+                return false;
+            return SL_IS_OK(_binded_socket->setPairAddress(&_socket));         
+        }
+
+        void close()
+        {
+            _binded_socket->dispose();
+            _binded_socket = NULL;
+        }
+        void flush()
+        {
+            clearReadCache();
+        }
+
+        sl_result waitForDataExt(size_t& size_hint, sl_u32 timeoutInMs)
+        {
+            u_result ans;
+            size_hint = 0;
+            ans = _binded_socket->waitforData(timeoutInMs);
+
+            switch (ans) {
+            case RESULT_OK:
+                size_hint = 1024; //dummy value
+                break;
+            }
+
+            return ans;
+        }
+
+        bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady)
+        {
+            if (actualReady)
+                *actualReady = size;
+            return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK);
+
+        }
+
+        int write(const void* data, size_t size)
+        {
+            return _binded_socket->sendTo(nullptr, data, size);
+        }
+
+        int read(void* buffer, size_t size)
+        {
+            size_t actualGet;
+
+            u_result ans = _binded_socket->recvFrom(buffer, size, actualGet);
+            if (IS_FAIL(ans)) return 0;
+            return actualGet;
+        
+        }
+
+        void clearReadCache() {
+            _binded_socket->clearRxCache();
+        }
+
+        void setStatus(_u32 flag){}
+        
+        int getChannelType() {
+            return CHANNEL_TYPE_UDP;
+        }
+
+	private:
+		rp::net::DGramSocket * _binded_socket;
+		rp::net::SocketAddress _socket;
+        std::string _ip;
+        int _port;
+	};
+
+    Result<IChannel*> createUdpChannel(const std::string& ip, int port)
+    {
+        return new  UdpChannel(ip, port);
+    }
+}