From 74d6d8fcb0c547787da01478c79f04fd8d4ea40e Mon Sep 17 00:00:00 2001
From: Chi Huu Huynh <73843190+Chi-EEE@users.noreply.github.com>
Date: Fri, 5 Jan 2024 15:54:30 +0000
Subject: [PATCH] Improve scanner creation

---
 .../src/car/system/lidar/LidarScanner.cxx        | 16 ++++++++++++++--
 app/raspberry_pi/src/main.cpp                    | 14 +++++++++-----
 2 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/app/raspberry_pi/src/car/system/lidar/LidarScanner.cxx b/app/raspberry_pi/src/car/system/lidar/LidarScanner.cxx
index ba289b56..cc812a8e 100644
--- a/app/raspberry_pi/src/car/system/lidar/LidarScanner.cxx
+++ b/app/raspberry_pi/src/car/system/lidar/LidarScanner.cxx
@@ -9,6 +9,7 @@
 
 #include <RPLidar.h>
 #include <spdlog/spdlog.h>
+#include <tl/expected.hpp>
 
 using namespace rplidar;
 
@@ -17,9 +18,20 @@ namespace car::system::lidar
 	class LidarScanner : public LidarDevice
 	{
 	public:
-		LidarScanner(const std::string &lidar_port) : lidar(RPLidar::create(lidar_port).value())
+		static tl::expected<std::unique_ptr<LidarScanner>, nullptr_t> create(const std::string &lidar_port)
 		{
-		};
+			auto maybe_lidar = RPLidar::create(lidar_port);
+			if (maybe_lidar.has_value())
+			{
+				return std::make_unique<LidarScanner>(std::move(maybe_lidar.value()));
+			}
+			else
+			{
+				return tl::make_unexpected(nullptr);
+			}
+		}
+		
+		LidarScanner(std::unique_ptr<RPLidar> lidar) : lidar(std::move(lidar)){};
 
 		~LidarScanner(){};
 
diff --git a/app/raspberry_pi/src/main.cpp b/app/raspberry_pi/src/main.cpp
index 56fc1b7c..2086be10 100644
--- a/app/raspberry_pi/src/main.cpp
+++ b/app/raspberry_pi/src/main.cpp
@@ -36,12 +36,17 @@ int main()
 	using namespace rplidar;
 
 	// spdlog::set_level(spdlog::level::off);
-	
+
 	std::string websocket_url = getWebSocketUrl();
 	spdlog::info("Got websocket url: {}", websocket_url);
 
-	//std::unique_ptr<LidarDummy> scanner = std::make_unique<LidarDummy>();
-	std::unique_ptr<LidarScanner> scanner = std::make_unique<LidarScanner>(GET_CONFIG_VALUE(lidar_port));
+	std::unique_ptr<LidarDummy> scanner = std::make_unique<LidarDummy>();
+	// auto maybe_scanner = LidarScanner::create(GET_CONFIG_VALUE(lidar_port));
+	// if (!maybe_scanner.has_value())
+	// {
+	// 	spdlog::error("Unable to connect to the Lidar Scanner");
+	// }
+	// std::unique_ptr<LidarScanner> &scanner = maybe_scanner.value();
 
 	std::unique_ptr<MessagingSystem> messaging_system = std::make_unique<MessagingSystem>(websocket_url);
 
@@ -51,8 +56,7 @@ int main()
 		websocket_url,
 		std::move(scanner),
 		std::move(messaging_system),
-		std::move(movement_system)
-	);
+		std::move(movement_system));
 
 	// The CarConsole object will display the UI and handle user input:
 	CarConsole car_console(std::move(car_system));