From 537891e2b1afdc69ffa638e33204916232812a50 Mon Sep 17 00:00:00 2001 From: Chi Huu Huynh <73843190+Chi-EEE@users.noreply.github.com> Date: Thu, 7 Dec 2023 23:06:10 +0000 Subject: [PATCH] Fix test for RPLidar --- libraries/RPLidar/include/ExpressPacket.hpp | 53 --------------- libraries/RPLidar/include/RPLidar.h | 74 ++++++++++++++++----- libraries/RPLidar/src/RPLidar.cpp | 5 +- libraries/RPLidar/test/test.cpp | 15 +++-- libraries/RPLidar/xmake.lua | 2 +- 5 files changed, 69 insertions(+), 80 deletions(-) delete mode 100644 libraries/RPLidar/include/ExpressPacket.hpp diff --git a/libraries/RPLidar/include/ExpressPacket.hpp b/libraries/RPLidar/include/ExpressPacket.hpp deleted file mode 100644 index f720718f..00000000 --- a/libraries/RPLidar/include/ExpressPacket.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef EXPRESSPACKET_H -#define EXPRESSPACKET_H - -#pragma once - -// Made with the help of ChatGPT - -#include -#include -#include - -class ExpressPacket { -public: - static const uint8_t sync1 = 0xa; - static const uint8_t sync2 = 0x5; - - ExpressPacket(std::vector 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((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 distance; - std::vector angle; - bool new_scan; - float start_angle; -}; - -#endif \ No newline at end of file diff --git a/libraries/RPLidar/include/RPLidar.h b/libraries/RPLidar/include/RPLidar.h index 524e792d..ef0f1305 100644 --- a/libraries/RPLidar/include/RPLidar.h +++ b/libraries/RPLidar/include/RPLidar.h @@ -22,22 +22,23 @@ #include #include +#include +#include + // iter_measures #include #include #include -#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> 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 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 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((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 distance; + std::vector angle; + bool new_scan; + float start_angle; +}; + /** * @brief Class for communicating with RPLidar rangefinder scanners * diff --git a/libraries/RPLidar/src/RPLidar.cpp b/libraries/RPLidar/src/RPLidar.cpp index 4f634677..b24f9d95 100644 --- a/libraries/RPLidar/src/RPLidar.cpp +++ b/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(SYNC_BYTE); - req += static_cast(cmd); - + req.push_back(SYNC_BYTE); + req.push_back(cmd); this->_serial->write(req); spdlog::debug("Command sent: {}", spdlog::to_hex(req)); } diff --git a/libraries/RPLidar/test/test.cpp b/libraries/RPLidar/test/test.cpp index 55ff8dd4..a5a0d152 100644 --- a/libraries/RPLidar/test/test.cpp +++ b/libraries/RPLidar/test/test.cpp @@ -1,16 +1,17 @@ #include +#include int main() { - auto lidar = RPLidar("COM3"); + auto lidar = std::make_unique("/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()> scanGenerator = lidar.iter_scans(); + std::function()> scanGenerator = lidar->iter_scans(); for (int i = 0; i < 10; i++) { std::vector scan = scanGenerator(); @@ -25,8 +26,8 @@ int main() } } - lidar.stop(); - lidar.stop_motor(); - lidar.disconnect(); + lidar->stop(); + lidar->stop_motor(); + lidar->disconnect(); return 0; } \ No newline at end of file diff --git a/libraries/RPLidar/xmake.lua b/libraries/RPLidar/xmake.lua index c4c94cfe..3ba5d61c 100644 --- a/libraries/RPLidar/xmake.lua +++ b/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") \ No newline at end of file