Use 10ms timeout instead of 0ms

Chi-EEE · Dec 10, 2023 · cf1a447 · cf1a447
1 parent 64e97ee
commit cf1a447
Show file tree

Hide file tree

Showing 4 changed files with 56 additions and 55 deletions.
diff --git 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<std::unique_ptr<RPLidar>, nullptr_t> create(const std::string& port, uint32_t baudrate = 115200U);
+		static tl::expected<std::unique_ptr<RPLidar>, nullptr_t> create(const std::string& port, uint32_t baudrate = B115200);
 
 		RPLidar(const std::string& port, uint32_t baudrate, std::unique_ptr<serial::Serial> serial);
 

diff --git a/libraries/RPLidar/src/RPLidar.cpp b/libraries/RPLidar/src/RPLidar.cpp
@@ -47,7 +47,7 @@ namespace rplidar {
 	{
 		try
 		{
-			std::unique_ptr<serial::Serial> serial = std::make_unique<serial::Serial>(port, baudrate, serial::Timeout(1000U));
+			std::unique_ptr<serial::Serial> serial = std::make_unique<serial::Serial>(port, baudrate, serial::Timeout(10U, 10U, 10U, 10U, 10U));
 			std::unique_ptr<RPLidar> lidar = std::make_unique<RPLidar>(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<uint8_t>(SYNC_BYTE);
-		req += static_cast<uint8_t>(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::tuple<uint8_t, bool, uint8_t>, std::string> RPLidar::_read_descriptor()
 	{
 		// Read descriptor packet
-		std::vector<uint8_t> descriptor(DESCRIPTOR_LEN);
+		std::array<uint8_t, DESCRIPTOR_LEN> 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
@@ -2,55 +2,58 @@
 #include <memory>
 #include <spdlog/spdlog.h>
 
-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<std::vector<Measure>()> scanGenerator = lidar->iter_scans();
-   // for (int i = 0; i < 10; i++)
-   // {
-   //     std::vector<Measure> 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<std::vector<Measure>()> scanGenerator = lidar->iter_scans();
+    // for (int i = 0; i < 10; i++)
+    // {
+    //     std::vector<Measure> 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
@@ -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")