Fix test for RPLidar

libraries/RPLidar/include/RPLidar.h

@@ -22,22 +22,23 @@
 #include <sstream>
 #include <iomanip>
 
+#include <tuple>
+#include <stdexcept>
+
 // iter_measures
 #include <functional>
 
 #include <spdlog/spdlog.h>
 #include <spdlog/fmt/bin_to_hex.h>
 
-#include "ExpressPacket.hpp"
-
-#define SYNC_BYTE 0xA5
-#define SYNC_BYTE2 0x5A
+constexpr uint8_t SYNC_BYTE = 0xA5;
+constexpr uint8_t SYNC_BYTE2 = 0x5A;
 
-#define GET_INFO_BYTE 0x50
-#define GET_HEALTH_BYTE 0x52
+constexpr uint8_t GET_INFO_BYTE = 0x50;
+constexpr uint8_t GET_HEALTH_BYTE = 0x52;
 
-#define STOP_BYTE 0x25
-#define RESET_BYTE 0x40
+constexpr uint8_t STOP_BYTE = 0x25;
+constexpr uint8_t RESET_BYTE = 0x40;
 
 enum ScanType
 {
@@ -58,16 +59,16 @@ static std::map<ScanType, std::map<std::string, uint8_t>> SCAN_TYPE = {
     {ScanType::FORCE, {{"byte", 0x21}, {"response", 129}, {"size", 5}}},
     {ScanType::EXPRESS, {{"byte", 0x82}, {"response", 130}, {"size", 84}}}};
 
-#define DESCRIPTOR_LEN 7
-#define INFO_LEN 20
-#define HEALTH_LEN 3
+constexpr int DESCRIPTOR_LEN = 7;
+constexpr int INFO_LEN = 20;
+constexpr int HEALTH_LEN = 3;
 
-#define INFO_TYPE 4
-#define HEALTH_TYPE 6
+constexpr int INFO_TYPE = 4;
+constexpr int HEALTH_TYPE = 6;
 
-#define MAX_MOTOR_PWM 1023
-#define DEFAULT_MOTOR_PWM 660
-#define SET_PWM_BYTE 0xF0
+constexpr int MAX_MOTOR_PWM = 1023;
+constexpr int DEFAULT_MOTOR_PWM = 660;
+constexpr uint8_t SET_PWM_BYTE = 0xF0;
 
 static std::map<int, std::string> HEALTH_STATUSES = {
     {0, "Good"},
@@ -108,6 +109,47 @@ struct Measure
     double distance;
 };
 
+class ExpressPacket {
+public:
+    static const uint8_t sync1 = 0xa;
+    static const uint8_t sync2 = 0x5;
+
+    ExpressPacket(std::vector<uint8_t> data) {
+        if ((data[0] >> 4) != sync1 || (data[1] >> 4) != sync2) {
+            throw std::invalid_argument("try to parse corrupted data");
+        }
+
+        uint8_t checksum = 0;
+        for (size_t i = 2; i < data.size(); i++) {
+            checksum ^= data[i];
+        }
+
+        if (checksum != ((data[0] & 0x0F) + ((data[1] & 0x0F) << 4))) {
+            throw std::invalid_argument("Invalid checksum");
+        }
+
+        new_scan = (data[2] >> 7) & 0x01;
+        start_angle = static_cast<float>((data[1] & 0x0F) << 8 | data[2]) / 64.0f;
+
+        for (size_t i = 4; i < data.size(); i += 5) {
+            distance.push_back(((data[i + 1] >> 2) & 0x3F) | ((data[i] & 0x3F) << 6));
+            angle.push_back((((data[i + 3] & 0x0F) + ((data[i + 1] & 0x01) << 4)) / 8.0f) * getSign(data[i + 1]));
+            distance.push_back(((data[i + 2] >> 2) & 0x3F) | ((data[i + 1] & 0x3F) << 6));
+            angle.push_back((((data[i + 3] >> 4) & 0x0F) + ((data[i + 2] & 0x01) << 4)) / 8.0f * getSign(data[i + 2]));
+        }
+    }
+
+    static int getSign(uint8_t value) {
+        return (value & 0x02) ? -1 : 1;
+    }
+
+public:
+    std::vector<uint16_t> distance;
+    std::vector<float> angle;
+    bool new_scan;
+    float start_angle;
+};
+
 /**
  * @brief Class for communicating with RPLidar rangefinder scanners
  * 

libraries/RPLidar/src/RPLidar.cpp

@@ -155,9 +155,8 @@ void RPLidar::_send_payload_cmd(uint8_t cmd, const std::string &payload)
 void RPLidar::_send_cmd(uint8_t cmd)
 {
     std::string req;
-    req += static_cast<char>(SYNC_BYTE);
-    req += static_cast<char>(cmd);
-
+    req.push_back(SYNC_BYTE);
+    req.push_back(cmd);
     this->_serial->write(req);
     spdlog::debug("Command sent: {}", spdlog::to_hex(req));
 }

libraries/RPLidar/test/test.cpp

@@ -1,16 +1,17 @@
 #include <RPLidar.h>
+#include <memory>
 
 int main()
 {
-    auto lidar = RPLidar("COM3");
+    auto lidar = std::make_unique<RPLidar>("/dev/ttyUSB0");
 
-    auto info = lidar.get_info();
+    auto info = lidar->get_info();
     std::cout << fmt::format("model: {}, firmware: ({}, {}), hardware: {}, serialnumber: {}\n", info.model, info.firmware.first, info.firmware.second, info.hardware, info.serialNumber);
 
-    auto health = lidar.get_health();
+    auto health = lidar->get_health();
     std::cout << fmt::format("({}, {})\n", health.status, health.errorCode);
 
-    std::function<std::vector<Measure>()> scanGenerator = lidar.iter_scans();
+    std::function<std::vector<Measure>()> scanGenerator = lidar->iter_scans();
     for (int i = 0; i < 10; i++)
     {
         std::vector<Measure> scan = scanGenerator();
@@ -25,8 +26,8 @@ int main()
         }
     }
 
-    lidar.stop();
-    lidar.stop_motor();
-    lidar.disconnect();
+    lidar->stop();
+    lidar->stop_motor();
+    lidar->disconnect();
     return 0;
 }

libraries/RPLidar/xmake.lua

@@ -12,5 +12,5 @@ target("test")
     set_kind("binary")
     add_files("test/*.cpp")
     add_includedirs("include")
+    add_packages("serial", "spdlog")
     add_deps("RPLidar")
-    add_links("RPLidar")