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 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..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,6 +176,16 @@ 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); @@ -209,5 +219,4 @@ void HesaiClient::CallbackFault(const FaultMessageInfo& fault_message_info) m_faults.pop_front(); } } - } // namespace mandeye 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..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" @@ -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::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