i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-13 19:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Adapted G2R

Browse Source
This commit is contained in:
Joe Dong
2024-03-15 10:24:38 +08:00
parent e5db48d03a
commit ee0a2f7011
6 changed files with 378 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+19 -4
View File
@@ -51,6 +51,7 @@
#include "orbbec_camera_msgs/srv/get_device_info.hpp" #include "orbbec_camera_msgs/srv/get_device_info.hpp"
#include "orbbec_camera_msgs/msg/extrinsics.hpp" #include "orbbec_camera_msgs/msg/extrinsics.hpp"
#include "orbbec_camera_msgs/msg/metadata.hpp" #include "orbbec_camera_msgs/msg/metadata.hpp"
#include "orbbec_camera_msgs/msg/imu_info.hpp"
#include "orbbec_camera_msgs/srv/get_int32.hpp" #include "orbbec_camera_msgs/srv/get_int32.hpp"
#include "orbbec_camera_msgs/srv/get_string.hpp" #include "orbbec_camera_msgs/srv/get_string.hpp"
#include "orbbec_camera_msgs/srv/set_int32.hpp" #include "orbbec_camera_msgs/srv/set_int32.hpp"
@@ -137,16 +138,17 @@ class OBCameraNode {
void clean(); void clean();
void startStreams(); void startStreams();
void startIMU();
void startIMUSyncStream();
void startIMU();
private: private:
struct IMUData { struct IMUData {
IMUData() = default; IMUData() = default;
IMUData(stream_index_pair stream, Eigen::Vector3d data, double timestamp) IMUData(stream_index_pair stream, Eigen::Vector3d data, double timestamp)
: stream_(std::move(stream)), data_(std::move(data)), timestamp_(timestamp) {} : stream_(std::move(stream)), data_(std::move(data)), timestamp_(timestamp) {}
bool isSet() const { return timestamp_ >= 0; } [[nodiscard]] bool isSet() const { return timestamp_ >= 0; }
stream_index_pair stream_{}; stream_index_pair stream_{};
Eigen::Vector3d data_{}; Eigen::Vector3d data_{};
double timestamp_ = -1; // in nanoseconds double timestamp_ = -1; // in nanoseconds
@@ -282,6 +284,9 @@ class OBCameraNode {
void onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame, void onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
const stream_index_pair& stream_index); const stream_index_pair& stream_index);
void publishMetadata(const std::shared_ptr<ob::Frame>& frame,
const stream_index_pair& stream_index, const std_msgs::msg::Header& header);
void onNewColorFrameCallback(); void onNewColorFrameCallback();
void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image, void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
@@ -304,6 +309,8 @@ class OBCameraNode {
bool setupFormatConvertType(OBFormat format); bool setupFormatConvertType(OBFormat format);
orbbec_camera_msgs::msg::IMUInfo createIMUInfo(const stream_index_pair& stream_index);
private: private:
rclcpp::Node* node_ = nullptr; rclcpp::Node* node_ = nullptr;
std::shared_ptr<ob::Device> device_ = nullptr; std::shared_ptr<ob::Device> device_ = nullptr;
@@ -321,6 +328,13 @@ class OBCameraNode {
std::map<stream_index_pair, ob_camera_intrinsic> stream_intrinsics_; std::map<stream_index_pair, ob_camera_intrinsic> stream_intrinsics_;
std::map<stream_index_pair, sensor_msgs::msg::CameraInfo> camera_infos_; std::map<stream_index_pair, sensor_msgs::msg::CameraInfo> camera_infos_;
std::map<stream_index_pair, OBCameraParam> ob_camera_param_; std::map<stream_index_pair, OBCameraParam> ob_camera_param_;
std::map<stream_index_pair, OBExtrinsic> depth_to_other_extrinsics_;
std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::Extrinsics>::SharedPtr>
depth_to_other_extrinsics_publishers_;
std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
metadata_publishers_;
std::map<stream_index_pair, rclcpp::Publisher<orbbec_camera_msgs::msg::IMUInfo>::SharedPtr>
imu_info_publishers_;
std::map<stream_index_pair, int> width_; std::map<stream_index_pair, int> width_;
std::map<stream_index_pair, int> height_; std::map<stream_index_pair, int> height_;
std::map<stream_index_pair, int> fps_; std::map<stream_index_pair, int> fps_;
@@ -337,6 +351,7 @@ class OBCameraNode {
std::map<stream_index_pair, std::vector<std::shared_ptr<ob::VideoStreamProfile>>> std::map<stream_index_pair, std::vector<std::shared_ptr<ob::VideoStreamProfile>>>
supported_profiles_; supported_profiles_;
std::map<stream_index_pair, std::shared_ptr<ob::StreamProfile>> stream_profile_; std::map<stream_index_pair, std::shared_ptr<ob::StreamProfile>> stream_profile_;
const stream_index_pair base_stream_ = DEPTH;
std::map<stream_index_pair, uint32_t> seq_; std::map<stream_index_pair, uint32_t> seq_;
std::map<stream_index_pair, cv::Mat> images_; std::map<stream_index_pair, cv::Mat> images_;
std::map<stream_index_pair, std::string> encoding_; std::map<stream_index_pair, std::string> encoding_;
@@ -445,7 +460,7 @@ class OBCameraNode {
uint8_t* rgb_buffer_ = nullptr; uint8_t* rgb_buffer_ = nullptr;
bool is_color_frame_decoded_ = false; bool is_color_frame_decoded_ = false;
std::mutex device_lock_; std::mutex device_lock_;
//For color // For color
std::queue<std::shared_ptr<ob::FrameSet>> colorFrameQueue_; std::queue<std::shared_ptr<ob::FrameSet>> colorFrameQueue_;
std::shared_ptr<std::thread> colorFrameThread_ = nullptr; std::shared_ptr<std::thread> colorFrameThread_ = nullptr;
std::mutex colorFrameMtx_; std::mutex colorFrameMtx_;
orbbec_camera/include/orbbec_camera/utils.h
+5 -1
View File
@@ -47,7 +47,9 @@ std::ostream& operator<<(std::ostream& os, const OBCameraParam& rhs);
orbbec_camera_msgs::msg::Extrinsics obExtrinsicsToMsg(const OBD2CTransform& extrinsics, orbbec_camera_msgs::msg::Extrinsics obExtrinsicsToMsg(const OBD2CTransform& extrinsics,
const std::string& frame_id); const std::string& frame_id);
rclcpp::Time frameTimeStampToROSTime(uint64_t ms); rclcpp::Time fromMsToROSTime(uint64_t ms);
rclcpp::Time fromUsToROSTime(uint64_t us);
std::string getObSDKVersion(); std::string getObSDKVersion();
@@ -80,4 +82,6 @@ std::string parseUsbPort(const std::string& line);
bool isValidJPEG(const std::shared_ptr<ob::ColorFrame>& frame); bool isValidJPEG(const std::shared_ptr<ob::ColorFrame>& frame);
std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type);
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera/src/ob_camera_node.cpp
+265 -148
View File
@@ -231,6 +231,7 @@ void OBCameraNode::setupDevices() {
} }
void OBCameraNode::setupProfiles() { void OBCameraNode::setupProfiles() {
// Image stream
for (const auto &elem : IMAGE_STREAMS) { for (const auto &elem : IMAGE_STREAMS) {
if (enable_stream_[elem]) { if (enable_stream_[elem]) {
const auto &sensor = sensors_[elem]; const auto &sensor = sensors_[elem];
@@ -294,6 +295,34 @@ void OBCameraNode::setupProfiles() {
<< "Format: " << magic_enum::enum_name(selected_profile->format())); << "Format: " << magic_enum::enum_name(selected_profile->format()));
} }
} }
// IMU
for (const auto &stream_index : HID_STREAMS) {
if (!enable_stream_[stream_index]) {
continue;
}
try {
auto profile_list = sensors_[stream_index]->getStreamProfileList();
if (stream_index == ACCEL) {
auto full_scale_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
auto sample_rate = sampleRateFromString(imu_rate_[stream_index]);
auto profile = profile_list->getAccelStreamProfile(full_scale_range, sample_rate);
stream_profile_[stream_index] = profile;
} else if (stream_index == GYRO) {
auto full_scale_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
auto sample_rate = sampleRateFromString(imu_rate_[stream_index]);
auto profile = profile_list->getGyroStreamProfile(full_scale_range, sample_rate);
stream_profile_[stream_index] = profile;
}
RCLCPP_INFO_STREAM(logger_, "stream " << stream_name_[stream_index] << " full scale range "
<< imu_range_[stream_index] << " sample rate "
<< imu_rate_[stream_index]);
} catch (const ob::Error &e) {
RCLCPP_INFO_STREAM(logger_, "Failed to setup << " << stream_name_[stream_index]
<< " profile: " << e.getMessage());
enable_stream_[stream_index] = false;
stream_profile_[stream_index] = nullptr;
}
}
} }
void OBCameraNode::startStreams() { void OBCameraNode::startStreams() {
@@ -329,97 +358,66 @@ void OBCameraNode::startStreams() {
pipeline_started_.store(true); pipeline_started_.store(true);
} }
void OBCameraNode::startIMUSyncStream() {
if (imuPipeline_ != nullptr) {
imuPipeline_.reset();
}
imuPipeline_ = std::make_unique<ob::Pipeline>(device_);
if (imu_sync_output_start_) {
return;
}
// ACCEL
auto accelProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_ACCEL);
auto accel_range = fullAccelScaleRangeFromString(imu_range_[ACCEL]);
auto accel_rate = sampleRateFromString(imu_rate_[ACCEL]);
auto accelProfile = accelProfiles->getAccelStreamProfile(accel_range, accel_rate);
// GYRO
auto gyroProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_GYRO);
auto gyro_range = fullGyroScaleRangeFromString(imu_range_[GYRO]);
auto gyro_rate = sampleRateFromString(imu_rate_[GYRO]);
auto gyroProfile = gyroProfiles->getGyroStreamProfile(gyro_range, gyro_rate);
std::shared_ptr<ob::Config> imuConfig = std::make_shared<ob::Config>();
imuConfig->enableStream(accelProfile);
imuConfig->enableStream(gyroProfile);
imuPipeline_->enableFrameSync();
imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::Frame> frame) {
auto frameSet = frame->as<ob::FrameSet>();
auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
if (aFrame && gFrame) {
onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
}
});
imu_sync_output_start_ = true;
if (!imu_sync_output_start_) {
RCLCPP_ERROR_STREAM(
logger_, "Failed to start IMU stream, please check the imu_rate and imu_range parameters.");
} else {
RCLCPP_INFO_STREAM(
logger_, "start accel stream with range: " << fullAccelScaleRangeToString(accel_range)
<< ",rate:" << sampleRateToString(accel_rate)
<< ", and start gyro stream with range:"
<< fullGyroScaleRangeToString(gyro_range)
<< ",rate:" << sampleRateToString(gyro_rate));
}
}
void OBCameraNode::startIMU() { void OBCameraNode::startIMU() {
if (enable_sync_output_accel_gyro_) { if (enable_sync_output_accel_gyro_) {
if (imuPipeline_ != nullptr) { startIMUSyncStream();
imuPipeline_.reset();
}
imuPipeline_ = std::make_unique<ob::Pipeline>(device_);
if (imu_sync_output_start_) {
return;
}
// ACCEL
auto accelProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_ACCEL);
auto accel_range = fullAccelScaleRangeFromString(imu_range_[ACCEL]);
auto accel_rate = sampleRateFromString(imu_rate_[ACCEL]);
auto accelProfile = accelProfiles->getAccelStreamProfile(accel_range, accel_rate);
// GYRO
auto gyroProfiles = imuPipeline_->getStreamProfileList(OB_SENSOR_GYRO);
auto gyro_range = fullGyroScaleRangeFromString(imu_range_[GYRO]);
auto gyro_rate = sampleRateFromString(imu_rate_[GYRO]);
auto gyroProfile = gyroProfiles->getGyroStreamProfile(gyro_range, gyro_rate);
std::shared_ptr<ob::Config> imuConfig = std::make_shared<ob::Config>();
imuConfig->enableStream(accelProfile);
imuConfig->enableStream(gyroProfile);
imuPipeline_->enableFrameSync();
imuPipeline_->start(imuConfig, [&](std::shared_ptr<ob::Frame> frame) {
auto frameSet = frame->as<ob::FrameSet>();
auto aFrame = frameSet->getFrame(OB_FRAME_ACCEL);
auto gFrame = frameSet->getFrame(OB_FRAME_GYRO);
if (aFrame && gFrame) {
onNewIMUFrameSyncOutputCallback(aFrame, gFrame);
}
});
imu_sync_output_start_ = true;
if (!imu_sync_output_start_) {
RCLCPP_ERROR_STREAM(
logger_,
"Failed to start IMU stream, please check the imu_rate and imu_range parameters.");
} else {
RCLCPP_INFO_STREAM(
logger_, "start accel stream with range: " << fullAccelScaleRangeToString(accel_range)
<< ",rate:" << sampleRateToString(accel_rate)
<< ", and start gyro stream with range:"
<< fullGyroScaleRangeToString(gyro_range)
<< ",rate:" << sampleRateToString(gyro_rate));
}
} else { } else {
for (const auto &stream_index : HID_STREAMS) { for (const auto &stream_index : HID_STREAMS) {
if (enable_stream_[stream_index] && !imu_started_[stream_index]) { if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
CHECK(sensors_.count(stream_index)); auto imu_profile = stream_profile_[stream_index];
auto profile_list = sensors_[stream_index]->getStreamProfileList(); CHECK(imu_profile);
for (size_t i = 0; i < profile_list->count(); i++) { RCLCPP_INFO_STREAM(logger_, "start " << stream_name_[stream_index] << " stream");
auto item = profile_list->getProfile(i); sensors_[stream_index]->start(
if (stream_index == ACCEL) { imu_profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
auto profile = item->as<ob::AccelStreamProfile>(); onNewIMUFrameCallback(frame, stream_index);
auto accel_rate = sampleRateFromString(imu_rate_[stream_index]); });
auto accel_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
if (profile->fullScaleRange() == accel_range && profile->sampleRate() == accel_rate) {
sensors_[stream_index]->start(
profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
onNewIMUFrameCallback(frame, stream_index);
});
imu_started_[stream_index] = true;
RCLCPP_INFO_STREAM(logger_, "start accel stream with "
<< magic_enum::enum_name(accel_range) << " range and "
<< magic_enum::enum_name(accel_rate) << " rate");
}
} else if (stream_index == GYRO) {
auto profile = item->as<ob::GyroStreamProfile>();
auto gyro_rate = sampleRateFromString(imu_rate_[stream_index]);
auto gyro_range = fullGyroScaleRangeFromString(imu_range_[stream_index]);
if (profile->fullScaleRange() == gyro_range && profile->sampleRate() == gyro_rate) {
sensors_[stream_index]->start(
profile, [this, stream_index](const std::shared_ptr<ob::Frame> &frame) {
onNewIMUFrameCallback(frame, stream_index);
});
RCLCPP_INFO_STREAM(logger_, "start gyro stream with "
<< magic_enum::enum_name(gyro_range) << " range and "
<< magic_enum::enum_name(gyro_rate) << " rate");
imu_started_[stream_index] = true;
}
}
}
}
}
for (const auto &stream_index : HID_STREAMS) {
if (enable_stream_[stream_index] && !imu_started_[stream_index]) {
RCLCPP_ERROR_STREAM(logger_, "Failed to start IMU stream: "
<< magic_enum::enum_name(stream_index.first)
<< ", please check the imu_rate and imu_range parameters");
} }
} }
} }
@@ -675,13 +673,23 @@ void OBCameraNode::setupPublishers() {
camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>( camera_info_publishers_[stream_index] = node_->create_publisher<CameraInfo>(
topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile), topic, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile)); camera_info_qos_profile));
metadata_publishers_[stream_index] = node_->create_publisher<orbbec_camera_msgs::msg::Metadata>(
name + "/metadata",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile));
} }
if (enable_sync_output_accel_gyro_) { if (enable_sync_output_accel_gyro_) {
std::string data_topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample"; std::string topic_name = stream_name_[GYRO] + "_" + stream_name_[ACCEL] + "/sample";
auto data_qos = getRMWQosProfileFromString(imu_qos_[GYRO]); auto data_qos = getRMWQosProfileFromString(imu_qos_[GYRO]);
imu_gyro_accel_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>( imu_gyro_accel_publisher_ = node_->create_publisher<sensor_msgs::msg::Imu>(
data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos)); topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
topic_name = stream_name_[GYRO] + "/imu_info";
imu_info_publishers_[GYRO] = node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
topic_name = stream_name_[ACCEL] + "/imu_info";
imu_info_publishers_[ACCEL] = node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
} else { } else {
for (const auto &stream_index : HID_STREAMS) { for (const auto &stream_index : HID_STREAMS) {
if (!enable_stream_[stream_index]) { if (!enable_stream_[stream_index]) {
@@ -691,8 +699,33 @@ void OBCameraNode::setupPublishers() {
auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]); auto data_qos = getRMWQosProfileFromString(imu_qos_[stream_index]);
imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>( imu_publishers_[stream_index] = node_->create_publisher<sensor_msgs::msg::Imu>(
data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos)); data_topic_name, rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
data_topic_name = stream_name_[stream_index] + "/imu_info";
imu_info_publishers_[stream_index] =
node_->create_publisher<orbbec_camera_msgs::msg::IMUInfo>(
data_topic_name,
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(data_qos), data_qos));
} }
} }
if (enable_stream_[DEPTH] && enable_stream_[INFRA0]) {
depth_to_other_extrinsics_publishers_[INFRA0] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_ir", rclcpp::QoS(1).transient_local());
}
if (enable_stream_[DEPTH] && enable_stream_[COLOR]) {
depth_to_other_extrinsics_publishers_[COLOR] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_color", rclcpp::QoS(1).transient_local());
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA1]) {
depth_to_other_extrinsics_publishers_[INFRA1] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_left_ir", rclcpp::QoS(1).transient_local());
}
if (enable_stream_[DEPTH] && enable_stream_[INFRA2]) {
depth_to_other_extrinsics_publishers_[INFRA2] =
node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
"/" + camera_name_ + "/depth_to_right_ir", rclcpp::QoS(1).transient_local());
}
} }
void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) { void OBCameraNode::publishPointCloud(const std::shared_ptr<ob::FrameSet> &frame_set) {
@@ -782,7 +815,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
} }
} }
} }
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp()); auto timestamp = fromMsToROSTime(depth_frame->systemTimeStamp());
if (!ordered_pc_) { if (!ordered_pc_) {
point_cloud_msg_.is_dense = true; point_cloud_msg_.is_dense = true;
point_cloud_msg_.width = valid_count; point_cloud_msg_.width = valid_count;
@@ -902,7 +935,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
} }
} }
} }
auto timestamp = frameTimeStampToROSTime(depth_frame->systemTimeStamp()); auto timestamp = fromMsToROSTime(depth_frame->systemTimeStamp());
if (!ordered_pc_) { if (!ordered_pc_) {
point_cloud_msg_.is_dense = true; point_cloud_msg_.is_dense = true;
point_cloud_msg_.width = valid_count; point_cloud_msg_.width = valid_count;
@@ -1109,9 +1142,10 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
return; return;
} }
bool has_subscriber = image_publishers_[stream_index].getNumSubscribers() > 0; bool has_subscriber = image_publishers_[stream_index].getNumSubscribers() > 0;
if (camera_info_publishers_[stream_index]->get_subscription_count() > 0) { has_subscriber =
has_subscriber = true; has_subscriber || camera_info_publishers_[stream_index]->get_subscription_count() > 0;
} has_subscriber =
has_subscriber || metadata_publishers_[stream_index]->get_subscription_count() > 0;
if (!has_subscriber) { if (!has_subscriber) {
return; return;
} }
@@ -1133,15 +1167,17 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
} }
int width = static_cast<int>(video_frame->width()); int width = static_cast<int>(video_frame->width());
int height = static_cast<int>(video_frame->height()); int height = static_cast<int>(video_frame->height());
auto frame_time_stamp = use_hardware_time_?video_frame->timeStamp():video_frame->systemTimeStamp(); auto timestamp = use_hardware_time_ ? fromUsToROSTime(video_frame->timeStampUs())
auto timestamp = frameTimeStampToROSTime(frame_time_stamp); : fromMsToROSTime(video_frame->systemTimeStamp());
if (!camera_param_) { if (!camera_param_) {
camera_param_ = pipeline_->getCameraParam(); camera_param_ = pipeline_->getCameraParam();
} }
auto &intrinsic = auto stream_profile = frame->getStreamProfile();
stream_index == COLOR ? camera_param_->rgbIntrinsic : camera_param_->depthIntrinsic; CHECK_NOTNULL(stream_profile);
auto &distortion = auto video_stream_profile = stream_profile->as<ob::VideoStreamProfile>();
stream_index == COLOR ? camera_param_->rgbDistortion : camera_param_->depthDistortion; CHECK_NOTNULL(video_stream_profile);
const auto& intrinsic = video_stream_profile->getIntrinsic();
const auto& distortion = video_stream_profile->getDistortion();
std::string frame_id = std::string frame_id =
depth_registration_ ? depth_aligned_frame_id_[stream_index] : optical_frame_id_[stream_index]; depth_registration_ ? depth_aligned_frame_id_[stream_index] : optical_frame_id_[stream_index];
auto camera_info = convertToCameraInfo(intrinsic, distortion, width); auto camera_info = convertToCameraInfo(intrinsic, distortion, width);
@@ -1155,10 +1191,6 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (image.empty() || image.cols != width || image.rows != height) { if (image.empty() || image.cols != width || image.rows != height) {
image.create(height, width, image_format_[stream_index]); image.create(height, width, image_format_[stream_index]);
} }
has_subscriber = image_publishers_[stream_index].getNumSubscribers() > 0;
if (!has_subscriber) {
return;
}
if (frame->type() == OB_FRAME_COLOR && !is_color_frame_decoded_) { if (frame->type() == OB_FRAME_COLOR && !is_color_frame_decoded_) {
RCLCPP_ERROR(logger_, "color frame is not decoded"); RCLCPP_ERROR(logger_, "color frame is not decoded");
return; return;
@@ -1178,12 +1210,40 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
image_msg->is_bigendian = false; image_msg->is_bigendian = false;
image_msg->step = width * unit_step_size_[stream_index]; image_msg->step = width * unit_step_size_[stream_index];
image_msg->header.frame_id = frame_id; image_msg->header.frame_id = frame_id;
publishMetadata(frame, stream_index, image_msg->header);
CHECK(image_publishers_.count(stream_index) > 0); CHECK(image_publishers_.count(stream_index) > 0);
image_publishers_[stream_index].publish(image_msg); image_publishers_[stream_index].publish(image_msg);
saveImageToFile(stream_index, image, image_msg); saveImageToFile(stream_index, image, image_msg);
} }
void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &frame,
const stream_index_pair &stream_index,
const std_msgs::msg::Header &header) {
if (metadata_publishers_.count(stream_index) == 0) {
return;
}
auto metadata_publisher = metadata_publishers_[stream_index];
if (metadata_publisher->get_subscription_count() == 0) {
return;
}
orbbec_camera_msgs::msg::Metadata metadata_msg;
metadata_msg.header = header;
nlohmann::json json_data;
for (int i = 0; i < OB_FRAME_METADATA_TYPE_COUNT; i++) {
auto meta_data_type = static_cast<OBFrameMetadataType>(i);
std::string field_name = metaDataTypeToString(meta_data_type);
if (!frame->hasMetadata(meta_data_type)) {
continue;
}
int64_t value = frame->getMetadataValue(meta_data_type);
json_data[field_name] = value;
}
metadata_msg.json_data = json_data.dump(2);
metadata_publisher->publish(metadata_msg);
}
void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &image, void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const cv::Mat &image,
const sensor_msgs::msg::Image::SharedPtr &image_msg) { const sensor_msgs::msg::Image::SharedPtr &image_msg) {
if (save_images_[stream_index]) { if (save_images_[stream_index]) {
@@ -1238,16 +1298,17 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
RCLCPP_ERROR_STREAM(logger_, "stream Accel Gryo publisher not initialized"); RCLCPP_ERROR_STREAM(logger_, "stream Accel Gryo publisher not initialized");
return; return;
} }
auto subscriber_count = imu_gyro_accel_publisher_->get_subscription_count(); bool has_subscriber = imu_gyro_accel_publisher_->get_subscription_count() > 0;
if (subscriber_count == 0) { has_subscriber = has_subscriber || imu_info_publishers_[GYRO]->get_subscription_count() > 0;
has_subscriber = has_subscriber || imu_info_publishers_[ACCEL]->get_subscription_count() > 0;
if (!has_subscriber) {
return; return;
} }
auto imu_msg = sensor_msgs::msg::Imu(); auto imu_msg = sensor_msgs::msg::Imu();
setDefaultIMUMessage(imu_msg); setDefaultIMUMessage(imu_msg);
imu_msg.header.frame_id = imu_optical_frame_id_; imu_msg.header.frame_id = imu_optical_frame_id_;
auto timestamp = frameTimeStampToROSTime(accelframe->systemTimeStamp()); auto timestamp = fromUsToROSTime(accelframe->timeStampUs());
imu_msg.header.stamp = timestamp; imu_msg.header.stamp = timestamp;
auto gyro_frame = gryoframe->as<ob::GyroFrame>(); auto gyro_frame = gryoframe->as<ob::GyroFrame>();
auto gyroData = gyro_frame->value(); auto gyroData = gyro_frame->value();
@@ -1260,6 +1321,12 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
imu_msg.linear_acceleration.y = accelData.y; imu_msg.linear_acceleration.y = accelData.y;
imu_msg.linear_acceleration.z = accelData.z; imu_msg.linear_acceleration.z = accelData.z;
imu_gyro_accel_publisher_->publish(imu_msg); imu_gyro_accel_publisher_->publish(imu_msg);
for (const auto &stream_index : {GYRO, ACCEL}) {
auto imu_info = createIMUInfo(stream_index);
imu_info.header = imu_msg.header;
imu_info.header.frame_id = imu_optical_frame_id_;
imu_info_publishers_[stream_index]->publish(imu_info);
}
} }
void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame, void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame,
@@ -1269,14 +1336,17 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
"stream " << stream_name_[stream_index] << " publisher not initialized"); "stream " << stream_name_[stream_index] << " publisher not initialized");
return; return;
} }
auto subscriber_count = imu_publishers_[stream_index]->get_subscription_count(); bool has_subscriber = imu_publishers_[stream_index]->get_subscription_count() > 0;
if (subscriber_count == 0) { has_subscriber =
has_subscriber || imu_info_publishers_[stream_index]->get_subscription_count() > 0;
if (!has_subscriber) {
return; return;
} }
auto imu_msg = sensor_msgs::msg::Imu(); auto imu_msg = sensor_msgs::msg::Imu();
setDefaultIMUMessage(imu_msg); setDefaultIMUMessage(imu_msg);
imu_msg.header.frame_id = optical_frame_id_[stream_index]; imu_msg.header.frame_id = optical_frame_id_[stream_index];
auto timestamp = frameTimeStampToROSTime(frame->systemTimeStamp()); auto timestamp = fromUsToROSTime(frame->timeStampUs());
imu_msg.header.stamp = timestamp; imu_msg.header.stamp = timestamp;
if (frame->type() == OB_FRAME_GYRO) { if (frame->type() == OB_FRAME_GYRO) {
auto gyro_frame = frame->as<ob::GyroFrame>(); auto gyro_frame = frame->as<ob::GyroFrame>();
@@ -1295,6 +1365,10 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
return; return;
} }
imu_publishers_[stream_index]->publish(imu_msg); imu_publishers_[stream_index]->publish(imu_msg);
auto imu_info = createIMUInfo(stream_index);
imu_info.header = imu_msg.header;
imu_info.header.frame_id = imu_optical_frame_id_;
imu_info_publishers_[stream_index]->publish(imu_info);
} }
void OBCameraNode::setDefaultIMUMessage(sensor_msgs::msg::Imu &imu_msg) { void OBCameraNode::setDefaultIMUMessage(sensor_msgs::msg::Imu &imu_msg) {
@@ -1408,52 +1482,47 @@ void OBCameraNode::publishStaticTF(const rclcpp::Time &t, const tf2::Vector3 &tr
} }
void OBCameraNode::calcAndPublishStaticTransform() { void OBCameraNode::calcAndPublishStaticTransform() {
tf2::Quaternion quaternion_optical, zero_rot, Q; tf2::Quaternion quaternion_optical, zero_rot;
zero_rot.setRPY(0.0, 0.0, 0.0); zero_rot.setRPY(0.0, 0.0, 0.0);
quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2); quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
tf2::Vector3 zero_trans(0, 0, 0); tf2::Vector3 zero_trans(0, 0, 0);
tf2::Vector3 trans(0, 0, 0); auto base_stream_profile = stream_profile_[base_stream_];
auto camera_param = pipeline_->getCameraParam(); CHECK_NOTNULL(base_stream_profile.get());
auto ex = camera_param.transform; for (const auto &item : stream_profile_) {
RCLCPP_INFO_STREAM(logger_, auto stream_index = item.first;
"transform x " << ex.trans[0] << " y " << ex.trans[1] << " z " << trans[2]); auto stream_profile = item.second;
Q = rotationMatrixToQuaternion(ex.rot); if (!stream_profile) {
Q = quaternion_optical * Q * quaternion_optical.inverse();
trans[0] = ex.trans[0];
trans[1] = ex.trans[1];
trans[2] = ex.trans[2];
tf2::Transform transform(Q, trans);
transform = transform.inverse();
Q = transform.getRotation();
trans = transform.getOrigin();
rclcpp::Time tf_timestamp = node_->now();
auto device_info = device_->getDeviceInfo();
auto pid = device_info->pid();
if (enable_stream_[COLOR]) {
if (pid != FEMTO_BOLT_PID) {
publishStaticTF(tf_timestamp, trans, Q, camera_link_frame_id_, frame_id_[COLOR]);
} else {
publishStaticTF(tf_timestamp, trans, zero_rot, camera_link_frame_id_, frame_id_[COLOR]);
}
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[COLOR],
optical_frame_id_[COLOR]);
}
for (const auto &stream_index : IMAGE_STREAMS) {
if (stream_index == COLOR || !enable_stream_[stream_index]) {
continue; continue;
} }
if (pid != FEMTO_BOLT_PID) { OBExtrinsic ex;
publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, try {
frame_id_[stream_index]); ex = stream_profile->getExtrinsicTo(base_stream_profile);
} else { } catch (const ob::Error &e) {
publishStaticTF(tf_timestamp, zero_trans, Q, camera_link_frame_id_, frame_id_[stream_index]); RCLCPP_ERROR_STREAM(logger_, "Failed to get " << stream_name_[stream_index]
<< " extrinsic: " << e.getMessage());
ex = OBExtrinsic({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0, 0, 0}});
} }
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
auto Q = rotationMatrixToQuaternion(ex.rot);
Q = quaternion_optical * Q * quaternion_optical.inverse();
tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]);
RCLCPP_INFO_STREAM(logger_, "Publishing static transform from " << camera_link_frame_id_
<< " to "
<< stream_name_[stream_index]);
RCLCPP_INFO_STREAM(logger_, "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
<< ", " << Q.getW());
auto timestamp = node_->now();
publishStaticTF(timestamp, trans, Q, camera_link_frame_id_, frame_id_[stream_index]);
publishStaticTF(timestamp, zero_trans, quaternion_optical, frame_id_[stream_index],
optical_frame_id_[stream_index]); optical_frame_id_[stream_index]);
if (depth_registration_ && depth_aligned_frame_id_.count(stream_index) > 0) {
publishStaticTF(timestamp, trans, Q, camera_link_frame_id_,
depth_aligned_frame_id_[stream_index]);
publishStaticTF(timestamp, zero_trans, quaternion_optical,
depth_aligned_frame_id_[stream_index], optical_frame_id_[stream_index]);
}
} }
publishStaticTF(tf_timestamp, zero_trans, zero_rot, camera_link_frame_id_, imu_frame_id_);
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, imu_frame_id_,
imu_optical_frame_id_);
} }
void OBCameraNode::publishStaticTransforms() { void OBCameraNode::publishStaticTransforms() {
@@ -1565,4 +1634,52 @@ bool OBCameraNode::setupFormatConvertType(OBFormat format) {
return true; return true;
} }
orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo(
const stream_index_pair &stream_index) {
orbbec_camera_msgs::msg::IMUInfo imu_info;
imu_info.header.frame_id = optical_frame_id_[stream_index];
imu_info.header.stamp = node_->now();
auto imu_profile = stream_profile_[stream_index];
if (stream_index == GYRO) {
auto gyro_profile = stream_profile_[stream_index]->as<ob::GyroStreamProfile>();
auto gyro_intrinsics = gyro_profile->getIntrinsic();
imu_info.noise_density = gyro_intrinsics.noiseDensity;
imu_info.random_walk = gyro_intrinsics.randomWalk;
imu_info.reference_temperature = gyro_intrinsics.referenceTemp;
imu_info.bias = {gyro_intrinsics.bias[0], gyro_intrinsics.bias[1], gyro_intrinsics.bias[2]};
imu_info.scale_misalignment = {
gyro_intrinsics.scaleMisalignment[0], gyro_intrinsics.scaleMisalignment[1],
gyro_intrinsics.scaleMisalignment[2], gyro_intrinsics.scaleMisalignment[3],
gyro_intrinsics.scaleMisalignment[4], gyro_intrinsics.scaleMisalignment[5],
gyro_intrinsics.scaleMisalignment[6], gyro_intrinsics.scaleMisalignment[7],
gyro_intrinsics.scaleMisalignment[8]};
imu_info.temperature_slope = {
gyro_intrinsics.tempSlope[0], gyro_intrinsics.tempSlope[1], gyro_intrinsics.tempSlope[2],
gyro_intrinsics.tempSlope[3], gyro_intrinsics.tempSlope[4], gyro_intrinsics.tempSlope[5],
gyro_intrinsics.tempSlope[6], gyro_intrinsics.tempSlope[7], gyro_intrinsics.tempSlope[8]};
} else if (stream_index == ACCEL) {
auto accel_profile = stream_profile_[stream_index]->as<ob::AccelStreamProfile>();
auto accel_intrinsics = accel_profile->getIntrinsic();
imu_info.noise_density = accel_intrinsics.noiseDensity;
imu_info.random_walk = accel_intrinsics.randomWalk;
imu_info.reference_temperature = accel_intrinsics.referenceTemp;
imu_info.bias = {accel_intrinsics.bias[0], accel_intrinsics.bias[1], accel_intrinsics.bias[2]};
imu_info.gravity = {accel_intrinsics.gravity[0], accel_intrinsics.gravity[1],
accel_intrinsics.gravity[2]};
imu_info.scale_misalignment = {
accel_intrinsics.scaleMisalignment[0], accel_intrinsics.scaleMisalignment[1],
accel_intrinsics.scaleMisalignment[2], accel_intrinsics.scaleMisalignment[3],
accel_intrinsics.scaleMisalignment[4], accel_intrinsics.scaleMisalignment[5],
accel_intrinsics.scaleMisalignment[6], accel_intrinsics.scaleMisalignment[7],
accel_intrinsics.scaleMisalignment[8]};
imu_info.temperature_slope = {accel_intrinsics.tempSlope[0], accel_intrinsics.tempSlope[1],
accel_intrinsics.tempSlope[2], accel_intrinsics.tempSlope[3],
accel_intrinsics.tempSlope[4], accel_intrinsics.tempSlope[5],
accel_intrinsics.tempSlope[6], accel_intrinsics.tempSlope[7],
accel_intrinsics.tempSlope[8]};
}
return imu_info;
}
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera/src/utils.cpp
+79 -2
View File
@@ -35,7 +35,6 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
info.d[6] = distortion.k5; info.d[6] = distortion.k5;
info.d[7] = distortion.k6; info.d[7] = distortion.k6;
info.k.fill(0.0); info.k.fill(0.0);
info.k[0] = intrinsic.fx; info.k[0] = intrinsic.fx;
info.k[2] = intrinsic.cx; info.k[2] = intrinsic.cx;
@@ -235,7 +234,7 @@ orbbec_camera_msgs::msg::Extrinsics obExtrinsicsToMsg(const OBD2CTransform &extr
return msg; return msg;
} }
rclcpp::Time frameTimeStampToROSTime(uint64_t ms) { rclcpp::Time fromMsToROSTime(uint64_t ms) {
auto total = static_cast<uint64_t>(ms * 1e6); auto total = static_cast<uint64_t>(ms * 1e6);
uint64_t sec = total / 1000000000; uint64_t sec = total / 1000000000;
uint64_t nano_sec = total % 1000000000; uint64_t nano_sec = total % 1000000000;
@@ -243,6 +242,14 @@ rclcpp::Time frameTimeStampToROSTime(uint64_t ms) {
return stamp; return stamp;
} }
rclcpp::Time fromUsToROSTime(uint64_t us) {
auto total = static_cast<uint64_t>(us * 1e3);
uint64_t sec = total / 1000000000;
uint64_t nano_sec = total % 1000000000;
rclcpp::Time stamp(sec, nano_sec);
return stamp;
}
std::string getObSDKVersion() { std::string getObSDKVersion() {
std::string major = std::to_string(ob::Version::getMajor()); std::string major = std::to_string(ob::Version::getMajor());
std::string minor = std::to_string(ob::Version::getMinor()); std::string minor = std::to_string(ob::Version::getMinor());
@@ -585,4 +592,74 @@ bool isValidJPEG(const std::shared_ptr<ob::ColorFrame> &frame) {
return true; return true;
} }
std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type) {
switch (meta_data_type) {
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_TIMESTAMP:
return "timestamp";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_SENSOR_TIMESTAMP:
return "sensor_timestamp";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_FRAME_NUMBER:
return "frame_number";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AUTO_EXPOSURE:
return "auto_exposure";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_EXPOSURE:
return "exposure";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_GAIN:
return "gain";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AUTO_WHITE_BALANCE:
return "auto_white_balance";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_WHITE_BALANCE:
return "white_balance";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_BRIGHTNESS:
return "brightness";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_CONTRAST:
return "contrast";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_SATURATION:
return "saturation";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_SHARPNESS:
return "sharpness";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_BACKLIGHT_COMPENSATION:
return "backlight_compensation";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HUE:
return "hue";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_GAMMA:
return "gamma";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_POWER_LINE_FREQUENCY:
return "power_line_frequency";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_LOW_LIGHT_COMPENSATION:
return "low_light_compensation";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_MANUAL_WHITE_BALANCE:
return "manual_white_balance";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_ACTUAL_FRAME_RATE:
return "actual_frame_rate";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_FRAME_RATE:
return "frame_rate";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_LEFT:
return "ae_roi_left";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_TOP:
return "ae_roi_top";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_RIGHT:
return "ae_roi_right";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_AE_ROI_BOTTOM:
return "ae_roi_bottom";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_EXPOSURE_PRIORITY:
return "exposure_priority";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_NAME:
return "hdr_sequence_name";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_SIZE:
return "hdr_sequence_size";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX:
return "hdr_sequence_index";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_LASER_POWER:
return "laser_power";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_LASER_POWER_MODE:
return "laser_power_mode";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_EMITTER_MODE:
return "emitter_mode";
case OBFrameMetadataType::OB_FRAME_METADATA_TYPE_GPIO_INPUT_DATA:
return "gpio_input_data";
default:
return "unknown";
}
}
} // namespace orbbec_camera } // namespace orbbec_camera
orbbec_camera_msgs/CMakeLists.txt
+1
View File
@@ -18,6 +18,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/DeviceInfo.msg" "msg/DeviceInfo.msg"
"msg/Extrinsics.msg" "msg/Extrinsics.msg"
"msg/Metadata.msg" "msg/Metadata.msg"
"msg/IMUInfo.msg"
"srv/GetBool.srv" "srv/GetBool.srv"
"srv/GetDeviceInfo.srv" "srv/GetDeviceInfo.srv"
"srv/GetCameraInfo.srv" "srv/GetCameraInfo.srv"
orbbec_camera_msgs/msg/IMUInfo.msg
+9
View File
@@ -0,0 +1,9 @@
std_msgs/Header header
float64 noise_density
float64 random_walk
float64 reference_temperature
float64[3] bias
float64[3] gravity
float64[9] scale_misalignment
float64[9] temperature_slope