move_base2/src/io/runtime_stats.cpp

/**
 * @file runtime_stats.cpp
 * @brief Hiện thực @ref move_base2::RuntimeStats.
 */
#include <move_base2/io/runtime_stats.h>

#include <algorithm>
#include <cstdio>
#include <cstring>
#include <fstream>
#include <sstream>

#ifdef __linux__
#include <dirent.h>
#include <sys/syscall.h>
#include <unistd.h>
#endif

#include <robot/robot.h>

namespace move_base2
{
namespace
{

/// Bề rộng cột nhãn của bảng — đủ cho `costmap/global_costmap` mà không xuống dòng.
constexpr int kLabelWidth = 26;

/// Tên hiển thị cho phần CPU không thuộc thread nào đã đăng ký (host ROS, ROS internals, plugin).
constexpr const char* kUnregisteredLabel = "(unregistered)";

/**
 * @brief Đệm khoảng trắng bên phải cho đủ @p width **ký tự hiển thị**.
 *
 * `printf("%-*s")` đếm BYTE, mà nhãn ở đây có dấu tiếng Việt (UTF-8, 2 byte/ký tự) — dùng thẳng
 * printf thì bảng lệch cột đúng bằng số dấu. Byte nối tiếp của UTF-8 luôn có dạng 10xxxxxx nên đếm
 * byte KHÔNG phải continuation là ra số ký tự.
 */
std::string padRight(const std::string& text, int width)
{
  int visible = 0;
  for (const char ch : text)
  {
    if ((static_cast<unsigned char>(ch) & 0xC0) != 0x80)
    {
      ++visible;
    }
  }
  std::string padded = text;
  for (int i = visible; i < width; ++i)
  {
    padded += ' ';
  }
  return padded;
}

double ticksPerSecond()
{
#ifdef __linux__
  const long hz = sysconf(_SC_CLK_TCK);
  return hz > 0 ? static_cast<double>(hz) : 100.0;
#else
  return 100.0;
#endif
}

/**
 * @brief Lấy utime+stime từ một dòng `/proc/.../stat`.
 *
 * Không tách theo khoảng trắng từ đầu dòng được: trường thứ hai là tên tiến trình, nằm trong ngoặc
 * đơn và **có thể chứa cả khoảng trắng lẫn ngoặc**. Mốc đáng tin duy nhất là dấu `)` cuối cùng.
 */
std::uint64_t parseCpuTicks(const std::string& stat_line)
{
  const std::size_t close = stat_line.rfind(')');
  if (close == std::string::npos)
  {
    return 0;
  }

  std::istringstream iss(stat_line.substr(close + 1));
  std::string field;
  // Sau dấu ')' , trường đầu tiên là state; utime là trường thứ 12, stime thứ 13.
  std::uint64_t utime = 0;
  std::uint64_t stime = 0;
  for (int index = 1; index <= 13; ++index)
  {
    if (!(iss >> field))
    {
      return 0;
    }
    if (index == 12)
    {
      utime = std::strtoull(field.c_str(), nullptr, 10);
    }
    else if (index == 13)
    {
      stime = std::strtoull(field.c_str(), nullptr, 10);
    }
  }
  return utime + stime;
}

std::uint64_t readCpuTicksFrom(const std::string& path)
{
  std::ifstream file(path);
  if (!file.is_open())
  {
    return 0;
  }
  std::string line;
  std::getline(file, line);
  return parseCpuTicks(line);
}

}  // namespace

RuntimeStats::RuntimeStats(double period_seconds)
  : period_seconds_(period_seconds)
  , ticks_per_second_(ticksPerSecond())
  , window_start_(std::chrono::steady_clock::now())
{
  if (!enabled())
  {
    return;
  }
  last_process_cpu_ticks_ = readProcessCpuTicks();
  last_rss_bytes_ = readProcessRssBytes();
}

// ================================================================================================
// Đọc /proc
// ================================================================================================

std::uint64_t RuntimeStats::readThreadCpuTicks(long tid)
{
#ifdef __linux__
  return readCpuTicksFrom("/proc/self/task/" + std::to_string(tid) + "/stat");
#else
  (void)tid;
  return 0;
#endif
}

std::uint64_t RuntimeStats::readProcessCpuTicks()
{
#ifdef __linux__
  return readCpuTicksFrom("/proc/self/stat");
#else
  return 0;
#endif
}

std::uint64_t RuntimeStats::readProcessRssBytes()
{
#ifdef __linux__
  std::ifstream file("/proc/self/statm");
  if (!file.is_open())
  {
    return 0;
  }
  std::uint64_t total_pages = 0;
  std::uint64_t resident_pages = 0;
  file >> total_pages >> resident_pages;
  const long page_size = sysconf(_SC_PAGESIZE);
  return resident_pages * static_cast<std::uint64_t>(page_size > 0 ? page_size : 4096);
#else
  return 0;
#endif
}

std::vector<long> RuntimeStats::listThreadIds()
{
  std::vector<long> tids;
#ifdef __linux__
  DIR* dir = opendir("/proc/self/task");
  if (dir == nullptr)
  {
    return tids;
  }
  while (const dirent* entry = readdir(dir))
  {
    if (entry->d_name[0] == '.')
    {
      continue;
    }
    tids.push_back(std::strtol(entry->d_name, nullptr, 10));
  }
  closedir(dir);
  std::sort(tids.begin(), tids.end());
#endif
  return tids;
}

// ================================================================================================
// Đăng ký
// ================================================================================================

RuntimeStats::SectionId RuntimeStats::section(const std::string& name)
{
  if (!enabled())
  {
    return kInvalidSection;
  }

  std::lock_guard<std::mutex> lock(mutex_);
  for (SectionId id = 0; id < sections_.size(); ++id)
  {
    if (sections_[id].name == name)
    {
      return id;
    }
  }
  sections_.push_back(Section{ name, 0, 0, 0 });
  return sections_.size() - 1;
}

void RuntimeStats::record(SectionId id, std::int64_t nanoseconds)
{
  if (!enabled() || id == kInvalidSection)
  {
    return;
  }

  std::lock_guard<std::mutex> lock(mutex_);
  if (id >= sections_.size())
  {
    return;
  }
  Section& section = sections_[id];
  ++section.calls;
  section.total_ns += nanoseconds;
  section.max_ns = std::max(section.max_ns, nanoseconds);
}

void RuntimeStats::registerCurrentThread(const std::string& label)
{
  if (!enabled())
  {
    return;
  }

#ifdef __linux__
  // syscall trực tiếp thay cho gettid(): wrapper của glibc chỉ có từ 2.30, gọi thẳng thì không phụ
  // thuộc phiên bản libc của máy build.
  const long tid = static_cast<long>(syscall(SYS_gettid));
#else
  const long tid = 0;
#endif

  std::lock_guard<std::mutex> lock(mutex_);
  for (Thread& thread : threads_)
  {
    if (thread.tid == tid)
    {
      thread.label = label;
      return;
    }
  }
  threads_.push_back(Thread{ tid, label, readThreadCpuTicks(tid) });
}

void RuntimeStats::beginThreadCapture()
{
  if (!enabled())
  {
    return;
  }
  std::lock_guard<std::mutex> lock(mutex_);
  capture_before_ = listThreadIds();
  capturing_ = true;
}

void RuntimeStats::endThreadCapture(const std::string& label)
{
  if (!enabled())
  {
    return;
  }

  std::lock_guard<std::mutex> lock(mutex_);
  if (!capturing_)
  {
    robot::log_warning("[move_base2] RuntimeStats::endThreadCapture('%s') without an open capture "
                       "window.\n",
                       label.c_str());
    return;
  }
  capturing_ = false;

  const std::vector<long> after = listThreadIds();
  int labelled = 0;
  for (const long tid : after)
  {
    if (std::binary_search(capture_before_.begin(), capture_before_.end(), tid))
    {
      continue;
    }
    threads_.push_back(Thread{ tid, label, readThreadCpuTicks(tid) });
    ++labelled;
  }

  if (labelled == 0)
  {
    // Không phải lỗi chết người, nhưng phải nói ra: im lặng ở đây nghĩa là bảng thiếu hẳn một
    // thành phần và người đọc lại tưởng thành phần đó không tốn gì.
    robot::log_warning("[move_base2] RuntimeStats: '%s' created no thread — its CPU column will "
                       "not appear.\n",
                       label.c_str());
  }
}

// ================================================================================================
// In bảng
// ================================================================================================

bool RuntimeStats::tick()
{
  if (!enabled())
  {
    return false;
  }

  const auto now = std::chrono::steady_clock::now();
  const double elapsed = std::chrono::duration<double>(now - window_start_).count();
  if (elapsed < period_seconds_)
  {
    return false;
  }

  const std::string table = render();
  robot::log_info("%s", table.c_str());
  return true;
}

std::string RuntimeStats::render()
{
  std::lock_guard<std::mutex> lock(mutex_);

  const auto now = std::chrono::steady_clock::now();
  const double window = std::chrono::duration<double>(now - window_start_).count();
  const double safe_window = window > 1e-6 ? window : 1e-6;

  const std::uint64_t process_ticks = readProcessCpuTicks();
  const std::uint64_t rss_bytes = readProcessRssBytes();
  const double process_cpu =
      100.0 * static_cast<double>(process_ticks - last_process_cpu_ticks_) / ticks_per_second_ / safe_window;
  const double rss_mb = static_cast<double>(rss_bytes) / (1024.0 * 1024.0);
  const double rss_delta_mb = (static_cast<double>(rss_bytes) - static_cast<double>(last_rss_bytes_)) / (1024.0 * 1024.0);

  const std::vector<long> all_tids = listThreadIds();

  char line[256];
  std::ostringstream out;
  out << "\n";
  std::snprintf(line, sizeof(line), "[move_base2] ===== runtime stats — window %.2f s =====\n",
                window);
  out << line;
  std::snprintf(line, sizeof(line),
                "  process: RSS %.1f MB (%+.1f MB in window, %+.1f MB/min)  CPU %.1f%%  thread %zu\n",
                rss_mb, rss_delta_mb, rss_delta_mb * 60.0 / safe_window, process_cpu, all_tids.size());
  out << line;

  // --- CPU theo thread ---------------------------------------------------------------------------
  out << "  " << padRight("thread", kLabelWidth) << "     CPU%\n";

  double registered_cpu = 0.0;
  std::size_t registered_alive = 0;
  for (Thread& thread : threads_)
  {
    const bool alive = std::binary_search(all_tids.begin(), all_tids.end(), thread.tid);
    const std::uint64_t ticks = alive ? readThreadCpuTicks(thread.tid) : thread.last_cpu_ticks;
    const double cpu =
        100.0 * static_cast<double>(ticks - thread.last_cpu_ticks) / ticks_per_second_ / safe_window;
    thread.last_cpu_ticks = ticks;

    if (alive)
    {
      registered_cpu += cpu;
      ++registered_alive;
    }

    std::snprintf(line, sizeof(line), " %8.1f%s\n", cpu, alive ? "" : "   (finished)");
    out << "    " << padRight(thread.label, kLabelWidth - 2) << line;
  }

  // Phần còn lại của tiến trình. Đây là con số quan trọng nhất khi đi tìm thủ phạm CPU: nếu nó lớn
  // hơn hẳn tổng các thread đã đăng ký thì vấn đề KHÔNG nằm trong navigation stack.
  const double other_cpu = process_cpu - registered_cpu;
  std::snprintf(line, sizeof(line), " %8.1f   (%zu thread)\n", other_cpu,
                all_tids.size() > registered_alive ? all_tids.size() - registered_alive : 0);
  out << "    " << padRight(kUnregisteredLabel, kLabelWidth - 2) << line;

  // --- Chi phí theo đoạn công việc ----------------------------------------------------------------
  std::snprintf(line, sizeof(line), " %8s %10s %10s %8s\n", "calls/s", "avg [ms]", "peak [ms]",
                "CPU%");
  out << "  " << padRight("work section", kLabelWidth) << line;

  for (Section& section : sections_)
  {
    const double calls_per_second = static_cast<double>(section.calls) / safe_window;
    const double avg_ms =
        section.calls > 0 ? static_cast<double>(section.total_ns) / static_cast<double>(section.calls) / 1e6 : 0.0;
    const double max_ms = static_cast<double>(section.max_ns) / 1e6;
    // Tỷ lệ chiếm dụng: tổng thời gian đoạn này chạy so với chiều dài cửa sổ, quy ra %/1 core —
    // cùng đơn vị với cột CPU% ở trên nên so sánh trực tiếp được.
    const double share = 100.0 * static_cast<double>(section.total_ns) / 1e9 / safe_window;

    std::snprintf(line, sizeof(line), " %8.1f %10.2f %10.2f %8.1f\n", calls_per_second, avg_ms, max_ms,
                  share);
    out << "    " << padRight(section.name, kLabelWidth - 2) << line;

    section.calls = 0;
    section.total_ns = 0;
    section.max_ns = 0;
  }

  window_start_ = now;
  last_process_cpu_ticks_ = process_ticks;
  last_rss_bytes_ = rss_bytes;

  return out.str();
}

}  // namespace move_base2