diff --git a/app/raspberry_pi/include/rplidar/RPLidar.h b/app/raspberry_pi/include/rplidar/RPLidar.h index 4cc2cb44..75e3d31c 100644 --- a/app/raspberry_pi/include/rplidar/RPLidar.h +++ b/app/raspberry_pi/include/rplidar/RPLidar.h @@ -120,7 +120,7 @@ namespace rplidar { class RPLidar { public: - static tl::expected, nullptr_t> create(const std::string& port, uint32_t baudrate = 115200U); + static tl::expected, nullptr_t> create(const std::string& port, uint32_t baudrate = B115200); RPLidar(const std::string& port, uint32_t baudrate, std::unique_ptr serial); diff --git a/libraries/RPLidar/src/RPLidar.cpp b/libraries/RPLidar/src/RPLidar.cpp index 57268f1f..5ebf53ff 100644 --- a/libraries/RPLidar/src/RPLidar.cpp +++ b/libraries/RPLidar/src/RPLidar.cpp @@ -47,7 +47,7 @@ namespace rplidar { { try { - std::unique_ptr serial = std::make_unique(port, baudrate, serial::Timeout(1000U)); + std::unique_ptr serial = std::make_unique(port, baudrate, serial::Timeout(10U, 10U, 10U, 10U, 10U)); std::unique_ptr lidar = std::make_unique(port, baudrate, std::move(serial)); return std::move(lidar); } @@ -159,12 +159,8 @@ namespace rplidar { */ void RPLidar::_send_cmd(uint8_t cmd) { - std::string req; - req += static_cast(SYNC_BYTE); - req += static_cast(cmd); - - this->_serial->write(req); - spdlog::debug("Command sent: {}", spdlog::to_hex(req)); + uint8_t msg[] = {SYNC_BYTE, cmd}; + this->_serial->write(msg, sizeof(msg)); } /** @@ -175,7 +171,8 @@ namespace rplidar { tl::expected, std::string> RPLidar::_read_descriptor() { // Read descriptor packet - std::vector descriptor(DESCRIPTOR_LEN); + std::array descriptor; + memset(descriptor.data(), '\0', sizeof(descriptor)); this->_serial->read(descriptor.data(), DESCRIPTOR_LEN); spdlog::debug("Received descriptor: {}", spdlog::to_hex(descriptor)); diff --git a/libraries/RPLidar/test/test.cpp b/libraries/RPLidar/test/test.cpp index 0d6e34da..a8fc726a 100644 --- a/libraries/RPLidar/test/test.cpp +++ b/libraries/RPLidar/test/test.cpp @@ -2,55 +2,58 @@ #include #include -int run_native() { - using namespace rplidar; - spdlog::set_level(spdlog::level::debug); +int run_native() +{ + using namespace rplidar; + spdlog::set_level(spdlog::level::debug); - // auto lidar_result = RPLidar::create("/dev/ttyUSB0"); - auto lidar_result = RPLidar::create("COM3"); - if (!lidar_result.has_value()) - { - return 0; - } - auto& lidar = std::move(lidar_result.value()); + auto lidar_result = RPLidar::create("/dev/ttyUSB0"); + // auto lidar_result = RPLidar::create("COM3"); + if (!lidar_result.has_value()) + { + std::cout << "Unable to open lidar\n"; + return 0; + } + auto &lidar = lidar_result.value(); + std::cout << "Connected to lidar\n"; - auto info_result = lidar->get_info(); - if (!info_result.has_value()) - { - std::cout << info_result.error(); - return 0; - } - auto& info = info_result.value(); - std::cout << fmt::format("model: {}, firmware: ({}, {}), hardware: {}, serialnumber: {}\n", info.model, info.firmware.first, info.firmware.second, info.hardware, info.serialNumber); + auto info_result = lidar->get_info(); + if (!info_result.has_value()) + { + std::cout << "Unable to get value of get_info() " << info_result.error(); + return 0; + } + auto &info = info_result.value(); + std::cout << fmt::format("model: {}, firmware: ({}, {}), hardware: {}, serialnumber: {}\n", info.model, info.firmware.first, info.firmware.second, info.hardware, info.serialNumber); - auto health_result = lidar->get_health(); - if (!health_result.has_value()) - { - std::cout << health_result.error(); - return 0; - } - auto& health = health_result.value(); - std::cout << fmt::format("({}, {})\n", health.status, health.errorCode); + auto health_result = lidar->get_health(); + if (!health_result.has_value()) + { + std::cout << "Unable to get value of get_health() "<< health_result.error(); + return 0; + } + auto &health = health_result.value(); + std::cout << fmt::format("({}, {})\n", health.status, health.errorCode); - // std::function()> scanGenerator = lidar->iter_scans(); - // for (int i = 0; i < 10; i++) - // { - // std::vector scan = scanGenerator(); - // std::cout << "Got " << scan.size() << " Measures!\n"; - // for (const Measure &measure : scan) - // { - // // Access individual measurements in the scan - // bool newScan = measure.newScan; - // int quality = measure.quality; - // float angle = measure.angle; - // float distance = measure.distance; - // } - // } + // std::function()> scanGenerator = lidar->iter_scans(); + // for (int i = 0; i < 10; i++) + // { + // std::vector scan = scanGenerator(); + // std::cout << "Got " << scan.size() << " Measures!\n"; + // for (const Measure &measure : scan) + // { + // // Access individual measurements in the scan + // bool newScan = measure.newScan; + // int quality = measure.quality; + // float angle = measure.angle; + // float distance = measure.distance; + // } + // } - lidar->stop(); - lidar->stop_motor(); - lidar->disconnect(); - return 0; + lidar->stop(); + lidar->stop_motor(); + lidar->disconnect(); + return 0; } int main() diff --git a/libraries/RPLidar/xmake.lua b/libraries/RPLidar/xmake.lua index cf58344f..e1ab8aaa 100644 --- a/libraries/RPLidar/xmake.lua +++ b/libraries/RPLidar/xmake.lua @@ -1,4 +1,5 @@ -add_requires("serial", "spdlog") +add_requires("serial") +add_requires("spdlog") add_requires("tl_expected") target("RPLidar") @@ -12,7 +13,7 @@ target("RPLidar") target("test") set_kind("binary") - add_files("test/*.cpp") + add_files("test/test.cpp") add_includedirs("include") add_packages("serial", "spdlog") add_packages("tl_expected")