Add tracy logging, fix status, log memory usage

.gitmodules

@@ -22,3 +22,6 @@
 [submodule "extras/oled_status/u8g8-arm"]
 	path = extras/oled_status/u8g8-arm
 	url = https://github.com/wuhanstudio/u8g2-arm-linux.git
+[submodule "3rd/tracy"]
+	path = 3rd/tracy
+	url = https://github.com/wolfpld/tracy.git

CMakeLists.txt

@@ -25,6 +25,19 @@ message("INCBIN")
 # INCBIN
 include_directories(3rd/incbin)
 
+# Tracy profiler — headers always available (macros are no-ops without TRACY_ENABLE)
+include_directories(3rd/tracy/public)
+set(MANDEYE_USE_TRACY OFF CACHE BOOL "Enable Tracy profiler")
+if(MANDEYE_USE_TRACY)
+    message(STATUS "Tracy profiler enabled")
+    option(TRACY_ENABLE "" ON)
+    option(TRACY_ON_DEMAND "" ON)
+    add_subdirectory(3rd/tracy)
+    set_property(TARGET TracyClient PROPERTY POSITION_INDEPENDENT_CODE ON)
+else()
+    message(STATUS "Tracy profiler disabled")
+endif()
+
 
 include_directories(code/)
 
@@ -155,6 +168,11 @@ set(CMAKE_CXX_LINK_FLAGS "${CMAKE_CXX_LINK_FLAGS} -latomic " )
 
 target_link_libraries(control_program pthread ${LIDAR_LIBRARIES} pistache atomic laszip ${LIBSERIAL_LIBRARY} minea gpiod zmq)
 
+if(MANDEYE_USE_TRACY)
+    target_link_libraries(control_program TracyClient)
+    target_compile_definitions(control_program PRIVATE TRACY_ENABLE)
+endif()
+
 set_target_properties(control_program PROPERTIES
         INSTALL_RPATH "/opt/mandeye/lib"
         BUILD_WITH_INSTALL_RPATH TRUE

code/lidars/BaseLidarClient.h

@@ -68,6 +68,11 @@ class BaseLidarClient : public mandeye_utils::TimeStampProvider
 	//! Move the data from the internal buffers to the caller, preparing new buffers
 	virtual std::pair<LidarPointsBufferPtr, LidarIMUBufferPtr> retrieveData() = 0;
 
+	//! gets a buffer size
+	virtual uint64_t GetBufferSize() const
+	{
+		return 0;
+	}
 	//! Get the current mapping from serial number to lidar id
 	virtual std::unordered_map<uint32_t, std::string> getSerialNumberToLidarIdMapping() const
 	{

code/lidars/hesai/HesaiClient.cpp

@@ -2,9 +2,9 @@
 
 #include <chrono>
 #include <iostream>
+
 namespace mandeye
 {
-
 nlohmann::json HesaiClient::produceStatus()
 {
 	nlohmann::json data;
@@ -45,6 +45,7 @@ nlohmann::json HesaiClient::produceStatus()
 
 	return data;
 }
+
 bool HesaiClient::startListener(const std::string& interfaceIp)
 {
 	std::cout << "HesaiClient: startListener called with interfaceIp: " << interfaceIp << std::endl;
@@ -95,7 +96,6 @@ std::pair<LidarPointsBufferPtr, LidarIMUBufferPtr> HesaiClient::retrieveData()
 
 void HesaiClient::DataThreadFunction()
 {
-
 	std::cout << "HesaiClient: DataThreadFunction started" << std::endl;
 	DriverParam param;
 	param.input_param.source_type = DATA_FROM_LIDAR;
@@ -136,9 +136,9 @@ void HesaiClient::DataThreadFunction()
 	m_lidar->Stop();
 	std::cout << "HesaiClient: DataThreadFunction ended" << std::endl;
 }
+
 void HesaiClient::CallbackFrame(const LidarDecodedFrame<LidarPointXYZICRT>& dataFrame)
 {
-
 	m_recivedPointMessages.fetch_add(1);
 	m_lidar_state = dataFrame.lidar_state;
 	m_work_mode = dataFrame.work_mode;
@@ -176,6 +176,16 @@ void HesaiClient::CallbackFrame(const LidarDecodedFrame<LidarPointXYZICRT>& data
 	}
 }
 
+uint64_t HesaiClient::GetBufferSize() const
+{
+	std::lock_guard<std::mutex> lock(m_bufferPointMutex);
+	if(!m_bufferLidarPtr)
+	{
+		return 0;
+	}
+	return m_bufferLidarPtr->size();
+}
+
 void HesaiClient::CallbackIMU(const LidarImuData& dataFrame)
 {
 	m_recivedIMUMessages.fetch_add(1);
@@ -209,5 +219,4 @@ void HesaiClient::CallbackFault(const FaultMessageInfo& fault_message_info)
 		m_faults.pop_front();
 	}
 }
-
 } // namespace mandeye

code/lidars/hesai/HesaiClient.h

@@ -51,15 +51,17 @@ class HesaiClient : public BaseLidarClient
 		return m_time_diff < 1.0; // laser report time with computer's timestamp
 	}
 
+	uint64_t GetBufferSize() const override;
+
 private:
 	void DataThreadFunction();
 	void CallbackFrame(const LidarDecodedFrame<LidarPointXYZICRT>& dataFrame);
 	void CallbackIMU(const LidarImuData& dataFrame);
 	void CallbackFault(const FaultMessageInfo& fault_message_info);
 
 	// Add any private members or methods if needed
-	std::mutex m_bufferImuMutex;
-	std::mutex m_bufferPointMutex;
+	mutable std::mutex m_bufferImuMutex;
+	mutable std::mutex m_bufferPointMutex;
 	LidarPointsBufferPtr m_bufferLidarPtr;
 	LidarIMUBufferPtr m_bufferIMUPtr;
 	std::thread m_watchThread;

code/main.cpp

@@ -23,6 +23,7 @@
 
 #include "hardware_config/mandeye.h"
 #include <chrono>
+#include <tracy/Tracy.hpp>
 
 #define MANDEYE_LIVOX_LISTEN_IP "192.168.1.5"
 #define MANDEYE_LIDAR_SKD "LIVOX_SDK2"
@@ -57,6 +58,48 @@ mandeye::States app_state{mandeye::States::WAIT_FOR_RESOURCES};
 
 using json = nlohmann::json;
 
+double readCpuTemperature()
+{
+	std::ifstream f("/sys/class/thermal/thermal_zone0/temp");
+	if(!f.is_open())
+		return -1.0;
+	int millideg = 0;
+	f >> millideg;
+	return millideg / 1000.0;
+}
+
+struct MemInfo
+{
+	long total_mb = 0;
+	long available_mb = 0;
+	long swap_total_mb = 0;
+	long swap_free_mb = 0;
+};
+
+MemInfo readMemInfo()
+{
+	MemInfo info;
+	std::ifstream f("/proc/meminfo");
+	if(!f.is_open())
+		return info;
+	std::string key;
+	long value = 0;
+	std::string unit;
+	while(f >> key >> value)
+	{
+		f >> unit; // kB
+		if(key == "MemTotal:")
+			info.total_mb = value / 1024;
+		else if(key == "MemAvailable:")
+			info.available_mb = value / 1024;
+		else if(key == "SwapTotal:")
+			info.swap_total_mb = value / 1024;
+		else if(key == "SwapFree:")
+			info.swap_free_mb = value / 1024;
+	}
+	return info;
+}
+
 std::string produceReport(bool reportUSB = true)
 {
 	json j;
@@ -68,6 +111,14 @@ std::string produceReport(bool reportUSB = true)
 	j["lidar_sdk"] = lidarSDKToUse;
 	j["buzzer"] = !disableBuzzer;
 	j["state"] = StatesToString.at(app_state);
+
+	j["cpu_temp_c"] = readCpuTemperature();
+	const auto mem = readMemInfo();
+	j["mem_total_mb"] = mem.total_mb;
+	j["mem_available_mb"] = mem.available_mb;
+	j["mem_used_mb"] = mem.total_mb - mem.available_mb;
+	j["swap_total_mb"] = mem.swap_total_mb;
+	j["swap_used_mb"] = mem.swap_total_mb - mem.swap_free_mb;
 	if(lidarClientPtr)
 	{
 		j["lidar"] = lidarClientPtr->produceStatus();
@@ -419,7 +470,7 @@ void stateWatcher()
 				mandeye::gpioClientPtr->setLed(hardware::LED::LED_GPIO_CONTINOUS_SCANNING, true);
 				std::this_thread::sleep_for(100ms);
 			}
-			if(now - chunkStart > std::chrono::seconds(10) && app_state == States::SCANNING)
+			if(now - chunkStart > std::chrono::seconds(5) && app_state == States::SCANNING)
 			{
 
 				mandeye::gpioClientPtr->setLed(hardware::LED::LED_GPIO_COPY_DATA, true);
@@ -441,7 +492,9 @@ void stateWatcher()
 				}
 				else
 				{
-					const auto fn = savePointcloudData(lidarBuffer, continousScanDirectory, chunksInExperimentCS + chunksInExperimentSS);
+					auto [fn, saveStats] = savePointcloudData(lidarBuffer, continousScanDirectory, chunksInExperimentCS + chunksInExperimentSS);
+					if(saveStats)
+						lastFileSaveStats = *saveStats;
 					saveImuData(imuBuffer, continousScanDirectory, chunksInExperimentCS + chunksInExperimentSS);
 					saveStatusData(continousScanDirectory, chunksInExperimentCS + chunksInExperimentSS);
 					auto lidarList = lidarClientPtr->getSerialNumberToLidarIdMapping();
@@ -798,6 +851,12 @@ int main(int argc, char** argv)
 		// start zeromq publisher
 		mandeye::publisherPtr = std::make_shared<mandeye::Publisher>();
 		mandeye::publisherPtr->SetTimeStampProvider(mandeye::lidarClientPtr);
+		while(mandeye::isRunning)
+		{
+			std::this_thread::sleep_for(std::chrono::milliseconds(100));
+			auto bufferSize = mandeye::lidarClientPtr->GetBufferSize();
+			TracyPlot("bufferSize", double(bufferSize) / 1e6);
+		}
 	});
 
 	std::thread thStateMachine([&]() { mandeye::stateWatcher(); });

code/save_laz.cpp

@@ -1,6 +1,7 @@
 #include "save_laz.h"
 #include <iostream>
 #include <laszip/laszip_api.h>
+#include <tracy/Tracy.hpp>
 
 nlohmann::json mandeye::LazStats::produceStatus() const
 {
@@ -15,6 +16,9 @@ nlohmann::json mandeye::LazStats::produceStatus() const
 }
 std::optional<mandeye::LazStats> mandeye::saveLaz(const std::string& filename, LidarPointsBufferPtr buffer)
 {
+	ZoneScoped;
+	TracyPlot("laz_buffer_points", (int64_t)buffer->size());
+
 	mandeye::LazStats stats;
 	stats.m_filename = filename;
 	stats.m_pointsCount = buffer->size();
@@ -28,19 +32,22 @@ std::optional<mandeye::LazStats> mandeye::saveLaz(const std::string& filename, L
 	double min_y{std::numeric_limits<double>::max()};
 	double min_z{std::numeric_limits<double>::max()};
 
-	for(auto& p : *buffer)
 	{
-		double x = p.x;
-		double y = p.y;
-		double z = p.z;
+		ZoneScopedN("find_bounds");
+		for(auto& p : *buffer)
+		{
+			double x = p.x;
+			double y = p.y;
+			double z = p.z;
 
-		max_x = std::max(max_x, x);
-		max_y = std::max(max_y, y);
-		max_z = std::max(max_z, z);
+			max_x = std::max(max_x, x);
+			max_y = std::max(max_y, y);
+			max_z = std::max(max_z, z);
 
-		min_x = std::min(min_x, x);
-		min_y = std::min(min_y, y);
-		min_z = std::min(min_z, z);
+			min_x = std::min(min_x, x);
+			min_y = std::min(min_y, y);
+			min_z = std::min(min_z, z);
+		}
 	}
 
 	std::cout << "processing: " << filename << "points " << buffer->size() << std::endl;
@@ -129,29 +136,32 @@ std::optional<mandeye::LazStats> mandeye::saveLaz(const std::string& filename, L
 	laszip_I64 p_count = 0;
 	laszip_F64 coordinates[3];
 
-	//for(int i = 0; i < buffer->size(); i++)
-	for(int i = 0; i < buffer->size(); i += step)
 	{
-
-		const auto& p = buffer->at(i);
-		point->intensity = p.intensity;
-		point->gps_time = p.timestamp * 1e-9;
-		point->classification = p.tag;
-		point->user_data = p.laser_id;
-		p_count++;
-		coordinates[0] = p.x;
-		coordinates[1] = p.y;
-		coordinates[2] = p.z;
-		if(laszip_set_coordinates(laszip_writer, coordinates))
+		ZoneScopedN("write_points");
+		//for(int i = 0; i < buffer->size(); i++)
+		for(int i = 0; i < buffer->size(); i += step)
 		{
-			fprintf(stderr, "DLL ERROR: setting coordinates for point %I64d\n", p_count);
-			return nullopt;
-		}
 
-		if(laszip_write_point(laszip_writer))
-		{
-			fprintf(stderr, "DLL ERROR: writing point %I64d\n", p_count);
-			return nullopt;
+			const auto& p = buffer->at(i);
+			point->intensity = p.intensity;
+			point->gps_time = p.timestamp * 1e-9;
+			point->classification = p.tag;
+			point->user_data = p.laser_id;
+			p_count++;
+			coordinates[0] = p.x;
+			coordinates[1] = p.y;
+			coordinates[2] = p.z;
+			if(laszip_set_coordinates(laszip_writer, coordinates))
+			{
+				fprintf(stderr, "DLL ERROR: setting coordinates for point %I64d\n", p_count);
+				return nullopt;
+			}
+
+			if(laszip_write_point(laszip_writer))
+			{
+				fprintf(stderr, "DLL ERROR: writing point %I64d\n", p_count);
+				return nullopt;
+			}
 		}
 	}
 
@@ -189,7 +199,9 @@ std::optional<mandeye::LazStats> mandeye::saveLaz(const std::string& filename, L
 	{
 		std::uintmax_t size = std::filesystem::file_size(filename);
 		stats.m_sizeMb = static_cast<float>(size) / (1024 * 1024);
+		TracyPlot("laz_file_size_mb", (double)stats.m_sizeMb);
 	}
+	TracyPlot("laz_save_duration_sec", (double)stats.m_saveDurationSec1);
 
 	return stats;
 }