[upgrade]upgrade the SDK to 1.6.0, and support RPLIDAR A3

CHANGELOG.rst

@@ -4,6 +4,14 @@ Changelog for package rplidar_ros
 
 Forthcoming
 -----------
+1.6.0 (2018-05-21)
+------------------
+* Release 1.6.0.
+* Update RPLIDAR SDK to 1.6.0
+* Support new product RPLIDAR A3(default 16K model and max_distance 25m)
+* Contributors: kint
+
+
 1.5.7 (2016-12-15)
 ------------------
 * Release 1.5.7.

README.md

@@ -22,19 +22,24 @@ There're two ways to run rplidar ros package
 
 I. Run rplidar node and view in the rviz
 ------------------------------------------------------------
-roslaunch rplidar_ros view_rplidar.launch
+roslaunch rplidar_ros view_rplidar.launch (for RPLIDAR A1/A2)
+or
+roslaunch rplidar_ros view_rplidar_a3.launch (for RPLIDAR A3)
 
 You should see rplidar's scan result in the rviz.
 
 II. Run rplidar node and view using test application
 ------------------------------------------------------------
-roslaunch rplidar_ros rplidar.launch
+roslaunch rplidar_ros rplidar.launch (for RPLIDAR A1/A2)
+or
+roslaunch rplidar_ros rplidar_a3.launch (for RPLIDAR A3)
 
 rosrun rplidar_ros rplidarNodeClient
 
 You should see rplidar's scan result in the console
 
+Notice: the different is serial_baudrate between A1/A2 and A3
+
 RPLidar frame
 =====================================================================
-RPLidar frame must be broadcasted according to picture shown in
-rplidar-frame.png
+RPLidar frame must be broadcasted according to picture shown in rplidar-frame.png

launch/rplidar.launch

@@ -1,7 +1,8 @@
 <launch>
   <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
-  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
-  <param name="serial_baudrate"     type="int"    value="115200"/>
+  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>
+  <param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
+  <!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 -->
   <param name="frame_id"            type="string" value="laser"/>
   <param name="inverted"            type="bool"   value="false"/>
   <param name="angle_compensate"    type="bool"   value="true"/>

launch/rplidar_a3.launch

@@ -0,0 +1,9 @@
+<launch>
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
+  <param name="serial_baudrate"     type="int"    value="256000"/><!--A3 -->
+  <param name="frame_id"            type="string" value="laser"/>
+  <param name="inverted"            type="bool"   value="false"/>
+  <param name="angle_compensate"    type="bool"   value="true"/>
+  </node>
+</launch>

launch/test_rplidar_a3.launch

@@ -0,0 +1,12 @@
+<launch>
+  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
+  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
+  <param name="serial_baudrate"     type="int"    value="256000"/>
+  <param name="frame_id"            type="string" value="laser"/>
+  <param name="inverted"            type="bool"   value="false"/>
+  <param name="angle_compensate"    type="bool"   value="true"/>
+
+ </node>
+  <node name="rplidarNodeClient"          pkg="rplidar_ros"  type="rplidarNodeClient" output="screen">
+  </node>
+</launch>

launch/view_rplidar_a3.launch

@@ -0,0 +1,10 @@
+<!--
+  Used for visualising rplidar in action.  
+  
+  It requires rplidar.launch.
+ -->
+<launch>
+  <include file="$(find rplidar_ros)/launch/rplidar_a3.launch" />
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rplidar_ros)/rviz/rplidar.rviz" />
+</launch>

package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package>
   <name>rplidar_ros</name>
-  <version>1.5.7</version>
-  <description>The rplidar ros package, support rplidar A2/A1 </description>
+  <version>1.6.0</version>
+  <description>The rplidar ros package, support rplidar A2/A1 and A3</description>
 
   <maintainer email="[email protected]">Slamtec ROS Maintainer</maintainer>
   <license>BSD</license>

sdk/README.txt

@@ -29,6 +29,6 @@ This folder contains RPLIDAR SDK source code which is provided by RoboPeak.
 
 RoboPeak Website: http://www.robopeak.com
 SlamTec HomePage: http://www.slamtec.com
-RPLIDAR_SDK_VERSION:	1.5.7
+RPLIDAR_SDK_VERSION:	1.6.0
 Note: The SDK version may not up-to-date.
 rplidar product: http://www.slamtec.com/en/Lidar

sdk/include/rplidar.h

@@ -34,10 +34,11 @@
 
 #pragma once
 
-#include "rptypes.h"
+#include <vector>
+#include "hal/types.h"
 #include "rplidar_protocol.h"
 #include "rplidar_cmd.h"
 
 #include "rplidar_driver.h"
 
-#define RPLIDAR_SDK_VERSION  "1.5.7"
+#define RPLIDAR_SDK_VERSION  "1.6.0"

sdk/include/rplidar_cmd.h

@@ -55,6 +55,8 @@
 // Commands with payload and have response
 #define RPLIDAR_CMD_EXPRESS_SCAN       0x82 //added in fw 1.17
 
+#define RPLIDAR_CMD_GET_LIDAR_CONF           0x84 //added in fw 1.24
+#define RPLIDAR_CMD_SET_LIDAR_CONF           0x85 //added in fw 1.24
 //add for A2 to set RPLIDAR motor pwm when using accessory board
 #define RPLIDAR_CMD_SET_MOTOR_PWM      0xF0
 #define RPLIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF
@@ -67,12 +69,24 @@
 // Payloads
 // ------------------------------------------
 #define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL      0 
-#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE    1
+#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE    0  // won't been supported but keep to prevent build fail
+//for express working flag(extending express scan protocol)
+#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST                 0x0001 
+#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION    0x0002
+
+//for ultra express working flag
+#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD                 0x0001 
+#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY    0x0002
 typedef struct _rplidar_payload_express_scan_t {
     _u8   working_mode;
-    _u32  reserved;
+    _u16  working_flags;
+    _u16  param;
 } __attribute__((packed)) rplidar_payload_express_scan_t;
 
+typedef struct _rplidar_payload_get_scan_conf_t {
+    _u32  type;
+    _u8   reserved[32];
+} __attribute__((packed)) rplidar_payload_get_scan_conf_t;
 #define MAX_MOTOR_PWM               1023
 #define DEFAULT_MOTOR_PWM           660
 typedef struct _rplidar_payload_motor_pwm_t {
@@ -94,6 +108,11 @@ typedef struct _rplidar_payload_acc_board_flag_t {
 
 // Added in FW ver 1.17
 #define RPLIDAR_ANS_TYPE_SAMPLE_RATE      0x15
+//added in FW ver 1.23alpha
+#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA  0x84
+//added in FW ver 1.24
+#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF     0x20
+#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF     0x21
 
 #define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG   0xFF
 
@@ -145,6 +164,49 @@ typedef struct _rplidar_response_capsule_measurement_nodes_t {
     _u16                            start_angle_sync_q6;
     rplidar_response_cabin_nodes_t  cabins[16];
 } __attribute__((packed)) rplidar_response_capsule_measurement_nodes_t;
+// ext1 : x2 boost mode
+
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS     12
+#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS   10
+
+typedef struct _rplidar_response_ultra_cabin_nodes_t {
+    // 31                                              0
+    // | predict2 10bit | predict1 10bit | major 12bit |
+    _u32 combined_x3;
+} __attribute__((packed)) rplidar_response_ultra_cabin_nodes_t;  
+
+typedef struct _rplidar_response_ultra_capsule_measurement_nodes_t {
+    _u8                             s_checksum_1; // see [s_checksum_1]
+    _u8                             s_checksum_2; // see [s_checksum_1]
+    _u16                            start_angle_sync_q6;
+    rplidar_response_ultra_cabin_nodes_t  ultra_cabins[32];
+} __attribute__((packed)) rplidar_response_ultra_capsule_measurement_nodes_t;
+#   define RPLIDAR_CONF_SCAN_COMMAND_STD            0
+#   define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS        1
+#   define RPLIDAR_CONF_SCAN_COMMAND_HQ             2
+#   define RPLIDAR_CONF_SCAN_COMMAND_BOOST          3
+#   define RPLIDAR_CONF_SCAN_COMMAND_STABILITY      4
+#   define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY    5
+#define RPLIDAR_CONF_MIN_ROT_FREQ                   0x00000004
+#define RPLIDAR_CONF_MAX_ROT_FREQ                   0x00000005
+#define RPLIDAR_CONF_MAX_DISTANCE                   0x00000060
+
+#define RPLIDAR_CONF_SCAN_MODE_COUNT                0x00000070
+#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE        0x00000071
+#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE         0x00000074
+#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE             0x00000075
+#define RPLIDAR_CONF_SCAN_MODE_TYPICAL              0x0000007C
+#define RPLIDAR_CONF_SCAN_MODE_NAME                 0x0000007F
+#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP                 4
+#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP               5
+typedef struct _rplidar_response_get_lidar_conf{
+    _u32 type;
+    _u8  payload[0];
+}__attribute__((packed)) rplidar_response_get_lidar_conf_t;
+typedef struct _rplidar_response_set_lidar_conf{
+    _u32 result;
+}__attribute__((packed)) rplidar_response_set_lidar_conf_t;
+
 
 typedef struct _rplidar_response_device_info_t {
     _u8   model;
@@ -158,6 +220,25 @@ typedef struct _rplidar_response_device_health_t {
     _u16  error_code;
 } __attribute__((packed)) rplidar_response_device_health_t;
 
+// Definition of the variable bit scale encoding mechanism
+#define RPLIDAR_VARBITSCALE_X2_SRC_BIT  9
+#define RPLIDAR_VARBITSCALE_X4_SRC_BIT  11
+#define RPLIDAR_VARBITSCALE_X8_SRC_BIT  12
+#define RPLIDAR_VARBITSCALE_X16_SRC_BIT 14
+
+#define RPLIDAR_VARBITSCALE_X2_DEST_VAL 512
+#define RPLIDAR_VARBITSCALE_X4_DEST_VAL 1280
+#define RPLIDAR_VARBITSCALE_X8_DEST_VAL 1792
+#define RPLIDAR_VARBITSCALE_X16_DEST_VAL 3328
+
+#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \
+    (  (((0x1<<(_BITS_)) - RPLIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \
+       ((RPLIDAR_VARBITSCALE_X16_DEST_VAL - RPLIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \
+       ((RPLIDAR_VARBITSCALE_X8_DEST_VAL - RPLIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \
+       ((RPLIDAR_VARBITSCALE_X4_DEST_VAL - RPLIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \
+       RPLIDAR_VARBITSCALE_X2_DEST_VAL - 1)
+
+
 #if defined(_WIN32)
 #pragma pack()
 #endif

sdk/include/rplidar_driver.h

@@ -41,15 +41,48 @@
 
 namespace rp { namespace standalone{ namespace rplidar {
 
+struct RplidarScanMode {
+    _u16    id;
+    float    us_per_sample;   // microseconds per sample
+    float    max_distance;    // max distance
+    _u8     ans_type;         // the answer type of the scam mode, its value should be RPLIDAR_ANS_TYPE_MEASUREMENT*
+    char    scan_mode[64];    // name of scan mode, max 63 characters
+};
+
+enum {
+    DRIVER_TYPE_SERIALPORT = 0x0,
+    DRIVER_TYPE_TCP = 0x1,
+};
+
+class ChannelDevice
+{
+public:
+    virtual bool bind(const char*, uint32_t ) = 0;
+    virtual bool open() {return true;}
+    virtual void close() = 0;
+    virtual void flush() {return;}
+    virtual bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0;
+    virtual int senddata(const _u8 * data, size_t size) = 0;
+    virtual int recvdata(unsigned char * data, size_t size) = 0;
+    virtual void setDTR() {return;}
+    virtual void clearDTR() {return;}
+    virtual void ReleaseRxTx() {return;}
+};
+
 class RPlidarDriver {
 public:
     enum {
         DEFAULT_TIMEOUT = 2000, //2000 ms
     };
 
     enum {
-        DRIVER_TYPE_SERIALPORT = 0x0,
+        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
@@ -62,7 +95,6 @@ class RPlidarDriver {
     static void DisposeDriver(RPlidarDriver * drv);
 
 
-public:
     /// Open the specified serial port and connect to a target RPLIDAR device
     ///
     /// \param port_path     the device path of the serial port 
@@ -74,7 +106,7 @@ class RPlidarDriver {
     ///
     /// \param flag          other flags
     ///        Reserved for future use, always set to Zero
-    virtual u_result connect(const char * port_path, _u32 baudrate, _u32 flag = 0) = 0;
+    virtual u_result connect(const char *, _u32, _u32 flag = 0) = 0;
 
 
     /// Disconnect with the RPLIDAR and close the serial port
@@ -89,6 +121,25 @@ class RPlidarDriver {
     //  \param timeout       The operation timeout value (in millisecond) for the serial port communication                     
     virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT) = 0;
 
+    // FW1.24
+    /*
+    * @brief Get all scan modes that supported by lidar
+    */
+    virtual u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+    virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+    virtual u_result checkSupportConfigCommands(bool& outSupport, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+    virtual u_result getScanModeCount(_u16& modeCount, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+    virtual u_result getLidarSampleDuration(float& sampleDurationRes, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+    virtual u_result getMaxDistance(float &maxDistance, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+    virtual u_result getScanModeAnsType(_u8 &ansType, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+    virtual u_result getScanModeName(char* modeName, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;
+
+    virtual u_result getLidarConf(_u32 type, std::vector<_u8> &outputBuf, const std::vector<_u8> &reserve = std::vector<_u8>(), _u32 timeout = DEFAULT_TIMEOUT) = 0;
+
+    virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL) = 0;
+    virtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _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.
     ///
@@ -152,10 +203,9 @@ class RPlidarDriver {
     /// \param autoExpressMode Force the core system to trying express mode first, if the system does not support express mode, it will use normal mode.
     ///
     /// \param timeout       The operation timeout value (in millisecond) for the serial port communication.
-    virtual u_result startScan(bool force = false, bool autoExpressMode = true) = 0;
+
     virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT) = 0;
-    virtual u_result startScanExpress(bool fixedAngle, _u32 timeout = DEFAULT_TIMEOUT) = 0;
-
+
     /// Check whether the device support express mode.
     /// 
     /// \param support       Return the result.
@@ -186,7 +236,7 @@ class RPlidarDriver {
     /// 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.
-	virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT) = 0;
+    virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT) = 0;
 
     /// Ascending the scan data according to the angle value in the scan.
     ///
@@ -197,10 +247,24 @@ class RPlidarDriver {
     /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. 
     virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count) = 0;
 
-protected:
-    RPlidarDriver() {}
+
+    /// \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. 
+
+    virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) = 0;
+
     virtual ~RPlidarDriver() {}
+protected:
+    RPlidarDriver(){}
+
+public:
+    ChannelDevice* _chanDev;
 };
 
 
+
+
 }}}

sdk/src/arch/linux/arch_linux.h

@@ -60,5 +60,5 @@
 #include <sys/select.h>
 #include <time.h>
 
-#include "arch/linux/timer.h"
+#include "timer.h"