1. Re-implemented the data retrieving logic based on async fetching a…

…nd decoding mechanism to improve performance 2. RPLIDAR C1 support 3. merge PR#28&#3O

CMakeLists.txt

@@ -24,6 +24,8 @@ FILE(GLOB SLLIDAR_SDK_SRC
   "${SLLIDAR_SDK_PATH}/src/arch/linux/*.cpp"
   "${SLLIDAR_SDK_PATH}/src/hal/*.cpp"
   "${SLLIDAR_SDK_PATH}/src/*.cpp"
+  "${SLLIDAR_SDK_PATH}/src/dataunpacker/*.cpp"
+  "${SLLIDAR_SDK_PATH}/src/dataunpacker/unpacker/*.cpp"
 )
 
 ################################################################################

README.md

@@ -19,6 +19,7 @@ SLAMTEC LIDAR Tutorial: <https://github.com/robopeak/rplidar_ros/wiki>
 |RPLIDAR A1              |
 |RPLIDAR A2              |
 |RPLIDAR A3              |
+|RPLIDAR C1              |
 |RPLIDAR S1              |
 |RPLIDAR S2              |
 |RPLIDAR S3              |
@@ -133,6 +134,12 @@ The command for RPLIDAR A3 is :
 ros2 launch sllidar_ros2 view_sllidar_a3_launch.py
 ```
 
+The command for RPLIDAR C1 is :
+
+```bash
+ros2 launch sllidar_ros2 view_sllidar_c1_launch.py
+```
+
 The command for RPLIDAR S1 is :
 
 ```bash

launch/sllidar_c1_launch.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import LogInfo
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    channel_type =  LaunchConfiguration('channel_type', default='serial')
+    serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
+    serial_baudrate = LaunchConfiguration('serial_baudrate', default='460800')
+    frame_id = LaunchConfiguration('frame_id', default='laser')
+    inverted = LaunchConfiguration('inverted', default='false')
+    angle_compensate = LaunchConfiguration('angle_compensate', default='true')
+    scan_mode = LaunchConfiguration('scan_mode', default='Standard')
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'channel_type',
+            default_value=channel_type,
+            description='Specifying channel type of lidar'),
+
+        DeclareLaunchArgument(
+            'serial_port',
+            default_value=serial_port,
+            description='Specifying usb port to connected lidar'),
+
+        DeclareLaunchArgument(
+            'serial_baudrate',
+            default_value=serial_baudrate,
+            description='Specifying usb port baudrate to connected lidar'),
+
+        DeclareLaunchArgument(
+            'frame_id',
+            default_value=frame_id,
+            description='Specifying frame_id of lidar'),
+
+        DeclareLaunchArgument(
+            'inverted',
+            default_value=inverted,
+            description='Specifying whether or not to invert scan data'),
+
+        DeclareLaunchArgument(
+            'angle_compensate',
+            default_value=angle_compensate,
+            description='Specifying whether or not to enable angle_compensate of scan data'),
+
+        DeclareLaunchArgument(
+            'scan_mode',
+            default_value=scan_mode,
+            description='Specifying scan mode of lidar'),
+
+        Node(
+            package='sllidar_ros2',
+            executable='sllidar_node',
+            name='sllidar_node',
+            parameters=[{'channel_type':channel_type,
+                         'serial_port': serial_port, 
+                         'serial_baudrate': serial_baudrate, 
+                         'frame_id': frame_id,
+                         'inverted': inverted, 
+                         'angle_compensate': angle_compensate, 
+                         'scan_mode': scan_mode}],
+            output='screen'),
+    ])
+

launch/sllidar_s2_launch.py

@@ -11,6 +11,7 @@
 
 
 def generate_launch_description():
+    channel_type =  LaunchConfiguration('channel_type', default='serial')
     serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
     serial_baudrate = LaunchConfiguration('serial_baudrate', default='1000000') #for s2 is 1000000
     frame_id = LaunchConfiguration('frame_id', default='laser')

launch/sllidar_s3_launch.py

@@ -11,6 +11,7 @@
 
 
 def generate_launch_description():
+    channel_type =  LaunchConfiguration('channel_type', default='serial')
     serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
     serial_baudrate = LaunchConfiguration('serial_baudrate', default='1000000')
     frame_id = LaunchConfiguration('frame_id', default='laser')

launch/view_sllidar_c1_launch.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import LogInfo
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    channel_type =  LaunchConfiguration('channel_type', default='serial')
+    serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
+    serial_baudrate = LaunchConfiguration('serial_baudrate', default='460800')
+    frame_id = LaunchConfiguration('frame_id', default='laser')
+    inverted = LaunchConfiguration('inverted', default='false')
+    angle_compensate = LaunchConfiguration('angle_compensate', default='true')
+    scan_mode = LaunchConfiguration('scan_mode', default='Standard')
+
+    rviz_config_dir = os.path.join(
+            get_package_share_directory('sllidar_ros2'),
+            'rviz',
+            'sllidar_ros2.rviz')
+
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'channel_type',
+            default_value=channel_type,
+            description='Specifying channel type of lidar'),
+
+        DeclareLaunchArgument(
+            'serial_port',
+            default_value=serial_port,
+            description='Specifying usb port to connected lidar'),
+
+        DeclareLaunchArgument(
+            'serial_baudrate',
+            default_value=serial_baudrate,
+            description='Specifying usb port baudrate to connected lidar'),
+
+        DeclareLaunchArgument(
+            'frame_id',
+            default_value=frame_id,
+            description='Specifying frame_id of lidar'),
+
+        DeclareLaunchArgument(
+            'inverted',
+            default_value=inverted,
+            description='Specifying whether or not to invert scan data'),
+
+        DeclareLaunchArgument(
+            'angle_compensate',
+            default_value=angle_compensate,
+            description='Specifying whether or not to enable angle_compensate of scan data'),
+
+        DeclareLaunchArgument(
+            'scan_mode',
+            default_value=scan_mode,
+            description='Specifying scan mode of lidar'),
+
+        Node(
+            package='sllidar_ros2',
+            executable='sllidar_node',
+            name='sllidar_node',
+            parameters=[{'channel_type':channel_type,
+                         'serial_port': serial_port, 
+                         'serial_baudrate': serial_baudrate, 
+                         'frame_id': frame_id,
+                         'inverted': inverted, 
+                         'angle_compensate': angle_compensate,
+                           'scan_mode': scan_mode
+                         }],
+            output='screen'),
+
+        Node(
+            package='rviz2',
+            executable='rviz2',
+            name='rviz2',
+            arguments=['-d', rviz_config_dir],
+            output='screen'),
+    ])
+

sdk/Makefile

@@ -42,11 +42,21 @@ CXXSRC += src/sl_lidar_driver.cpp \
           src/hal/thread.cpp\
           src/sl_crc.cpp\
 	      src/sl_serial_channel.cpp\
-	      src/sl_tcp_channel.cpp\
+	      src/sl_lidarprotocol_codec.cpp\
+          src/sl_async_transceiver.cpp\
+          src/sl_tcp_channel.cpp\
 	      src/sl_udp_channel.cpp
 
+
 C_INCLUDES += -I$(CURDIR)/include -I$(CURDIR)/src
 
+
+#dataunpacker
+CXXSRC += $(shell find $(CURDIR)/src/dataunpacker/ -name "*.cpp")
+
+C_INCLUDES += -I$(CURDIR)/src/dataunpacker  -I$(CURDIR)/src/dataunpacker/unpacker
+
+
 ifeq ($(BUILD_TARGET_PLATFORM),Linux)
 CXXSRC += src/arch/linux/net_serial.cpp \
           src/arch/linux/net_socket.cpp \

sdk/include/rplidar_cmd.h

@@ -113,6 +113,8 @@ typedef sl_lidar_payload_new_bps_confirmation_t  rplidar_payload_new_bps_confirm
 #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
@@ -149,7 +151,7 @@ typedef sl_lidar_response_cabin_nodes_t rplidar_response_cabin_nodes_t;
 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

sdk/include/rplidar_protocol.h

@@ -55,6 +55,7 @@
 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

sdk/include/sl_lidar.h

@@ -36,7 +36,12 @@
 #include "sl_lidar_driver.h"
 
 #define SL_LIDAR_SDK_VERSION_MAJOR  2
-#define SL_LIDAR_SDK_VERSION_MINOR  0
+#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        (#SL_LIDAR_SDK_VERSION_MAJOR "." #SL_LIDAR_SDK_VERSION_MINOR "." #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))

sdk/include/sl_lidar_cmd.h

@@ -33,6 +33,11 @@
 
 #pragma once
 
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable:4200)
+#endif
+
 #include "sl_lidar_protocol.h"
 
  // Commands
@@ -101,7 +106,6 @@ typedef struct _sl_lidar_payload_hq_scan_t
 typedef struct _sl_lidar_payload_get_scan_conf_t
 {
     sl_u32  type;
-    sl_u8   reserved[32];
 } __attribute__((packed)) sl_lidar_payload_get_scan_conf_t;
 
 typedef struct _sl_payload_set_scan_conf_t {
@@ -137,21 +141,24 @@ typedef struct _sl_lidar_payload_new_bps_confirmation_t {
 #define SL_LIDAR_ANS_TYPE_DEVINFO          0x4
 #define SL_LIDAR_ANS_TYPE_DEVHEALTH        0x6
 
-#define SL_LIDAR_ANS_TYPE_MEASUREMENT                0x81
+#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
-#define SL_LIDAR_ANS_TYPE_MEASUREMENTT_ULTRA_DENSE_CAPSULED 0x86
+#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.23alpha
-#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA  0x84
+
 //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_MEASUREMENT_DENSE_CAPSULED        0x85
+
+
 #define SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG   0xFF
 
 #define SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK      (0x1)
@@ -300,6 +307,11 @@ typedef struct _sl_lidar_response_hq_capsule_measurement_nodes_t
 #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
@@ -314,6 +326,7 @@ typedef struct _sl_lidar_response_get_lidar_conf
 
 typedef struct _sl_lidar_response_set_lidar_conf
 {
+    sl_u32 type;
     sl_u32 result;
 }__attribute__((packed)) sl_lidar_response_set_lidar_conf_t;
 
@@ -369,3 +382,7 @@ typedef struct  _sl_lidar_response_desired_rot_speed_t{
 #if defined(_WIN32)
 #pragma pack()
 #endif
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#endif