Merge pull request #8 from matthieuvigne/fix_memory_error_getScanData…

…WithIntervalHq

[sdk] Fix memory error in getScanDataWithIntervalHq.

sdk/sdk/include/rplidar_driver.h

@@ -302,13 +302,16 @@ class RPlidarDriver {
     /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. 
     DEPRECATED(virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)) = 0;
 
-    /// Return received scan points even if it's not complete scan
+    /// 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 nodebuffer     Buffer provided by the caller application to store the scan data. This buffer must be initialized by
+    ///                       the caller.
     ///
-    /// \param count          Once the interface returns, this parameter will store the actual received data count.
+    /// \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_TIMEOUT to indicate that not even a single node can be retrieved since last call. 
+    /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call.
+    /// The interface will return RESULT_REMAINING_DATA to indicate that the given buffer is full, but that there remains data to be read.
     virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) = 0;
 
     virtual ~RPlidarDriver() {}

sdk/sdk/src/hal/types.h

@@ -88,6 +88,7 @@ typedef uint32_t u_result;
 #define RESULT_OK                       0
 #define RESULT_FAIL_BIT                 0x80000000
 #define RESULT_ALREADY_DONE             0x20
+#define RESULT_REMAINING_DATA           0x21
 #define RESULT_INVALID_DATA             (0x8000 | RESULT_FAIL_BIT)
 #define RESULT_OPERATION_FAIL           (0x8001 | RESULT_FAIL_BIT)
 #define RESULT_OPERATION_TIMEOUT        (0x8002 | RESULT_FAIL_BIT)

sdk/sdk/src/rplidar_driver.cpp

@@ -1862,19 +1862,27 @@ u_result RPlidarDriverImplCommon::getScanDataWithInterval(rplidar_response_measu
 u_result RPlidarDriverImplCommon::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count)
 {
     size_t size_to_copy = 0;
+    // Prevent crash in case lidar is not scanning - that way this function will leave nodebuffer untouched and set
+    // count to 0.
+    if (_isScanning)
     {
         rp::hal::AutoLocker l(_lock);
         if (_cached_scan_node_hq_count_for_interval_retrieve == 0)
         {
             return RESULT_OPERATION_TIMEOUT;
         }
-        //copy all the nodes(_cached_scan_node_count_for_interval_retrieve nodes) in _cached_scan_node_buf_for_interval_retrieve
-        size_to_copy = _cached_scan_node_hq_count_for_interval_retrieve;
+        // Copy at most count nodes from _cached_scan_node_buf_for_interval_retrieve
+        size_to_copy = min(_cached_scan_node_hq_count_for_interval_retrieve, count);
         memcpy(nodebuffer, _cached_scan_node_hq_buf_for_interval_retrieve, size_to_copy * sizeof(rplidar_response_measurement_node_hq_t));
-        _cached_scan_node_hq_count_for_interval_retrieve = 0;
+        _cached_scan_node_hq_count_for_interval_retrieve -= size_to_copy;
+        // Move remaining data to the start of the array.
+        memmove(&_cached_scan_node_hq_buf_for_interval_retrieve[0], &_cached_scan_node_hq_buf_for_interval_retrieve[size_to_copy], _cached_scan_node_hq_count_for_interval_retrieve *  sizeof(rplidar_response_measurement_node_hq_t));
     }
     count = size_to_copy;
 
+	// If there is remaining data, return with a warning.
+	if (_cached_scan_node_hq_count_for_interval_retrieve > 0)
+		return RESULT_REMAINING_DATA;
     return RESULT_OK;
 }