From 7e81a6ffd10e7d298645c70190e357f934425387 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Thu, 11 Jun 2026 23:50:22 +0200 Subject: [PATCH 1/4] Added tracy, logging mem usage Signed-off-by: Michal Pelka --- .gitmodules | 3 +++ 3rd/tracy | 1 + 2 files changed, 4 insertions(+) create mode 160000 3rd/tracy diff --git a/.gitmodules b/.gitmodules index b0d3451..bb1d88b 100644 --- a/.gitmodules +++ b/.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 diff --git a/3rd/tracy b/3rd/tracy new file mode 160000 index 0000000..05cceee --- /dev/null +++ b/3rd/tracy @@ -0,0 +1 @@ +Subproject commit 05cceee0df3b8d7c6fa87e9638af311dbabc63cb From 8324a1ec0963169f0bb3dfe1919cc0425f655dc6 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 12 Jun 2026 00:21:34 +0200 Subject: [PATCH 2/4] Add Tracy profiler, system stats to status, fix save_duration_sec1 bug - Add Tracy v0.13.1 as submodule (3rd/tracy), off by default (MANDEYE_USE_TRACY) - Tracy headers always available so macros are no-ops without TRACY_ENABLE - Add ZoneScoped/TracyPlot to saveLaz (find_bounds, write_points, buffer/file size) - Add TracyPlot for lidar buffer size in HesaiClient and BaseLidarClient - Add cpu_temp_c, mem_used_mb, mem_total_mb, swap_used_mb to /json/status - Fix: savePointcloudData result discarded in continuous scan path (save_duration_sec1 always -1) Co-Authored-By: Claude Sonnet 4.6 --- CMakeLists.txt | 18 ++++++++ code/lidars/BaseLidarClient.h | 5 +++ code/lidars/hesai/HesaiClient.cpp | 9 ++++ code/lidars/hesai/HesaiClient.h | 6 ++- code/main.cpp | 61 +++++++++++++++++++++++++- code/save_laz.cpp | 72 ++++++++++++++++++------------- 6 files changed, 137 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f1060c6..2132d5f 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/code/lidars/BaseLidarClient.h b/code/lidars/BaseLidarClient.h index 4a77397..a71365b 100644 --- a/code/lidars/BaseLidarClient.h +++ b/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 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 getSerialNumberToLidarIdMapping() const { diff --git a/code/lidars/hesai/HesaiClient.cpp b/code/lidars/hesai/HesaiClient.cpp index 6cdc6b5..824fc0f 100644 --- a/code/lidars/hesai/HesaiClient.cpp +++ b/code/lidars/hesai/HesaiClient.cpp @@ -176,6 +176,15 @@ void HesaiClient::CallbackFrame(const LidarDecodedFrame& data } } +uint64_t HesaiClient::GetBufferSize() const { + std::lock_guard lock(m_bufferPointMutex); + if (!m_bufferLidarPtr) { + return 0; + } + return m_bufferLidarPtr->size(); +} + + void HesaiClient::CallbackIMU(const LidarImuData& dataFrame) { m_recivedIMUMessages.fetch_add(1); diff --git a/code/lidars/hesai/HesaiClient.h b/code/lidars/hesai/HesaiClient.h index 74ddac8..552492d 100644 --- a/code/lidars/hesai/HesaiClient.h +++ b/code/lidars/hesai/HesaiClient.h @@ -51,6 +51,8 @@ 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& dataFrame); @@ -58,8 +60,8 @@ class HesaiClient : public BaseLidarClient 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; diff --git a/code/main.cpp b/code/main.cpp index 9eb37aa..63545cb 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -57,6 +57,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 +110,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 +469,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 +491,8 @@ 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 +849,12 @@ int main(int argc, char** argv) // start zeromq publisher mandeye::publisherPtr = std::make_shared(); 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(); }); diff --git a/code/save_laz.cpp b/code/save_laz.cpp index 7a999ae..10cca73 100644 --- a/code/save_laz.cpp +++ b/code/save_laz.cpp @@ -1,6 +1,7 @@ #include "save_laz.h" #include #include +#include nlohmann::json mandeye::LazStats::produceStatus() const { @@ -15,6 +16,9 @@ nlohmann::json mandeye::LazStats::produceStatus() const } std::optional 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::saveLaz(const std::string& filename, L double min_y{std::numeric_limits::max()}; double min_z{std::numeric_limits::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::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::saveLaz(const std::string& filename, L { std::uintmax_t size = std::filesystem::file_size(filename); stats.m_sizeMb = static_cast(size) / (1024 * 1024); + TracyPlot("laz_file_size_mb", (double)stats.m_sizeMb); } + TracyPlot("laz_save_duration_sec", (double)stats.m_saveDurationSec1); return stats; } \ No newline at end of file From c4a849c8915775e6af2039411b525181a1af3ed7 Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 12 Jun 2026 00:33:44 +0200 Subject: [PATCH 3/4] Fix CI: add missing tracy/Tracy.hpp include to main.cpp MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit TracyPlot macro used in main.cpp but header was only included transitively — add direct include so no-op macros are defined when MANDEYE_USE_TRACY is OFF. Co-Authored-By: Claude Sonnet 4.6 --- code/lidars/BaseLidarClient.h | 4 ++-- code/lidars/hesai/HesaiClient.cpp | 14 +++++++------- code/main.cpp | 8 +++++--- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/code/lidars/BaseLidarClient.h b/code/lidars/BaseLidarClient.h index a71365b..5b993e2 100644 --- a/code/lidars/BaseLidarClient.h +++ b/code/lidars/BaseLidarClient.h @@ -47,7 +47,7 @@ class BaseLidarClient : public mandeye_utils::TimeStampProvider { public: //! Initialize the client with a configuration - virtual void Init(const nlohmann::json& config) {}; + virtual void Init(const nlohmann::json& config) { }; virtual ~BaseLidarClient() = default; //! Produce a status report in JSON format @@ -57,7 +57,7 @@ class BaseLidarClient : public mandeye_utils::TimeStampProvider virtual bool startListener(const std::string& interfaceIp) = 0; //! Stops the listener on a specific interface IP - virtual void stopListener() {}; + virtual void stopListener() { }; //! Start logging data from the Lidar and IMU virtual void startLog() = 0; diff --git a/code/lidars/hesai/HesaiClient.cpp b/code/lidars/hesai/HesaiClient.cpp index 824fc0f..3282ee2 100644 --- a/code/lidars/hesai/HesaiClient.cpp +++ b/code/lidars/hesai/HesaiClient.cpp @@ -2,9 +2,9 @@ #include #include + 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 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& dataFrame) { - m_recivedPointMessages.fetch_add(1); m_lidar_state = dataFrame.lidar_state; m_work_mode = dataFrame.work_mode; @@ -176,15 +176,16 @@ void HesaiClient::CallbackFrame(const LidarDecodedFrame& data } } -uint64_t HesaiClient::GetBufferSize() const { +uint64_t HesaiClient::GetBufferSize() const +{ std::lock_guard lock(m_bufferPointMutex); - if (!m_bufferLidarPtr) { + if(!m_bufferLidarPtr) + { return 0; } return m_bufferLidarPtr->size(); } - void HesaiClient::CallbackIMU(const LidarImuData& dataFrame) { m_recivedIMUMessages.fetch_add(1); @@ -218,5 +219,4 @@ void HesaiClient::CallbackFault(const FaultMessageInfo& fault_message_info) m_faults.pop_front(); } } - } // namespace mandeye diff --git a/code/main.cpp b/code/main.cpp index 63545cb..92f1bce 100644 --- a/code/main.cpp +++ b/code/main.cpp @@ -23,6 +23,7 @@ #include "hardware_config/mandeye.h" #include +#include #define MANDEYE_LIVOX_LISTEN_IP "192.168.1.5" #define MANDEYE_LIDAR_SKD "LIVOX_SDK2" @@ -492,7 +493,8 @@ void stateWatcher() else { auto [fn, saveStats] = savePointcloudData(lidarBuffer, continousScanDirectory, chunksInExperimentCS + chunksInExperimentSS); - if(saveStats) lastFileSaveStats = *saveStats; + if(saveStats) + lastFileSaveStats = *saveStats; saveImuData(imuBuffer, continousScanDirectory, chunksInExperimentCS + chunksInExperimentSS); saveStatusData(continousScanDirectory, chunksInExperimentCS + chunksInExperimentSS); auto lidarList = lidarClientPtr->getSerialNumberToLidarIdMapping(); @@ -849,11 +851,11 @@ int main(int argc, char** argv) // start zeromq publisher mandeye::publisherPtr = std::make_shared(); mandeye::publisherPtr->SetTimeStampProvider(mandeye::lidarClientPtr); - while (mandeye::isRunning) + while(mandeye::isRunning) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); auto bufferSize = mandeye::lidarClientPtr->GetBufferSize(); - TracyPlot("bufferSize", double(bufferSize)/1e6); + TracyPlot("bufferSize", double(bufferSize) / 1e6); } }); From c66b48d7c87f030f540440856982a871c6cc4f2e Mon Sep 17 00:00:00 2001 From: Michal Pelka Date: Fri, 12 Jun 2026 00:41:42 +0200 Subject: [PATCH 4/4] Fix clang-format-18 violations in BaseLidarClient.h Co-Authored-By: Claude Sonnet 4.6 --- code/lidars/BaseLidarClient.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/code/lidars/BaseLidarClient.h b/code/lidars/BaseLidarClient.h index 5b993e2..a71365b 100644 --- a/code/lidars/BaseLidarClient.h +++ b/code/lidars/BaseLidarClient.h @@ -47,7 +47,7 @@ class BaseLidarClient : public mandeye_utils::TimeStampProvider { public: //! Initialize the client with a configuration - virtual void Init(const nlohmann::json& config) { }; + virtual void Init(const nlohmann::json& config) {}; virtual ~BaseLidarClient() = default; //! Produce a status report in JSON format @@ -57,7 +57,7 @@ class BaseLidarClient : public mandeye_utils::TimeStampProvider virtual bool startListener(const std::string& interfaceIp) = 0; //! Stops the listener on a specific interface IP - virtual void stopListener() { }; + virtual void stopListener() {}; //! Start logging data from the Lidar and IMU virtual void startLog() = 0;