feat(gemini2): add OrbbecSDK_ROS2/
This commit is contained in:
X-lanni
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,123 @@
|
||||
// Copyright 2023 Intel Corporation. All Rights Reserved.
|
||||
//
|
||||
// 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.
|
||||
|
||||
// DESCRIPTION: #
|
||||
// ------------ #
|
||||
// This tool created a node which can be used to calulate the specified topic's latency.
|
||||
// Input parameters:
|
||||
// - topic_name : <String>
|
||||
// - topic to which latency need to be calculated
|
||||
// - topic_type : <String>
|
||||
// - Message type of the topic.
|
||||
// - Valid inputs: {'image','points','imu','metadata','camera_info','rgbd','imu_info','tf'}
|
||||
// Note:
|
||||
// - This tool doesn't support calulating latency for extrinsic topics.
|
||||
// Because, those topics doesn't have timestamp in it and this tool uses
|
||||
// that timestamp as an input to calculate the latency.
|
||||
//
|
||||
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <chrono>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
#include "frame_latency.hpp"
|
||||
|
||||
namespace orbbec_camera {
|
||||
|
||||
FrameLatencyNode::FrameLatencyNode(const std::string& node_name, const std::string& ns,
|
||||
const rclcpp::NodeOptions& node_options)
|
||||
: Node(node_name, ns, node_options), logger_(this->get_logger()) {}
|
||||
|
||||
std::string topic_name = "/camera/color/image_raw";
|
||||
std::string topic_type = "image";
|
||||
|
||||
template <typename MsgType>
|
||||
void FrameLatencyNode::createListener(const std::string& topic_name,
|
||||
const rmw_qos_profile_t qos_profile) {
|
||||
RCLCPP_INFO_STREAM(logger_, "createListener");
|
||||
using namespace std::chrono_literals;
|
||||
timer_ = this->create_wall_timer(1s, [this, topic_name=topic_name]() {
|
||||
// print fps
|
||||
RCLCPP_INFO_STREAM(logger_, "topic: " << topic_name << " fps: " << frame_count_ / 1.0);
|
||||
frame_count_ = 0;
|
||||
});
|
||||
sub_ = this->create_subscription<MsgType>(
|
||||
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile),
|
||||
[&, this](const std::shared_ptr<MsgType> msg) {
|
||||
rclcpp::Time curr_time = this->get_clock()->now();
|
||||
auto latency = (curr_time - msg->header.stamp).seconds();
|
||||
frame_count_++;
|
||||
RCLCPP_INFO_STREAM_THROTTLE(logger_, *this->get_clock(), 1000.0,
|
||||
"Got msg with "
|
||||
<< msg->header.frame_id << " frame id at address 0x"
|
||||
<< std::hex << reinterpret_cast<std::uintptr_t>(msg.get())
|
||||
<< std::dec << " with latency of " << latency << " [sec]");
|
||||
});
|
||||
}
|
||||
|
||||
void FrameLatencyNode::createTFListener(const std::string& topic_name,
|
||||
const rmw_qos_profile_t qos_profile) {
|
||||
sub_ = this->create_subscription<tf2_msgs::msg::TFMessage>(
|
||||
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile),
|
||||
[&, this](const std::shared_ptr<tf2_msgs::msg::TFMessage> msg) {
|
||||
rclcpp::Time curr_time = this->get_clock()->now();
|
||||
auto latency = (curr_time - msg->transforms.back().header.stamp).seconds();
|
||||
RCLCPP_INFO_STREAM_THROTTLE(
|
||||
logger_, *this->get_clock(), 1000.0,
|
||||
"Got msg with " << msg->transforms.back().header.frame_id << " frame id at address 0x"
|
||||
<< std::hex << reinterpret_cast<std::uintptr_t>(msg.get()) << std::dec
|
||||
<< " with latency of " << latency << " [sec]");
|
||||
});
|
||||
}
|
||||
|
||||
FrameLatencyNode::FrameLatencyNode(const rclcpp::NodeOptions& node_options)
|
||||
: Node("frame_latency", "/", node_options), logger_(this->get_logger()) {
|
||||
RCLCPP_INFO_STREAM(logger_, "frame_latency node is UP!");
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_,
|
||||
"Intra-Process is " << (this->get_node_options().use_intra_process_comms() ? "ON" : "OFF"));
|
||||
|
||||
topic_name = this->declare_parameter("topic_name", topic_name);
|
||||
topic_type = this->declare_parameter("topic_type", topic_type);
|
||||
|
||||
RCLCPP_INFO_STREAM(logger_, "Subscribing to Topic: " << topic_name);
|
||||
|
||||
if (topic_type == "image") {
|
||||
createListener<sensor_msgs::msg::Image>(topic_name, rmw_qos_profile_default);
|
||||
} else if (topic_type == "points") {
|
||||
createListener<sensor_msgs::msg::PointCloud2>(topic_name, rmw_qos_profile_default);
|
||||
} else if (topic_type == "imu") {
|
||||
createListener<sensor_msgs::msg::Imu>(topic_name, rmw_qos_profile_sensor_data);
|
||||
} else if (topic_type == "metadata") {
|
||||
createListener<orbbec_camera_msgs::msg::Metadata>(topic_name, rmw_qos_profile_default);
|
||||
} else if (topic_type == "camera_info") {
|
||||
createListener<sensor_msgs::msg::CameraInfo>(topic_name, rmw_qos_profile_default);
|
||||
} else if (topic_type == "rgbd") {
|
||||
createListener<orbbec_camera_msgs::msg::RGBD>(topic_name, rmw_qos_profile_default);
|
||||
} else if (topic_type == "imu_info") {
|
||||
createListener<orbbec_camera_msgs::msg::IMUInfo>(topic_name, rmw_qos_profile_default);
|
||||
} else if (topic_type == "tf") {
|
||||
createTFListener(topic_name, rmw_qos_profile_default);
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(logger_, "Specified message type '" << topic_type << "' is not supported");
|
||||
}
|
||||
}
|
||||
} // namespace orbbec_camera
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::FrameLatencyNode)
|
||||
@@ -0,0 +1,55 @@
|
||||
// Copyright 2023 Intel Corporation. All Rights Reserved.
|
||||
//
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "sensor_msgs/msg/image.hpp"
|
||||
#include "sensor_msgs/msg/imu.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
|
||||
#include <diagnostic_updater/diagnostic_updater.hpp>
|
||||
#include <diagnostic_updater/publisher.hpp>
|
||||
#include "orbbec_camera_msgs/msg/imu_info.hpp"
|
||||
#include "orbbec_camera_msgs/msg/extrinsics.hpp"
|
||||
#include "orbbec_camera_msgs/msg/metadata.hpp"
|
||||
#include "orbbec_camera_msgs/msg/rgbd.hpp"
|
||||
#include <sensor_msgs/image_encodings.hpp>
|
||||
#include <sensor_msgs/msg/camera_info.hpp>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <tf2_msgs/msg/tf_message.hpp>
|
||||
|
||||
namespace orbbec_camera {
|
||||
class FrameLatencyNode : public rclcpp::Node {
|
||||
public:
|
||||
explicit FrameLatencyNode(const rclcpp::NodeOptions& node_options =
|
||||
rclcpp::NodeOptions().use_intra_process_comms(true));
|
||||
|
||||
FrameLatencyNode(const std::string& node_name, const std::string& ns,
|
||||
const rclcpp::NodeOptions& node_options =
|
||||
rclcpp::NodeOptions().use_intra_process_comms(true));
|
||||
|
||||
template <typename MsgType>
|
||||
void createListener(const std::string& topic_name, rmw_qos_profile_t qos_profile);
|
||||
|
||||
void createTFListener(const std::string& topic_name, rmw_qos_profile_t qos_profile);
|
||||
|
||||
private:
|
||||
std::shared_ptr<void> sub_ = nullptr;
|
||||
|
||||
rclcpp::Logger logger_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
size_t frame_count_ = 0;
|
||||
};
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,83 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <orbbec_camera/ob_camera_node_driver.h>
|
||||
#include <orbbec_camera/ob_camera_node.h>
|
||||
#include <memory>
|
||||
#include <magic_enum/magic_enum.hpp>
|
||||
#include <iostream>
|
||||
|
||||
using namespace orbbec_camera;
|
||||
|
||||
std::shared_ptr<ob::Device> initializeDevice(std::shared_ptr<ob::Pipeline> pipeline) {
|
||||
auto device = pipeline->getDevice();
|
||||
if (!device) {
|
||||
std::cout << "No device found" << std::endl;
|
||||
return nullptr;
|
||||
}
|
||||
return device;
|
||||
}
|
||||
|
||||
void listSensorProfiles(const std::shared_ptr<ob::Device>& device) {
|
||||
auto sensor_list = device->getSensorList();
|
||||
for (size_t i = 0; i < sensor_list->count(); i++) {
|
||||
auto sensor = sensor_list->getSensor(i);
|
||||
auto profile_list = sensor->getStreamProfileList();
|
||||
for (size_t j = 0; j < profile_list->count(); j++) {
|
||||
auto origin_profile = profile_list->getProfile(j);
|
||||
if (sensor->type() == OB_SENSOR_COLOR || sensor->type() == OB_SENSOR_DEPTH ||
|
||||
sensor->type() == OB_SENSOR_IR || sensor->type() == OB_SENSOR_IR_LEFT ||
|
||||
sensor->type() == OB_SENSOR_IR_RIGHT) {
|
||||
auto profile = origin_profile->as<ob::VideoStreamProfile>();
|
||||
std::cout << magic_enum::enum_name(sensor->type()) << " profile: " << profile->width()
|
||||
<< "x" << profile->height() << " " << profile->fps() << "fps "
|
||||
<< magic_enum::enum_name(profile->format()) << std::endl;
|
||||
} else if (sensor->type() == OB_SENSOR_ACCEL) {
|
||||
auto profile = origin_profile->as<ob::AccelStreamProfile>();
|
||||
std::cout << magic_enum::enum_name(sensor->type()) << " profile: " << profile->sampleRate()
|
||||
<< " full scale_range " << profile->fullScaleRange() << std::endl;
|
||||
} else if (sensor->type() == OB_SENSOR_GYRO) {
|
||||
auto profile = origin_profile->as<ob::GyroStreamProfile>();
|
||||
std::cout << magic_enum::enum_name(sensor->type()) << " profile: " << profile->sampleRate()
|
||||
<< " full scale_range " << profile->fullScaleRange() << std::endl;
|
||||
} else {
|
||||
std::cout << "Unknown profile: " << magic_enum::enum_name(sensor->type()) << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void printDeviceProperties(const std::shared_ptr<ob::Device>& device) {
|
||||
if (!device->isPropertySupported(OB_STRUCT_CURRENT_DEPTH_ALG_MODE, OB_PERMISSION_READ_WRITE)) {
|
||||
std::cout << "Current device not support depth work mode!" << std::endl;
|
||||
return;
|
||||
}
|
||||
auto current_depth_mode = device->getCurrentDepthWorkMode();
|
||||
std::cout << "Current depth mode: " << current_depth_mode.name << std::endl;
|
||||
auto depth_mode_list = device->getDepthWorkModeList();
|
||||
std::cout << "Depth mode list: " << std::endl;
|
||||
for (uint32_t i = 0; i < depth_mode_list->count(); i++) {
|
||||
std::cout << "Depth_mode_list[" << i << "]: " << (*depth_mode_list)[i].name << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void printPreset(const std::shared_ptr<ob::Device>& device) {
|
||||
auto preset_list = device->getAvailablePresetList();
|
||||
if (!preset_list || preset_list->count() == 0) {
|
||||
return;
|
||||
}
|
||||
std::cout << "Preset list:" << std::endl;
|
||||
for (uint32_t i = 0; i < preset_list->count(); i++) {
|
||||
auto name = preset_list->getName(i);
|
||||
std::cout << "Preset list[" << i << "]: " << name << std::endl;
|
||||
}
|
||||
}
|
||||
int main() {
|
||||
auto pipeline = std::make_shared<ob::Pipeline>();
|
||||
auto device = initializeDevice(pipeline);
|
||||
if (!device) {
|
||||
return -1; // Device initialization failed
|
||||
}
|
||||
listSensorProfiles(device);
|
||||
printDeviceProperties(device);
|
||||
printPreset(device);
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
/*******************************************************************************
|
||||
* 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 <orbbec_camera/ob_camera_node.h>
|
||||
|
||||
int main() {
|
||||
std::shared_ptr<ob::Pipeline> pipeline = std::make_shared<ob::Pipeline>();
|
||||
auto device = pipeline->getDevice();
|
||||
if (!device) {
|
||||
std::cout << "No device found" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
if (!device->isPropertySupported(OB_STRUCT_CURRENT_DEPTH_ALG_MODE, OB_PERMISSION_READ_WRITE)) {
|
||||
std::cout << "Current device not support depth work mode!" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
auto current_depth_mode = device->getCurrentDepthWorkMode();
|
||||
std::cout << "current depth mode: " << current_depth_mode.name << std::endl;
|
||||
auto depth_mode_list = device->getDepthWorkModeList();
|
||||
std::cout << "depth mode list: " << std::endl;
|
||||
for (uint32_t i = 0; i < depth_mode_list->count(); i++) {
|
||||
std::cout << "depth_mode_list[" << i << "]: " << (*depth_mode_list)[i].name;
|
||||
|
||||
std::cout << std::endl;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
/*******************************************************************************
|
||||
* 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 <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <orbbec_camera/ob_camera_node_driver.h>
|
||||
#include <orbbec_camera/utils.h>
|
||||
|
||||
int main() {
|
||||
try {
|
||||
auto context = std::make_unique<ob::Context>();
|
||||
context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
|
||||
auto list = context->queryDeviceList();
|
||||
for (size_t i = 0; i < list->deviceCount(); i++) {
|
||||
auto device = list->getDevice(i);
|
||||
auto device_info = device->getDeviceInfo();
|
||||
std::string serial = device_info->serialNumber();
|
||||
std::string uid = device_info->uid();
|
||||
auto usb_port = orbbec_camera::parseUsbPort(uid);
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "serial: " << serial);
|
||||
RCLCPP_INFO_STREAM(rclcpp::get_logger("list_device_node"), "usb port: " << usb_port);
|
||||
}
|
||||
} catch (const std::exception &e) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("list_device_node"), e.what());
|
||||
} catch (ob::Error &e) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("list_device_node"), e.getMessage());
|
||||
} catch (...) {
|
||||
RCLCPP_ERROR_STREAM(rclcpp::get_logger("list_device_node"), "unknown error");
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
#include "topic_statistics.hpp"
|
||||
|
||||
int main(int argc, char * argv[]) {
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<orbbec_camera::tools::TopicStatistics>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,181 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
#include <statistics_msgs/msg/metrics_message.hpp>
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
|
||||
namespace orbbec_camera {
|
||||
namespace tools {
|
||||
|
||||
class TopicStatistics : public rclcpp::Node {
|
||||
public:
|
||||
TopicStatistics() : Node("topic_statistics") {
|
||||
this->declare_parameter("image_topic", "/camera/color/image_raw");
|
||||
this->declare_parameter("statistics_topic", "/statistics");
|
||||
|
||||
image_topic_ = this->get_parameter("image_topic").as_string();
|
||||
statistics_topic_ = this->get_parameter("statistics_topic").as_string();
|
||||
|
||||
RCLCPP_INFO(get_logger(), "TopicStatistics starting up");
|
||||
initialize();
|
||||
initialize_csv();
|
||||
}
|
||||
|
||||
void initialize() {
|
||||
// Create a subscriber to the image topic
|
||||
auto sub_opt = rclcpp::SubscriptionOptions();
|
||||
sub_opt.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable;
|
||||
sub_opt.topic_stats_options.publish_topic = statistics_topic_;
|
||||
sub_opt.topic_stats_options.publish_period = std::chrono::milliseconds(1000);
|
||||
|
||||
image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
|
||||
image_topic_, 10, std::bind(&TopicStatistics::image_callback, this, std::placeholders::_1),
|
||||
sub_opt);
|
||||
|
||||
// Create a subscriber to the statistics topic
|
||||
statistics_sub_ = this->create_subscription<statistics_msgs::msg::MetricsMessage>(
|
||||
statistics_topic_, 10,
|
||||
std::bind(&TopicStatistics::statistics_callback, this, std::placeholders::_1));
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Subscribed to image topic: %s", image_topic_.c_str());
|
||||
RCLCPP_INFO(get_logger(), "Subscribed to statistics topic: %s", statistics_topic_.c_str());
|
||||
}
|
||||
|
||||
void initialize_csv() {
|
||||
// Get the current working directory
|
||||
std::filesystem::path cwd = std::filesystem::current_path();
|
||||
csv_path_ = cwd.string() + "/statistics.csv";
|
||||
|
||||
// Open the file in output mode, which will create/overwrite the file
|
||||
std::ofstream csv_file(csv_path_, std::ios::out);
|
||||
if (csv_file.is_open()) {
|
||||
// Write the header to the CSV file
|
||||
csv_file << "\"_time\",\"message_type\",\"min\",\"avg\",\"max\"\n";
|
||||
csv_file.close();
|
||||
csv_initialized_ = true;
|
||||
} else {
|
||||
RCLCPP_ERROR(get_logger(), "Unable to open statistics.csv for writing");
|
||||
}
|
||||
}
|
||||
|
||||
void write_statistics_to_csv(const statistics_msgs::msg::MetricsMessage& msg) {
|
||||
if (!csv_initialized_) {
|
||||
// If the CSV hasn't been initialized, initialize it
|
||||
initialize_csv();
|
||||
}
|
||||
|
||||
std::ofstream csv_file(csv_path_, std::ios::app);
|
||||
if (csv_file.is_open()) {
|
||||
// Extract the timestamp from the MetricsMessage
|
||||
auto time_ns = rclcpp::Time(msg.window_stop);
|
||||
auto time_point = std::chrono::system_clock::from_time_t(time_ns.seconds());
|
||||
std::time_t time_t_value = std::chrono::system_clock::to_time_t(time_point);
|
||||
std::tm* tm_value = std::gmtime(&time_t_value);
|
||||
char time_str[20];
|
||||
std::strftime(time_str, sizeof(time_str), "%Y-%m-%d %H:%M:%S", tm_value);
|
||||
|
||||
// Determine the message type (e.g., "age" or "period")
|
||||
std::string message_type;
|
||||
if (msg.metrics_source.find("_age") != std::string::npos) {
|
||||
message_type = "age";
|
||||
} else if (msg.metrics_source.find("_period") != std::string::npos) {
|
||||
message_type = "period";
|
||||
} else {
|
||||
message_type = "unknown";
|
||||
}
|
||||
|
||||
// Write the timestamp and message type to the CSV file
|
||||
csv_file << "\"" << time_str << "\",\"" << message_type << "\",";
|
||||
|
||||
// Variables to store the min, avg, and max values
|
||||
double min = 0, avg = 0, max = 0;
|
||||
|
||||
// Iterate through the statistics and assign values based on the type
|
||||
for (const auto& statistic : msg.statistics) {
|
||||
switch (statistic.data_type) {
|
||||
case 1: // avg
|
||||
avg = statistic.data;
|
||||
break;
|
||||
case 2: // min
|
||||
min = statistic.data;
|
||||
break;
|
||||
case 3: // max
|
||||
max = statistic.data;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Write the min, avg, and max values to the CSV file
|
||||
csv_file << min << " ms," << avg << " ms," << max << " ms\n";
|
||||
csv_file.close();
|
||||
} else {
|
||||
RCLCPP_ERROR(get_logger(), "Unable to open statistics.csv for writing");
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
void image_callback(const sensor_msgs::msg::Image::UniquePtr msg) {
|
||||
image_count_++;
|
||||
image_size_ += msg->data.size();
|
||||
RCLCPP_DEBUG(get_logger(), "Received image %d, size: %zu bytes", image_count_,
|
||||
msg->data.size());
|
||||
}
|
||||
|
||||
void statistics_callback(const statistics_msgs::msg::MetricsMessage::UniquePtr msg) {
|
||||
RCLCPP_INFO(get_logger(), "Statistics received:\n%s", metrics_message_to_string(*msg).c_str());
|
||||
write_statistics_to_csv(*msg);
|
||||
}
|
||||
|
||||
std::string metrics_message_to_string(const statistics_msgs::msg::MetricsMessage& msg) {
|
||||
std::stringstream ss;
|
||||
ss << "Metric name: " << msg.metrics_source << " source: " << msg.measurement_source_name
|
||||
<< " unit: " << msg.unit;
|
||||
ss << "\nWindow start: " << msg.window_start.nanosec << " end: " << msg.window_stop.nanosec;
|
||||
|
||||
for (const auto& statistic : msg.statistics) {
|
||||
ss << "\n"
|
||||
<< statistic_type_to_string(statistic.data_type) << ": " << std::to_string(statistic.data);
|
||||
}
|
||||
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
std::string statistic_type_to_string(int8_t type) {
|
||||
switch (type) {
|
||||
case 1:
|
||||
return "avg";
|
||||
case 2:
|
||||
return "min";
|
||||
case 3:
|
||||
return "max";
|
||||
case 4:
|
||||
return "std_dev";
|
||||
case 5:
|
||||
return "sample_count";
|
||||
default:
|
||||
return "unknown";
|
||||
}
|
||||
}
|
||||
|
||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
|
||||
rclcpp::Subscription<statistics_msgs::msg::MetricsMessage>::SharedPtr statistics_sub_;
|
||||
|
||||
std::string image_topic_;
|
||||
std::string statistics_topic_;
|
||||
int image_count_ = 0;
|
||||
size_t image_size_ = 0;
|
||||
std::string csv_path_;
|
||||
bool csv_initialized_ = false;
|
||||
};
|
||||
|
||||
} // namespace tools
|
||||
} // namespace orbbec_camera
|
||||
Reference in New Issue
Block a user