Implemented code for Slamtec rplidar C1

12 files changed, 1244 insertions, 0 deletions

diff --git a/LIDAR/sdk/src/hal/abs_rxtx.h b/LIDAR/sdk/src/hal/abs_rxtx.h
new file mode 100644
index 0000000..54900e3
--- /dev/null
+++ b/LIDAR/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;
+};
+
+}}
+
+
+
diff --git a/LIDAR/sdk/src/hal/assert.h b/LIDAR/sdk/src/hal/assert.h
new file mode 100644
index 0000000..37cfdd5
--- /dev/null
+++ b/LIDAR/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
diff --git a/LIDAR/sdk/src/hal/byteops.h b/LIDAR/sdk/src/hal/byteops.h
new file mode 100644
index 0000000..c86bde3
--- /dev/null
+++ b/LIDAR/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
diff --git a/LIDAR/sdk/src/hal/byteorder.h b/LIDAR/sdk/src/hal/byteorder.h
new file mode 100644
index 0000000..d06a9af
--- /dev/null
+++ b/LIDAR/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
diff --git a/LIDAR/sdk/src/hal/event.h b/LIDAR/sdk/src/hal/event.h
new file mode 100644
index 0000000..ada15a5
--- /dev/null
+++ b/LIDAR/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
+};
+}}
diff --git a/LIDAR/sdk/src/hal/locker.h b/LIDAR/sdk/src/hal/locker.h
new file mode 100644
index 0000000..2a18e6b
--- /dev/null
+++ b/LIDAR/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;
+};
+
+
+}}
+
diff --git a/LIDAR/sdk/src/hal/socket.h b/LIDAR/sdk/src/hal/socket.h
new file mode 100644
index 0000000..cf4fab0
--- /dev/null
+++ b/LIDAR/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() {}
+};
+
+}}
diff --git a/LIDAR/sdk/src/hal/thread.cpp b/LIDAR/sdk/src/hal/thread.cpp
new file mode 100644
index 0000000..bc68dd5
--- /dev/null
+++ b/LIDAR/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
+
+
diff --git a/LIDAR/sdk/src/hal/thread.h b/LIDAR/sdk/src/hal/thread.h
new file mode 100644
index 0000000..74b6ff7
--- /dev/null
+++ b/LIDAR/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;
+};
+
+}}
+
diff --git a/LIDAR/sdk/src/hal/types.h b/LIDAR/sdk/src/hal/types.h
new file mode 100644
index 0000000..6e23faa
--- /dev/null
+++ b/LIDAR/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
diff --git a/LIDAR/sdk/src/hal/util.h b/LIDAR/sdk/src/hal/util.h
new file mode 100644
index 0000000..205a858
--- /dev/null
+++ b/LIDAR/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_;
+
diff --git a/LIDAR/sdk/src/hal/waiter.h b/LIDAR/sdk/src/hal/waiter.h
new file mode 100644
index 0000000..7f83b9b
--- /dev/null
+++ b/LIDAR/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