mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-04 03:57:46 +08:00
Update orbbec_camera package to be compatible with cuvslam
This commit is contained in:
jj
@@ -0,0 +1,25 @@
|
||||
/*******************************************************************************
|
||||
* Copyright (c) 2023 Orbbec 3D Technology, Inc
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*******************************************************************************/
|
||||
#include "multi_save_cloud_node.hpp"
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<orbbec_camera::tools::MultiCameraCloudSubscriber>();
|
||||
rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 8);
|
||||
executor.add_node(node);
|
||||
executor.spin();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,110 @@
|
||||
#pragma once
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <orbbec_camera/ob_camera_node_driver.h>
|
||||
#include <orbbec_camera/ob_camera_node.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 <std_msgs/msg/int32.hpp>
|
||||
#include <filesystem>
|
||||
|
||||
namespace orbbec_camera {
|
||||
namespace tools {
|
||||
|
||||
class MultiCameraCloudSubscriber : public rclcpp::Node {
|
||||
public:
|
||||
MultiCameraCloudSubscriber() : Node("multi_camera_cloud_subscriber") { topic_init(); }
|
||||
// ~MultiCameraCloudSubscriber() {
|
||||
// }
|
||||
|
||||
private:
|
||||
std::mutex image_mutex_;
|
||||
void topic_init() {
|
||||
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
|
||||
current_path = std::filesystem::current_path().string();
|
||||
stand_cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
"/camera/depth/points", custom_qos,
|
||||
[this](std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
|
||||
this->stand_cloud_Callback(msg);
|
||||
});
|
||||
transform_cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
"/rear_camera/depth/points", custom_qos,
|
||||
[this](std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
|
||||
this->transform_cloud_Callback(msg);
|
||||
});
|
||||
}
|
||||
|
||||
void stand_cloud_Callback(std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
std::stringstream ss;
|
||||
auto now = std::time(nullptr);
|
||||
ss << std::put_time(std::localtime(&now), "%Y%m%d_%H%M%S");
|
||||
std::string filename = current_path + "/point_cloud/points_" + ss.str() + ".ply";
|
||||
if (!std::filesystem::exists(current_path + "/point_cloud")) {
|
||||
std::filesystem::create_directory(current_path + "/point_cloud");
|
||||
}
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_cloud_subscriber"), "Saving point cloud to " << filename);
|
||||
saveDepthPointsToPly(msg, filename);
|
||||
|
||||
}
|
||||
void transform_cloud_Callback(std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
std::stringstream ss;
|
||||
auto now = std::time(nullptr);
|
||||
ss << std::put_time(std::localtime(&now), "%Y%m%d_%H%M%S");
|
||||
std::string filename = current_path + "/rear_camera/point_cloud/points_" + ss.str() + ".ply";
|
||||
if (!std::filesystem::exists(current_path + "/point_cloud")) {
|
||||
std::filesystem::create_directory(current_path + "/point_cloud");
|
||||
}
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_cloud_subscriber"),
|
||||
"Saving transform_cloud to " << filename);
|
||||
saveDepthPointsToPly(msg, filename);
|
||||
}
|
||||
void saveDepthPointsToPly(const std::shared_ptr<const sensor_msgs::msg::PointCloud2> &msg,
|
||||
const std::string &fileName) {
|
||||
FILE *fp = fopen(fileName.c_str(), "wb+");
|
||||
CHECK_NOTNULL(fp);
|
||||
CHECK_NOTNULL(msg);
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
|
||||
|
||||
// First, count the actual number of valid points
|
||||
size_t valid_points = 0;
|
||||
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
|
||||
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
|
||||
++valid_points;
|
||||
}
|
||||
}
|
||||
|
||||
// Reset the iterators
|
||||
iter_x = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "x");
|
||||
iter_y = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "y");
|
||||
iter_z = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "z");
|
||||
|
||||
fprintf(fp, "ply\n");
|
||||
fprintf(fp, "format ascii 1.0\n");
|
||||
fprintf(fp, "element vertex %zu\n", valid_points);
|
||||
fprintf(fp, "property float x\n");
|
||||
fprintf(fp, "property float y\n");
|
||||
fprintf(fp, "property float z\n");
|
||||
fprintf(fp, "end_header\n");
|
||||
|
||||
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
|
||||
if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) {
|
||||
fprintf(fp, "%.3f %.3f %.3f\n", *iter_x, *iter_y, *iter_z);
|
||||
}
|
||||
}
|
||||
|
||||
fflush(fp);
|
||||
fclose(fp);
|
||||
}
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr stand_cloud_sub_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr transform_cloud_sub_;
|
||||
std::string current_path;
|
||||
};
|
||||
} // namespace tools
|
||||
} // namespace orbbec_camera
|
||||
@@ -154,7 +154,7 @@ class MultiCameraSubscriber : public rclcpp::Node {
|
||||
color_metadata_.exposure_buffs.resize(left_ir_topics_.size());
|
||||
color_metadata_.gain_buffs.resize(left_ir_topics_.size());
|
||||
callback_called_ = std::vector<bool>(left_ir_topics_.size(), false);
|
||||
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
|
||||
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data),rmw_qos_profile_sensor_data);
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
|
||||
"camera_name_.size(): " << camera_name_.size());
|
||||
for (size_t i = 0; i < camera_name_.size(); ++i) {
|
||||
|
||||
@@ -1,300 +0,0 @@
|
||||
#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;
|
||||
}
|
||||
@@ -1,148 +0,0 @@
|
||||
#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 StartBenchmark : public rclcpp::Node {
|
||||
public:
|
||||
explicit StartBenchmark(const rclcpp::NodeOptions& options) : Node("StartBenchmark", options) {
|
||||
params_init();
|
||||
auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
|
||||
for (size_t i = 0; i < camera_name_.size(); ++i) {
|
||||
color_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
|
||||
color_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
|
||||
this->color_Callback(msg, i);
|
||||
}));
|
||||
depth_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
|
||||
depth_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
|
||||
this->depth_Callback(msg, i);
|
||||
}));
|
||||
left_ir_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
|
||||
left_ir_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
|
||||
this->left_ir_Callback(msg, i);
|
||||
}));
|
||||
right_ir_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
|
||||
right_ir_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
|
||||
this->right_ir_Callback(msg, i);
|
||||
}));
|
||||
depth_point_cloud_subs_.push_back(this->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
depth_point_cloud_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
|
||||
this->depth_point_cloud_Callback(msg, i);
|
||||
}));
|
||||
color_point_cloud_subs_.push_back(this->create_subscription<sensor_msgs::msg::PointCloud2>(
|
||||
color_point_cloud_topics_[i], custom_qos,
|
||||
[this, i](std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg) {
|
||||
this->color_point_cloud_Callback(msg, i);
|
||||
}));
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), color_topics_[i] << " is subed ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), depth_topics_[i] << " is subed ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), left_ir_topics_[i] << " is subed ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), right_ir_topics_[i] << " is subed ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
depth_point_cloud_topics_[i] << " is subed ");
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
color_point_cloud_topics_[i] << " is subed ");
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subs_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> depth_subs_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> left_ir_subs_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> right_ir_subs_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
|
||||
depth_point_cloud_subs_;
|
||||
std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr>
|
||||
color_point_cloud_subs_;
|
||||
|
||||
|
||||
std::vector<std::string> camera_name_;
|
||||
std::vector<std::string> color_topics_;
|
||||
std::vector<std::string> depth_topics_;
|
||||
std::vector<std::string> left_ir_topics_;
|
||||
std::vector<std::string> right_ir_topics_;
|
||||
std::vector<std::string> depth_point_cloud_topics_;
|
||||
std::vector<std::string> color_point_cloud_topics_;
|
||||
|
||||
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;
|
||||
camera_name_ =
|
||||
json_data["start_benchmark_params"]["camera_name"].get<std::vector<std::string>>();
|
||||
color_topics_.resize(camera_name_.size());
|
||||
depth_topics_.resize(camera_name_.size());
|
||||
left_ir_topics_.resize(camera_name_.size());
|
||||
right_ir_topics_.resize(camera_name_.size());
|
||||
depth_point_cloud_topics_.resize(camera_name_.size());
|
||||
color_point_cloud_topics_.resize(camera_name_.size());
|
||||
for (size_t i = 0; i < camera_name_.size(); ++i) {
|
||||
color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw";
|
||||
depth_topics_[i] = "/" + camera_name_[i] + "/depth/image_raw";
|
||||
left_ir_topics_[i] = "/" + camera_name_[i] + "/left_ir/image_raw";
|
||||
right_ir_topics_[i] = "/" + camera_name_[i] + "/right_ir/image_raw";
|
||||
depth_point_cloud_topics_[i] = "/" + camera_name_[i] + "/depth/points";
|
||||
color_point_cloud_topics_[i] = "/" + camera_name_[i] + "/depth_registered/points";
|
||||
}
|
||||
}
|
||||
|
||||
void color_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->step << "color is subed " << index << "is subed");
|
||||
}
|
||||
void depth_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->step << "depth is subed " << index << "is subed");
|
||||
}
|
||||
void left_ir_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->step << "left_ir is subed " << index << "is subed");
|
||||
}
|
||||
void right_ir_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->step << "right_ir is subed " << index << "is subed");
|
||||
}
|
||||
void depth_point_cloud_Callback(std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg,
|
||||
size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->point_step << "depth_point_cloud is subed " << index << "is subed");
|
||||
}
|
||||
void color_point_cloud_Callback(std::shared_ptr<const sensor_msgs::msg::PointCloud2> msg,
|
||||
size_t index) {
|
||||
std::lock_guard<std::mutex> lock(image_mutex_);
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
rclcpp::get_logger("StartBenchmark"),
|
||||
"time is : " << msg->point_step << "color_point_cloud is subed " << index << "is subed");
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace tools
|
||||
} // namespace orbbec_camera
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::tools::StartBenchmark)
|
||||
Reference in New Issue
Block a user