mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-09-12 03:00:20 +08:00
302 lines
11 KiB
C++
302 lines
11 KiB
C++
#include <rclcpp/rclcpp.hpp>
|
|
#include <rclcpp_components/register_node_macro.hpp>
|
|
#include <orbbec_camera/ob_camera_node_driver.h>
|
|
#include <orbbec_camera/utils.h>
|
|
#include "orbbec_camera_msgs/msg/metadata.hpp"
|
|
#include <message_filters/subscriber.h>
|
|
#include <message_filters/sync_policies/approximate_time.h>
|
|
#include <message_filters/synchronizer.h>
|
|
#include <filesystem>
|
|
|
|
namespace orbbec_camera {
|
|
namespace tools {
|
|
|
|
class ObBenchmark : public rclcpp::Node {
|
|
public:
|
|
ObBenchmark() : Node("ObBenchmark") {
|
|
params_init();
|
|
stat_process();
|
|
function_ = this->create_wall_timer(std::chrono::seconds(test_cycle_),
|
|
std::bind(&ObBenchmark::functionCallback, this));
|
|
config_ = this->create_wall_timer(std::chrono::seconds(switch_cycle_),
|
|
std::bind(&ObBenchmark::configCallback, this));
|
|
}
|
|
~ObBenchmark() { kill_process(process_name_); }
|
|
|
|
private:
|
|
rclcpp::TimerBase::SharedPtr function_;
|
|
rclcpp::TimerBase::SharedPtr config_;
|
|
|
|
std::vector<float> cpu_usage_;
|
|
std::vector<float> memory_usage_;
|
|
std::vector<std::string> current_time_;
|
|
|
|
int usage_count_ = 0;
|
|
int launch_count_ = 0;
|
|
long prevIdle = 0;
|
|
long prevTotal = 0;
|
|
std::string process_pid_;
|
|
|
|
std::string process_name_;
|
|
int switch_cycle_;
|
|
int test_cycle_;
|
|
int skip_number_;
|
|
|
|
std::mutex image_mutex_;
|
|
|
|
void params_init() {
|
|
std::ifstream file(
|
|
"install/orbbec_camera/share/orbbec_camera/config/tools/startbenchmark/"
|
|
"start_benchmark_params.json");
|
|
if (!file.is_open()) {
|
|
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to open JSON file.");
|
|
return;
|
|
}
|
|
nlohmann::json json_data;
|
|
file >> json_data;
|
|
process_name_ = json_data["start_benchmark_params"]["process_name"].get<std::string>();
|
|
switch_cycle_ = json_data["start_benchmark_params"]["switch_cycle"].get<int>();
|
|
test_cycle_ = json_data["start_benchmark_params"]["test_cycle"].get<int>();
|
|
skip_number_ = json_data["start_benchmark_params"]["skip_number"].get<int>();
|
|
}
|
|
std::string exec_command(const std::string& command) {
|
|
std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
|
|
if (!pipe) {
|
|
std::cerr << "Failed to run command" << std::endl;
|
|
return "";
|
|
}
|
|
|
|
char buffer[128];
|
|
std::string result = "";
|
|
while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) {
|
|
result += buffer;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
float get_ps_cpu_usage_by_process(const std::string& process_name) {
|
|
std::string command = "ps aux | grep '" + process_name + "' | grep -v grep";
|
|
std::string result = exec_command(command);
|
|
std::istringstream stream(result);
|
|
std::string line;
|
|
float total_cpu_usage = 0.0;
|
|
|
|
while (std::getline(stream, line)) {
|
|
std::istringstream line_stream(line);
|
|
std::string user, pid, cpu, mem, command;
|
|
line_stream >> user >> pid >> cpu >> mem;
|
|
|
|
if (cpu.empty() || line.find("grep") != std::string::npos) {
|
|
continue;
|
|
}
|
|
|
|
try {
|
|
process_pid_ = pid;
|
|
float cpu_usage = std::stof(cpu);
|
|
total_cpu_usage += cpu_usage;
|
|
} catch (const std::invalid_argument& e) {
|
|
continue;
|
|
}
|
|
}
|
|
|
|
return total_cpu_usage;
|
|
}
|
|
|
|
float get_top_cpu_usage_by_process() {
|
|
std::string command =
|
|
"top -bn1 -p " + process_pid_ + " | grep '" + process_pid_ + "' | awk '{print $9}'";
|
|
std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
|
|
if (!pipe) {
|
|
std::cerr << "Failed to run command" << std::endl;
|
|
return -1.0f;
|
|
}
|
|
char buffer[128];
|
|
float total_cpu_usage = 0.0f;
|
|
bool found_process = false;
|
|
while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) {
|
|
std::stringstream ss(buffer);
|
|
float cpu_usage = 0.0f;
|
|
ss >> cpu_usage;
|
|
if (ss) {
|
|
total_cpu_usage += cpu_usage;
|
|
found_process = true;
|
|
}
|
|
}
|
|
if (!found_process) {
|
|
std::cerr << "No matching processes found for PID: " << process_pid_ << std::endl;
|
|
return -1.0f;
|
|
}
|
|
return total_cpu_usage;
|
|
}
|
|
float get_memory_usage_by_process() {
|
|
std::string command =
|
|
"top -b -n 1 -p " + process_pid_ + " | grep '" + process_pid_ + "' | awk '{print $6}'";
|
|
std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
|
|
if (!pipe) {
|
|
std::cerr << "Failed to run command" << std::endl;
|
|
return -1.0f;
|
|
}
|
|
char buffer[128];
|
|
std::string result = "";
|
|
|
|
while (fgets(buffer, sizeof(buffer), pipe.get()) != nullptr) {
|
|
result += buffer;
|
|
}
|
|
std::stringstream ss(result);
|
|
long memory_usage_kb;
|
|
ss >> memory_usage_kb;
|
|
float memory_usage_mb = static_cast<float>(memory_usage_kb) / 1024.0f;
|
|
std::stringstream formatted_result;
|
|
formatted_result << std::fixed << std::setprecision(1) << memory_usage_mb;
|
|
return std::stof(formatted_result.str());
|
|
}
|
|
std::string getCurrentTimes() {
|
|
auto now = std::chrono::system_clock::now();
|
|
auto now_time_t = std::chrono::system_clock::to_time_t(now);
|
|
std::tm tm = *std::localtime(&now_time_t);
|
|
std::ostringstream date_stream;
|
|
date_stream << std::put_time(&tm, "%H:%M:%S");
|
|
std::string date_str = date_stream.str();
|
|
return date_str;
|
|
}
|
|
void ensure_directory_exists(const std::string& path) {
|
|
size_t found = path.find_last_of("/\\");
|
|
std::string dir_path = path.substr(0, found);
|
|
|
|
struct stat info;
|
|
if (stat(dir_path.c_str(), &info) != 0) {
|
|
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
|
"Directory does not exist, creating it: " << dir_path);
|
|
if (mkdir(dir_path.c_str(), 0777) != 0) {
|
|
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to create directory: " << dir_path.c_str());
|
|
}
|
|
}
|
|
}
|
|
void save_to_csv(const std::string& filename) {
|
|
ensure_directory_exists(filename);
|
|
std::ofstream file;
|
|
file.open(filename, std::ios_base::app);
|
|
if (file.is_open()) {
|
|
file << "Time,CPU,Memory(MB)"
|
|
<< "\n";
|
|
for (int i = skip_number_; i < usage_count_ - 1; i++) {
|
|
file << current_time_[i] << "," << cpu_usage_[i] << "%"
|
|
<< "," << memory_usage_[i] << "\n";
|
|
}
|
|
float cpu_average = std::accumulate(cpu_usage_.begin() + skip_number_,
|
|
cpu_usage_.begin() + usage_count_ - 1, 0.0f) /
|
|
(usage_count_ - (skip_number_ + 1));
|
|
float memory_average = std::accumulate(memory_usage_.begin() + skip_number_,
|
|
memory_usage_.begin() + usage_count_ - 1, 0.0f) /
|
|
(usage_count_ - (skip_number_ + 1));
|
|
file << "Average: ," << cpu_average << "%, " << memory_average << "\n";
|
|
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
|
"Average: " << cpu_average << "%" << memory_average << "MB");
|
|
file.close();
|
|
} else {
|
|
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to open file for writing.");
|
|
}
|
|
cpu_usage_.clear();
|
|
memory_usage_.clear();
|
|
current_time_.clear();
|
|
usage_count_ = 0;
|
|
++launch_count_;
|
|
}
|
|
void kill_process(const std::string& process_name) {
|
|
std::string command = "ps aux | grep " + process_name + " | grep -v grep | awk '{print $2}'";
|
|
|
|
std::shared_ptr<FILE> pipe(popen(command.c_str(), "r"), pclose);
|
|
if (!pipe) {
|
|
RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to run command to find PID.");
|
|
return;
|
|
}
|
|
|
|
std::vector<int> pids;
|
|
char path[1035];
|
|
while (fgets(path, sizeof(path), pipe.get()) != nullptr) {
|
|
int pid = std::stoi(path);
|
|
pids.push_back(pid);
|
|
std::cout << "----------------------------------------------------------" << std::endl;
|
|
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"), "Found PID: " << pid);
|
|
}
|
|
|
|
if (pids.empty()) {
|
|
RCLCPP_WARN(this->get_logger(), "No matching processes found.");
|
|
return;
|
|
}
|
|
|
|
for (int pid : pids) {
|
|
if (kill(pid, SIGINT) == 0) {
|
|
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
|
"Successfully sent SIGINT (Ctrl+C) to process with PID " << pid);
|
|
} else {
|
|
RCLCPP_ERROR_STREAM(this->get_logger(),
|
|
"Failed to send SIGINT to process with PID " << pid);
|
|
}
|
|
}
|
|
}
|
|
void stat_process() {
|
|
const char* ros2_launch = "/opt/ros/humble/bin/ros2";
|
|
std::string start_launch = "ob_benchmark_" + std::to_string(launch_count_) + ".launch.py";
|
|
std::vector<const char*> args = {ros2_launch, "launch", "orbbec_camera", start_launch.c_str(),
|
|
nullptr};
|
|
|
|
pid_t pid = fork();
|
|
|
|
if (pid == 0) {
|
|
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
|
"Child process: launching ros2 launch.");
|
|
if (execvp(ros2_launch, (char* const*)args.data()) == -1) {
|
|
RCLCPP_ERROR_STREAM(this->get_logger(),
|
|
"Failed to execute ros2 launch: " << strerror(errno));
|
|
exit(1);
|
|
}
|
|
} else if (pid > 0) {
|
|
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
|
"Parent process: continuing execution.");
|
|
} else {
|
|
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"), "Fork failed!");
|
|
}
|
|
}
|
|
|
|
void functionCallback() {
|
|
std::lock_guard<std::mutex> lock(image_mutex_);
|
|
// RCLCPP_INFO_STREAM(
|
|
// rclcpp::get_logger("ObBenchmark"),
|
|
// "get_ps_cpu_usage_by_process: " << get_ps_cpu_usage_by_process(process_name_));
|
|
// RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
|
// "get_top_cpu_usage_by_process: " << get_top_cpu_usage_by_process());
|
|
float cpu_usage =
|
|
get_ps_cpu_usage_by_process(process_name_) * 0.4 + get_top_cpu_usage_by_process() * 0.6;
|
|
float memory_usage = get_memory_usage_by_process();
|
|
cpu_usage_.push_back(cpu_usage);
|
|
memory_usage_.push_back(memory_usage);
|
|
current_time_.push_back(getCurrentTimes());
|
|
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
|
"CurrentTimes: " << current_time_[usage_count_]);
|
|
RCLCPP_INFO_STREAM(rclcpp::get_logger("ObBenchmark"),
|
|
"CPU Usage of " << process_name_ << ":" << cpu_usage_[usage_count_] << "%");
|
|
RCLCPP_INFO_STREAM(
|
|
rclcpp::get_logger("ObBenchmark"),
|
|
"Memory Usage of " << process_name_ << ":" << memory_usage_[usage_count_] << "MB");
|
|
usage_count_++;
|
|
}
|
|
void configCallback() {
|
|
std::lock_guard<std::mutex> lock(image_mutex_);
|
|
std::string csv = std::string("ob_benchmark/") + std::to_string(launch_count_) + ".csv";
|
|
|
|
save_to_csv(csv);
|
|
kill_process(process_name_);
|
|
stat_process();
|
|
}
|
|
};
|
|
|
|
} // namespace tools
|
|
} // namespace orbbec_camera
|
|
int main(int argc, char** argv) {
|
|
rclcpp::init(argc, argv);
|
|
rclcpp::spin(std::make_shared<orbbec_camera::tools::ObBenchmark>());
|
|
rclcpp::shutdown();
|
|
return 0;
|
|
}
|