mirror of
https://github.com/orbbec/OrbbecSDK_ROS2.git
synced 2026-10-04 20:17:47 +08:00
init commit
This commit is contained in:
Joe Dong
@@ -0,0 +1,249 @@
|
||||
#include "orbbec_camera/ob_camera_node.h"
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <thread>
|
||||
|
||||
#include "orbbec_camera/utils.h"
|
||||
namespace orbbec_camera {
|
||||
void OBCameraNode::setupCameraCtrlServices() {
|
||||
using std_srvs::srv::SetBool;
|
||||
for (auto stream_index : IMAGE_STREAMS) {
|
||||
auto stream_name = stream_name_[stream_index.first];
|
||||
if (enable_[stream_index]) {
|
||||
std::string service_name = "/get/" + stream_name + "/exposure";
|
||||
get_exposure_srv_[stream_index] = node_->create_service<GetInt32>(
|
||||
service_name, [this, stream_index = stream_index](
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<GetInt32::Request> request,
|
||||
std::shared_ptr<GetInt32::Response> response) {
|
||||
getExposureCallback(request, response, stream_index);
|
||||
});
|
||||
|
||||
service_name = "/set/" + stream_name + "/exposure";
|
||||
set_exposure_srv_[stream_index] = node_->create_service<SetInt32>(
|
||||
service_name, [this, stream_index = stream_index](
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetInt32::Request> request,
|
||||
std::shared_ptr<SetInt32::Response> response) {
|
||||
setExposureCallback(request, response, stream_index);
|
||||
});
|
||||
service_name = "/get/" + stream_name + "/gain";
|
||||
get_gain_srv_[stream_index] = node_->create_service<GetInt32>(
|
||||
service_name, [this, stream_index = stream_index](
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<GetInt32::Request> request,
|
||||
std::shared_ptr<GetInt32::Response> response) {
|
||||
getGainCallback(request, response, stream_index);
|
||||
});
|
||||
|
||||
service_name = "set/" + stream_name + "/gain";
|
||||
set_gain_srv_[stream_index] = node_->create_service<SetInt32>(
|
||||
service_name, [this, stream_index = stream_index](
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetInt32::Request> request,
|
||||
std::shared_ptr<SetInt32::Response> response) {
|
||||
setGainCallback(request, response, stream_index);
|
||||
});
|
||||
if (stream_index.first == OB_STREAM_COLOR || stream_index.first == OB_STREAM_DEPTH) {
|
||||
service_name = "/set/" + stream_name +
|
||||
"/"
|
||||
"auto_exposure";
|
||||
set_auto_exposure_srv_[stream_index] = node_->create_service<SetBool>(
|
||||
service_name, [this, stream_index = stream_index](
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setAutoExposureCallback(request, response, stream_index);
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
set_fan_mode_srv_ = node_->create_service<SetInt32>(
|
||||
"/set_fan_mode", [this](const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetInt32::Request> request,
|
||||
std::shared_ptr<SetInt32::Response> response) {
|
||||
setFanModeCallback(request_header, request, response);
|
||||
});
|
||||
set_floor_enable_srv_ = node_->create_service<SetBool>(
|
||||
"/set_floor_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setFloorEnableCallback(request_header, request, response);
|
||||
});
|
||||
set_laser_enable_srv_ = node_->create_service<SetBool>(
|
||||
"/set_laser_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setLaserEnableCallback(request_header, request, response);
|
||||
});
|
||||
set_ldp_enable_srv_ = node_->create_service<SetBool>(
|
||||
"/set_ldp_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetBool::Request> request,
|
||||
std::shared_ptr<SetBool::Response> response) {
|
||||
setLdpEnableCallback(request_header, request, response);
|
||||
});
|
||||
|
||||
get_white_balance_srv_ = node_->create_service<GetInt32>(
|
||||
"/get/white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<GetInt32::Request> request,
|
||||
std::shared_ptr<GetInt32::Response> response) {
|
||||
getWhiteBalanceCallback(request_header, request, response);
|
||||
});
|
||||
|
||||
set_white_balance_srv_ = node_->create_service<SetInt32>(
|
||||
"/set/white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<SetInt32::Request> request,
|
||||
std::shared_ptr<SetInt32::Response> response) {
|
||||
setWhiteBalanceCallback(request_header, request, response);
|
||||
});
|
||||
}
|
||||
|
||||
void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
|
||||
std::shared_ptr<SetInt32::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
auto stream = stream_index.first;
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR:
|
||||
device_->setIntProperty(OB_PROP_IR_EXPOSURE_INT, request->data);
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
device_->setIntProperty(OB_PROP_DEPTH_EXPOSURE_INT, request->data);
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
device_->setIntProperty(OB_PROP_COLOR_EXPOSURE_INT, request->data);
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getGainCallback(const std::shared_ptr<GetInt32::Request>& request,
|
||||
std::shared_ptr<GetInt32::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
auto stream = stream_index.first;
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR:
|
||||
response->data = device_->getIntProperty(OB_PROP_IR_GAIN_INT);
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
response->data = device_->getIntProperty(OB_PROP_DEPTH_GAIN_INT);
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
response->data = device_->getIntProperty(OB_PROP_COLOR_GAIN_INT);
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
}
|
||||
void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& request,
|
||||
std::shared_ptr<SetInt32::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
auto stream = stream_index.first;
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR:
|
||||
device_->setIntProperty(OB_PROP_IR_GAIN_INT, request->data);
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
device_->setIntProperty(OB_PROP_DEPTH_GAIN_INT, request->data);
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
device_->setIntProperty(OB_PROP_COLOR_GAIN_INT, request->data);
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::getWhiteBalanceCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
|
||||
const std::shared_ptr<GetInt32::Request>& request,
|
||||
std::shared_ptr<GetInt32::Response>& response) {
|
||||
response->data = device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT);
|
||||
}
|
||||
|
||||
void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
|
||||
const std::shared_ptr<SetInt32 ::Request>& request,
|
||||
std::shared_ptr<SetInt32 ::Response>& response) {
|
||||
device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, request->data);
|
||||
}
|
||||
|
||||
void OBCameraNode::setAutoExposureCallback(
|
||||
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
auto stream = stream_index.first;
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR:
|
||||
response->message = "IR not support set auto exposure";
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
device_->setIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL, request->data);
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
device_->setIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, request->data);
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setFanModeCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
|
||||
const std::shared_ptr<SetInt32::Request>& request,
|
||||
std::shared_ptr<SetInt32::Response>& response) {
|
||||
(void)request_header;
|
||||
(void)response;
|
||||
bool fan_mode = request->data;
|
||||
device_->setBoolProperty(OB_PROP_FAN_WORK_MODE_INT, fan_mode);
|
||||
}
|
||||
|
||||
void OBCameraNode::setFloorEnableCallback(
|
||||
const std::shared_ptr<rmw_request_id_t>& request_header,
|
||||
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
|
||||
(void)request_header;
|
||||
(void)response;
|
||||
bool floor_enable = request->data;
|
||||
device_->setBoolProperty(OB_PROP_FLOOD_BOOL, floor_enable);
|
||||
}
|
||||
|
||||
void OBCameraNode::setLaserEnableCallback(
|
||||
const std::shared_ptr<rmw_request_id_t>& request_header,
|
||||
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
|
||||
(void)request_header;
|
||||
(void)response;
|
||||
bool laser_enable = request->data;
|
||||
device_->setBoolProperty(OB_PROP_LASER_BOOL, laser_enable);
|
||||
}
|
||||
|
||||
void OBCameraNode::setLdpEnableCallback(
|
||||
const std::shared_ptr<rmw_request_id_t>& request_header,
|
||||
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
|
||||
(void)request_header;
|
||||
(void)response;
|
||||
bool ldp_enable = request->data;
|
||||
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
|
||||
}
|
||||
void OBCameraNode::getExposureCallback(const std::shared_ptr<GetInt32::Request>& request,
|
||||
std::shared_ptr<GetInt32 ::Response>& response,
|
||||
const stream_index_pair& stream_index) {
|
||||
auto stream = stream_index.first;
|
||||
switch (stream) {
|
||||
case OB_STREAM_IR:
|
||||
response->data = device_->getIntProperty(OB_PROP_IR_EXPOSURE_INT);
|
||||
break;
|
||||
case OB_STREAM_DEPTH:
|
||||
response->data = device_->getIntProperty(OB_PROP_DEPTH_EXPOSURE_INT);
|
||||
break;
|
||||
case OB_STREAM_COLOR:
|
||||
response->data = device_->getIntProperty(OB_PROP_COLOR_EXPOSURE_INT);
|
||||
break;
|
||||
default:
|
||||
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
|
||||
break;
|
||||
}
|
||||
}
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,488 @@
|
||||
#include "orbbec_camera/ob_camera_node.h"
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <thread>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
|
||||
#include "orbbec_camera/utils.h"
|
||||
namespace orbbec_camera {
|
||||
using namespace std::chrono_literals;
|
||||
OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device)
|
||||
: node_(node), device_(device), logger_(node->get_logger()) {
|
||||
static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node);
|
||||
dynamic_tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node);
|
||||
// FIXME:
|
||||
is_running_.store(true);
|
||||
format_[DEPTH] = OB_FORMAT_Y16;
|
||||
image_format_[OB_STREAM_DEPTH] = CV_16UC1;
|
||||
encoding_[DEPTH] = sensor_msgs::image_encodings::TYPE_16UC1;
|
||||
stream_name_[OB_STREAM_DEPTH] = "depth";
|
||||
unit_step_size_[DEPTH] = sizeof(uint16_t);
|
||||
format_[IR0] = OB_FORMAT_Y16;
|
||||
image_format_[OB_STREAM_IR] = CV_16UC1;
|
||||
encoding_[IR0] = sensor_msgs::image_encodings::MONO16;
|
||||
stream_name_[OB_STREAM_IR] = "ir";
|
||||
unit_step_size_[IR0] = sizeof(uint8_t);
|
||||
|
||||
format_[COLOR] = OB_FORMAT_I420;
|
||||
image_format_[OB_STREAM_COLOR] = CV_8UC3;
|
||||
encoding_[COLOR] = sensor_msgs::image_encodings::BGR8;
|
||||
stream_name_[OB_STREAM_COLOR] = "color";
|
||||
unit_step_size_[COLOR] = 3;
|
||||
|
||||
compression_params_.push_back(cv::IMWRITE_PNG_COMPRESSION);
|
||||
compression_params_.push_back(0);
|
||||
compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY);
|
||||
compression_params_.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
|
||||
setupTopics();
|
||||
startPipeline();
|
||||
}
|
||||
|
||||
OBCameraNode::~OBCameraNode() {
|
||||
if (tf_thread_->joinable()) {
|
||||
tf_thread_->join();
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setupDevices() {
|
||||
auto sensor_list = device_->getSensorList();
|
||||
for (size_t i = 0; i < sensor_list->count(); i++) {
|
||||
auto sensor = sensor_list->getSensor(i);
|
||||
auto profiles = sensor->getStreamProfileList();
|
||||
for (size_t j = 0; j < profiles->count(); j++) {
|
||||
auto profile = profiles->getProfile(j);
|
||||
stream_index_pair sip{profile->type(), 0};
|
||||
if (sensors_.find(sip) != sensors_.end()) {
|
||||
continue;
|
||||
}
|
||||
sensors_[sip] = sensor;
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto& [stream_index, enable] : enable_) {
|
||||
if (enable && sensors_.find(stream_index) == sensors_.end()) {
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
magic_enum::enum_name(stream_index.first)
|
||||
<< "sensor isn't supported by current device! -- Skipping...");
|
||||
enable_[stream_index] = false;
|
||||
}
|
||||
}
|
||||
|
||||
auto camera_param_list = device_->getCalibrationCameraParamList();
|
||||
for (size_t i = 0; i < camera_param_list->count(); i++) {
|
||||
auto camera_param = camera_param_list->getCameraParam(i);
|
||||
RCLCPP_ERROR_STREAM(logger_, "param \n" << camera_param);
|
||||
camera_params_.emplace_back(camera_param);
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::setupProfiles() {
|
||||
config_ = std::make_shared<ob::Config>();
|
||||
config_->setAlignMode(ALIGN_D2C_HW_MODE);
|
||||
for (const auto& elem : IMAGE_STREAMS) {
|
||||
if (enable_[elem]) {
|
||||
const auto& sensor = sensors_[elem];
|
||||
auto profiles = sensor->getStreamProfileList();
|
||||
for (size_t i = 0; i < profiles->count(); i++) {
|
||||
auto profile = profiles->getProfile(i)->as<ob::VideoStreamProfile>();
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
logger_, "Sensor profile: "
|
||||
<< "stream_type: " << magic_enum::enum_name(profile->type())
|
||||
<< "Format: " << profile->format() << ", Width: " << profile->width()
|
||||
<< ", Height: " << profile->height() << ", FPS: " << profile->fps());
|
||||
enabled_profiles_[elem].emplace_back(profile);
|
||||
}
|
||||
|
||||
auto selected_profile =
|
||||
profiles->getVideoStreamProfile(width_[elem], height_[elem], format_[elem], fps_[elem]);
|
||||
auto default_profile = profiles->getVideoStreamProfile();
|
||||
if (!selected_profile) {
|
||||
RCLCPP_WARN_STREAM(logger_, "Given stream configuration is not supported by the device! "
|
||||
<< " Stream: " << magic_enum::enum_name(elem.first)
|
||||
<< ", Stream Index: " << elem.second
|
||||
<< ", Width: " << width_[elem]
|
||||
<< ", Height: " << height_[elem] << ", FPS: " << fps_[elem]
|
||||
<< ", Format: " << format_[elem]);
|
||||
if (default_profile) {
|
||||
RCLCPP_WARN_STREAM(logger_, "Using default profile instead.");
|
||||
selected_profile = default_profile;
|
||||
} else {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
logger_, " NO default_profile found , Stream: " << magic_enum::enum_name(elem.first)
|
||||
<< " will be disable");
|
||||
enable_[elem] = false;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
CHECK_NOTNULL(selected_profile);
|
||||
config_->enableStream(selected_profile);
|
||||
images_[elem] =
|
||||
cv::Mat(height_[elem], width_[elem], image_format_[elem.first], cv::Scalar(0, 0, 0));
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, " stream is enabled - width: " << width_[elem] << ", height: " << height_[elem]
|
||||
<< ", fps: " << fps_[elem] << ", "
|
||||
<< "Format: " << selected_profile->format());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::startPipeline() {
|
||||
pipeline_ = std::make_unique<ob::Pipeline>(device_);
|
||||
pipeline_->start(config_, [this](std::shared_ptr<ob::FrameSet> frame_set) {
|
||||
frameSetCallback(std::move(frame_set));
|
||||
});
|
||||
}
|
||||
|
||||
void OBCameraNode::getParameters() {
|
||||
for (auto stream_index : IMAGE_STREAMS) {
|
||||
std::string param_name = stream_name_[stream_index.first] + "_width";
|
||||
width_[stream_index] = node_->declare_parameter<int>(param_name, IMAGE_WIDTH);
|
||||
param_name = stream_name_[stream_index.first] + "_height";
|
||||
height_[stream_index] = node_->declare_parameter<int>(param_name, IMAGE_HEIGHT);
|
||||
param_name = stream_name_[stream_index.first] + "_fps";
|
||||
fps_[stream_index] = node_->declare_parameter<int>(param_name, IMAGE_FPS);
|
||||
param_name = "enable_" + stream_name_[stream_index.first];
|
||||
enable_[stream_index] = node_->declare_parameter<bool>(param_name, true);
|
||||
param_name = stream_name_[stream_index.first] + "_frame_id";
|
||||
std::string default_frame_id = "camera_" + stream_name_[stream_index.first] + "_frame";
|
||||
frame_id_[stream_index] = node_->declare_parameter<std::string>(param_name, default_frame_id);
|
||||
std::string default_optical_frame_id =
|
||||
"camera_" + stream_name_[stream_index.first] + "_optical_frame";
|
||||
param_name = stream_name_[stream_index.first] + "_optical_frame_id";
|
||||
optical_frame_id_[stream_index] =
|
||||
node_->declare_parameter<std::string>(param_name, default_optical_frame_id);
|
||||
depth_aligned_frame_id_[stream_index] = stream_name_[OB_STREAM_COLOR] + "_optical_frame";
|
||||
}
|
||||
publish_tf_ = node_->declare_parameter<bool>("publish_tf", true);
|
||||
align_depth_ = node_->declare_parameter<bool>("align_depth", true);
|
||||
tf_publish_rate_ = node_->declare_parameter<double>("tf_publish_rate", 10.0);
|
||||
}
|
||||
|
||||
void OBCameraNode::setupTopics() {
|
||||
getParameters();
|
||||
setupDevices();
|
||||
updateStreamCalibData();
|
||||
setupProfiles();
|
||||
setupCameraCtrlServices();
|
||||
setupPublishers();
|
||||
publishStaticTransforms();
|
||||
}
|
||||
|
||||
void OBCameraNode::setupPublishers() {
|
||||
static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_);
|
||||
using PointCloud2 = sensor_msgs::msg::PointCloud2;
|
||||
using CameraInfo = sensor_msgs::msg::CameraInfo;
|
||||
point_cloud_publisher_ =
|
||||
node_->create_publisher<PointCloud2>("depth/points", rclcpp::QoS{1}.best_effort());
|
||||
for (const auto& stream_index : IMAGE_STREAMS) {
|
||||
std::string name = stream_name_[stream_index.first];
|
||||
std::string topic = name + "/image_raw";
|
||||
image_publishers_[stream_index] = image_transport::create_publisher(node_, topic);
|
||||
topic = name + "/camera_info";
|
||||
camera_info_publishers_[stream_index] =
|
||||
node_->create_publisher<CameraInfo>(topic, rclcpp::QoS{1}.best_effort());
|
||||
}
|
||||
extrinsics_publisher_ = node_->create_publisher<orbbec_camera_msgs::msg::Extrinsics>(
|
||||
"extrinsic", rclcpp::QoS{1}.transient_local());
|
||||
}
|
||||
|
||||
void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set,
|
||||
const rclcpp::Time& t) {
|
||||
static int cnt = 0;
|
||||
const std::string home_dir = std::getenv("HOME");
|
||||
const std::string pc_file_name = home_dir + "/pc/point_cloud.ply";
|
||||
auto camera_param = findCameraParam();
|
||||
CHECK(camera_param.has_value());
|
||||
if ((++cnt) % 20 == 0) {
|
||||
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
|
||||
RCLCPP_INFO_STREAM(logger_, "has rgb pc");
|
||||
point_cloud_filter_.setCameraParam(*camera_param);
|
||||
point_cloud_filter_.setCreatePointFormat(OB_FORMAT_RGB_POINT);
|
||||
auto frame = point_cloud_filter_.process(frame_set);
|
||||
saveRGBPointsToPly(frame, pc_file_name);
|
||||
} else if (frame_set->depthFrame() != nullptr) {
|
||||
RCLCPP_INFO_STREAM(logger_, "has depth pc");
|
||||
point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
|
||||
auto frame = point_cloud_filter_.process(frame_set);
|
||||
savePointsToPly(frame, pc_file_name);
|
||||
}
|
||||
}
|
||||
if (frame_set->depthFrame() != nullptr && frame_set->colorFrame() != nullptr) {
|
||||
//publishColorPointCloud(frame_set, t);
|
||||
publishDepthPointCloud(frame_set, t);
|
||||
|
||||
} else if (frame_set->depthFrame() != nullptr) {
|
||||
publishDepthPointCloud(frame_set, t);
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set,
|
||||
const rclcpp::Time& t) {
|
||||
point_cloud_filter_.setCreatePointFormat(OB_FORMAT_POINT);
|
||||
auto frame = point_cloud_filter_.process(frame_set);
|
||||
size_t point_size = frame->dataSize() / sizeof(OBPoint);
|
||||
auto* points = (OBPoint*)frame->data();
|
||||
sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
|
||||
modifier.setPointCloud2FieldsByString(1, "xyz");
|
||||
modifier.resize(point_size);
|
||||
point_cloud_msg_.width = width_[DEPTH];
|
||||
point_cloud_msg_.height = height_[DEPTH];
|
||||
std::string format_str = "intensity";
|
||||
|
||||
point_cloud_msg_.point_step =
|
||||
addPointField(point_cloud_msg_, format_str.c_str(), 1, sensor_msgs::msg::PointField::FLOAT32,
|
||||
point_cloud_msg_.point_step);
|
||||
point_cloud_msg_.row_step = point_cloud_msg_.width * point_cloud_msg_.point_step;
|
||||
point_cloud_msg_.data.resize(point_cloud_msg_.height * point_cloud_msg_.row_step);
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud_msg_, "y");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud_msg_, "z");
|
||||
size_t valid_count = 0;
|
||||
for (size_t point_idx = 0; point_idx < point_size; point_idx++, points++) {
|
||||
bool valid_pixel(points->z > 0);
|
||||
if (valid_pixel) {
|
||||
*iter_x = points->x / 1000.0;
|
||||
*iter_y = points->y / 1000.0;
|
||||
*iter_z = points->z / 1000.0;
|
||||
|
||||
++iter_x;
|
||||
++iter_y;
|
||||
++iter_z;
|
||||
++valid_count;
|
||||
}
|
||||
}
|
||||
point_cloud_msg_.header.stamp = t;
|
||||
point_cloud_msg_.header.frame_id = "camera_link";
|
||||
// TODO: fill frame_id
|
||||
point_cloud_publisher_->publish(point_cloud_msg_);
|
||||
}
|
||||
|
||||
void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_set,
|
||||
const rclcpp::Time& t) {
|
||||
point_cloud_filter_.setCreatePointFormat(OB_FORMAT_RGB_POINT);
|
||||
auto frame = point_cloud_filter_.process(frame_set);
|
||||
size_t point_size = frame->dataSize() / sizeof(OBColorPoint);
|
||||
auto* points = (OBColorPoint*)frame->data();
|
||||
sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
|
||||
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
|
||||
modifier.resize(point_size);
|
||||
point_cloud_msg_.width = width_[DEPTH];
|
||||
point_cloud_msg_.height = height_[DEPTH];
|
||||
std::string format_str = "intensity";
|
||||
|
||||
point_cloud_msg_.point_step =
|
||||
addPointField(point_cloud_msg_, format_str.c_str(), 1, sensor_msgs::msg::PointField::FLOAT32,
|
||||
point_cloud_msg_.point_step);
|
||||
point_cloud_msg_.row_step = point_cloud_msg_.width * point_cloud_msg_.point_step;
|
||||
point_cloud_msg_.data.resize(point_cloud_msg_.height * point_cloud_msg_.row_step);
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud_msg_, "y");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud_msg_, "z");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_r(point_cloud_msg_, "r");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_g(point_cloud_msg_, "g");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_b(point_cloud_msg_, "b");
|
||||
size_t valid_count = 0;
|
||||
for (size_t point_idx = 0; point_idx < point_size; point_idx++, points++) {
|
||||
bool valid_pixel(points->z > 0);
|
||||
if (valid_pixel) {
|
||||
*iter_x = points->x;
|
||||
*iter_y = points->y;
|
||||
*iter_z = points->z;
|
||||
*iter_r = points->r;
|
||||
*iter_g = points->g;
|
||||
*iter_b = points->b;
|
||||
|
||||
++iter_x;
|
||||
++iter_y;
|
||||
++iter_z;
|
||||
++iter_r;
|
||||
++iter_g;
|
||||
++iter_b;
|
||||
++valid_count;
|
||||
}
|
||||
}
|
||||
point_cloud_msg_.header.stamp = t;
|
||||
point_cloud_msg_.header.frame_id = "camera_link";
|
||||
point_cloud_publisher_->publish(point_cloud_msg_);
|
||||
}
|
||||
void OBCameraNode::frameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
|
||||
// FIXME:
|
||||
auto nano_sec = static_cast<uint64_t>(frame_set->timeStampUs() * 1e3);
|
||||
rclcpp::Time t = rclcpp::Time(0, nano_sec);
|
||||
stream_index_pair sip;
|
||||
if (auto color_frame = frame_set->colorFrame()) {
|
||||
publishColorFrame(color_frame, t);
|
||||
}
|
||||
if (auto depth_frame = frame_set->depthFrame()) {
|
||||
publishDepthFrame(depth_frame, t);
|
||||
}
|
||||
// if (auto ir_frame = frame_set->irFrame()) {
|
||||
// publishFrame(ir_frame, t, IR0);
|
||||
// }
|
||||
publishPointCloud(frame_set, t);
|
||||
}
|
||||
std::optional<OBCameraParam> OBCameraNode::findCameraParam() {
|
||||
for (auto param : camera_params_) {
|
||||
int depth_w = param.depthIntrinsic.width;
|
||||
int depth_h = param.depthIntrinsic.height;
|
||||
int color_w = param.rgbIntrinsic.width;
|
||||
int color_h = param.rgbIntrinsic.height;
|
||||
if ((depth_w * height_[DEPTH] == depth_h * width_[DEPTH]) &&
|
||||
(color_w * height_[COLOR] == color_h * width_[COLOR])) {
|
||||
return param;
|
||||
}
|
||||
}
|
||||
return {};
|
||||
}
|
||||
void OBCameraNode::updateStreamCalibData() {
|
||||
auto param = findCameraParam();
|
||||
CHECK(param.has_value());
|
||||
camera_infos_[DEPTH] = convertToCameraInfo(param->depthIntrinsic, param->depthDistortion);
|
||||
camera_infos_[COLOR] = convertToCameraInfo(param->rgbIntrinsic, param->rgbDistortion);
|
||||
camera_infos_[IR0] = camera_infos_[DEPTH];
|
||||
}
|
||||
|
||||
void OBCameraNode::publishStaticTF(const rclcpp::Time& t, const std::vector<float>& trans,
|
||||
const tf2::Quaternion& q, const std::string& from,
|
||||
const std::string& to) {
|
||||
CHECK_EQ(trans.size(), 3u);
|
||||
geometry_msgs::msg::TransformStamped msg;
|
||||
msg.header.stamp = t;
|
||||
msg.header.frame_id = from;
|
||||
msg.child_frame_id = to;
|
||||
msg.transform.translation.x = trans.at(2) / 1000.0;
|
||||
msg.transform.translation.y = -trans.at(0) / 1000.0;
|
||||
msg.transform.translation.z = -trans.at(1) / 1000.0;
|
||||
msg.transform.rotation.x = q.getX();
|
||||
msg.transform.rotation.y = q.getY();
|
||||
msg.transform.rotation.z = q.getZ();
|
||||
msg.transform.rotation.w = q.getW();
|
||||
static_tf_msgs_.push_back(msg);
|
||||
}
|
||||
|
||||
void OBCameraNode::calcAndPublishStaticTransform() {
|
||||
tf2::Quaternion quaternion_optical, zero_rot;
|
||||
zero_rot.setRPY(0.0, 0.0, 0.0);
|
||||
quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
|
||||
std::vector<float> zero_trans = {0, 0, 0};
|
||||
auto camera_param = findCameraParam();
|
||||
CHECK(camera_param.has_value());
|
||||
auto ex = camera_param->transform;
|
||||
auto Q = rotationMatrixToQuaternion(ex.rot);
|
||||
Q = quaternion_optical * Q * quaternion_optical.inverse();
|
||||
std::vector<float> trans = {ex.trans[0], ex.trans[1], ex.trans[2]};
|
||||
rclcpp::Time tf_timestamp = node_->now();
|
||||
|
||||
publishStaticTF(tf_timestamp, trans, Q, frame_id_[DEPTH], frame_id_[COLOR]);
|
||||
publishStaticTF(tf_timestamp, trans, Q, "camera_link", frame_id_[COLOR]);
|
||||
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[COLOR],
|
||||
optical_frame_id_[COLOR]);
|
||||
publishStaticTF(tf_timestamp, zero_trans, quaternion_optical, frame_id_[DEPTH],
|
||||
optical_frame_id_[DEPTH]);
|
||||
publishStaticTF(tf_timestamp, zero_trans, zero_rot, "camera_link", frame_id_[DEPTH]);
|
||||
extrinsics_publisher_->publish(obExtrinsicsToMsg(ex, frame_id_[COLOR]));
|
||||
}
|
||||
|
||||
void OBCameraNode::publishStaticTransforms() {
|
||||
calcAndPublishStaticTransform();
|
||||
if (tf_publish_rate_ > 0) {
|
||||
tf_thread_ = std::make_shared<std::thread>([this]() { publishDynamicTransforms(); });
|
||||
} else {
|
||||
static_tf_broadcaster_->sendTransform(static_tf_msgs_);
|
||||
}
|
||||
}
|
||||
void OBCameraNode::publishDynamicTransforms() {
|
||||
RCLCPP_WARN(logger_, "Publishing dynamic camera transforms (/tf) at %g Hz", tf_publish_rate_);
|
||||
std::mutex mu;
|
||||
std::unique_lock<std::mutex> lock(mu);
|
||||
while (rclcpp::ok() && is_running_) {
|
||||
tf_cv_.wait_for(lock, std::chrono::milliseconds((int)(1000.0 / tf_publish_rate_)),
|
||||
[this] { return (!(is_running_)); });
|
||||
{
|
||||
rclcpp::Time t = node_->now();
|
||||
for (auto& msg : static_tf_msgs_) {
|
||||
msg.header.stamp = t;
|
||||
}
|
||||
dynamic_tf_broadcaster_->sendTransform(static_tf_msgs_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNode::publishColorFrame(std::shared_ptr<ob::ColorFrame> frame, const rclcpp::Time& t) {
|
||||
RCLCPP_INFO_STREAM(logger_, "publish color frame");
|
||||
format_convert_filter.setFormatConvertType(FORMAT_I420_TO_RGB888);
|
||||
frame = format_convert_filter.process(frame)->as<ob::ColorFrame>();
|
||||
format_convert_filter.setFormatConvertType(FORMAT_RGB888_TO_BGR);
|
||||
frame = format_convert_filter.process(frame)->as<ob::ColorFrame>();
|
||||
auto width = frame->width();
|
||||
auto height = frame->height();
|
||||
auto stream = COLOR;
|
||||
RCLCPP_INFO_STREAM(logger_, "stream " << magic_enum::enum_name(stream.first)
|
||||
<< " get image width " << width << ", height " << height
|
||||
<< ", format " << magic_enum::enum_name(frame->format()));
|
||||
auto& image = images_[stream];
|
||||
if (image.size() != cv::Size(width, height)) {
|
||||
image.create(height, width, image.type());
|
||||
}
|
||||
image.data = (uint8_t*)frame->data();
|
||||
auto& camera_info_publisher = camera_info_publishers_.at(stream);
|
||||
auto& image_publisher = image_publishers_.at(stream);
|
||||
auto& cam_info = camera_infos_.at(stream);
|
||||
if (cam_info.width != width) {
|
||||
RCLCPP_ERROR(logger_, "cam info error");
|
||||
cam_info.height = height;
|
||||
cam_info.width = width;
|
||||
}
|
||||
cam_info.header.stamp = t;
|
||||
camera_info_publisher->publish(cam_info);
|
||||
sensor_msgs::msg::Image::SharedPtr img;
|
||||
img = cv_bridge::CvImage(std_msgs::msg::Header(), encoding_.at(stream), image).toImageMsg();
|
||||
|
||||
img->width = width;
|
||||
img->height = height;
|
||||
img->is_bigendian = false;
|
||||
img->step = width * unit_step_size_[stream];
|
||||
img->header.frame_id = depth_aligned_frame_id_[stream];
|
||||
img->header.stamp = t;
|
||||
image_publisher.publish(img);
|
||||
}
|
||||
|
||||
void OBCameraNode::publishDepthFrame(std::shared_ptr<ob::DepthFrame> frame, const rclcpp::Time& t) {
|
||||
RCLCPP_INFO_STREAM(logger_, "publish depth frame");
|
||||
auto width = frame->width();
|
||||
auto height = frame->height();
|
||||
auto stream = DEPTH;
|
||||
RCLCPP_INFO_STREAM(logger_, "stream " << magic_enum::enum_name(stream.first)
|
||||
<< " get image width " << width << ", height " << height
|
||||
<< ", format " << magic_enum::enum_name(frame->format()));
|
||||
auto& image = images_[stream];
|
||||
if (image.size() != cv::Size(width, height)) {
|
||||
image.create(height, width, image.type());
|
||||
}
|
||||
image.data = (uint8_t*)frame->data();
|
||||
auto& camera_info_publisher = camera_info_publishers_.at(stream);
|
||||
auto& image_publisher = image_publishers_.at(stream);
|
||||
auto& cam_info = camera_infos_.at(stream);
|
||||
if (cam_info.width != width) {
|
||||
RCLCPP_ERROR(logger_, "cam info error");
|
||||
cam_info.height = height;
|
||||
cam_info.width = width;
|
||||
}
|
||||
cam_info.header.stamp = t;
|
||||
camera_info_publisher->publish(cam_info);
|
||||
sensor_msgs::msg::Image::SharedPtr img;
|
||||
img = cv_bridge::CvImage(std_msgs::msg::Header(), encoding_.at(stream), image).toImageMsg();
|
||||
|
||||
img->width = width;
|
||||
img->height = height;
|
||||
img->is_bigendian = false;
|
||||
img->step = width * unit_step_size_[stream];
|
||||
img->header.frame_id = depth_aligned_frame_id_[stream];
|
||||
img->header.stamp = t;
|
||||
image_publisher.publish(img);
|
||||
}
|
||||
|
||||
void OBCameraNode::publishIRFrame(std::shared_ptr<ob::IRFrame> frame, const rclcpp::Time& t) {}
|
||||
|
||||
void OBCameraNode::publishFrame(std::shared_ptr<ob::Frame> frame, const rclcpp::Time& t,
|
||||
const stream_index_pair& stream) {}
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,115 @@
|
||||
#include "orbbec_camera/ob_camera_node_factory.h"
|
||||
namespace orbbec_camera {
|
||||
OBCameraNodeFactory::OBCameraNodeFactory(const rclcpp::NodeOptions &node_options)
|
||||
: Node("orbbec_camera_node", "/", node_options),
|
||||
ctx_(std::make_unique<ob::Context>()),
|
||||
logger_(this->get_logger()) {
|
||||
init();
|
||||
}
|
||||
OBCameraNodeFactory::OBCameraNodeFactory(const std::string &node_name, const std::string &ns,
|
||||
const rclcpp::NodeOptions &node_options)
|
||||
: Node(node_name, ns, node_options),
|
||||
ctx_(std::make_unique<ob::Context>()),
|
||||
logger_(this->get_logger()) {
|
||||
init();
|
||||
}
|
||||
|
||||
OBCameraNodeFactory::~OBCameraNodeFactory() {
|
||||
is_alive_.store(false);
|
||||
if (query_thread_.joinable()) {
|
||||
query_thread_.join();
|
||||
}
|
||||
}
|
||||
void OBCameraNodeFactory::init() {
|
||||
is_alive_.store(true);
|
||||
serial_number_ = declare_parameter<std::string>("serial_number", "BX4NC10000S");
|
||||
query_thread_ = std::thread([=]() {
|
||||
std::chrono::milliseconds timespan(static_cast<int>(reconnect_timeout_ * 1e3));
|
||||
rclcpp::Time first_try_time = this->now();
|
||||
while (is_alive_ && !device_) {
|
||||
CHECK_NOTNULL(ctx_);
|
||||
auto list = ctx_->queryDeviceList();
|
||||
getDevice(list);
|
||||
if (device_) {
|
||||
ctx_->setDeviceChangedCallback([this](std::shared_ptr<ob::DeviceList> removed_list,
|
||||
std::shared_ptr<ob::DeviceList> added_list) {
|
||||
deviceDisconnectCallback(removed_list);
|
||||
deviceConnectCallback(added_list);
|
||||
});
|
||||
startDevice();
|
||||
} else {
|
||||
std::chrono::milliseconds actual_timespan(timespan);
|
||||
if (wait_for_device_timeout_ > 0) {
|
||||
auto time_to_timeout(wait_for_device_timeout_ -
|
||||
(this->get_clock()->now() - first_try_time).seconds());
|
||||
if (time_to_timeout < 0) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "wait for device timeout of " << wait_for_device_timeout_
|
||||
<< " secs expired");
|
||||
exit(1);
|
||||
} else {
|
||||
double max_timespan_secs = static_cast<double>(
|
||||
std::chrono::duration_cast<std::chrono::seconds>(timespan).count());
|
||||
actual_timespan = std::chrono::milliseconds(
|
||||
static_cast<int>(std::min(max_timespan_secs, time_to_timeout) * 1e3));
|
||||
}
|
||||
}
|
||||
std::this_thread::sleep_for(actual_timespan);
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
void OBCameraNodeFactory::deviceConnectCallback(
|
||||
const std::shared_ptr<ob::DeviceList> &device_list) {
|
||||
CHECK_NOTNULL(device_list);
|
||||
if (!device_) {
|
||||
getDevice(device_list);
|
||||
if (device_) {
|
||||
startDevice();
|
||||
} else {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNodeFactory::deviceDisconnectCallback(
|
||||
const std::shared_ptr<ob::DeviceList> &device_list) {
|
||||
CHECK_NOTNULL(device_list);
|
||||
auto dev = device_list->getDeviceBySN(serial_number_.c_str());
|
||||
if (dev) {
|
||||
RCLCPP_ERROR_STREAM(logger_, "The device has been disconnected!");
|
||||
ob_camera_node_.reset(nullptr);
|
||||
device_.reset();
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNodeFactory::getDevice(const std::shared_ptr<ob::DeviceList> &list) {
|
||||
if (device_) {
|
||||
return;
|
||||
}
|
||||
if (0 == list->deviceCount()) {
|
||||
RCLCPP_WARN_STREAM(logger_, "No orbbec devices were found!");
|
||||
return;
|
||||
}
|
||||
for (size_t i = 0; i < list->deviceCount(); i++) {
|
||||
auto dev = list->getDevice(i);
|
||||
if (dev != nullptr) {
|
||||
device_ = dev;
|
||||
RCLCPP_INFO_STREAM(logger_, "get device name " << dev->getDeviceInfo()->name());
|
||||
RCLCPP_INFO_STREAM(logger_, "get device pid " << dev->getDeviceInfo()->pid());
|
||||
RCLCPP_INFO_STREAM(logger_, "get device vid " << dev->getDeviceInfo()->vid());
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"get device serial_name " << dev->getDeviceInfo()->serialNumber());
|
||||
RCLCPP_INFO_STREAM(logger_,
|
||||
"get device firmware version " << dev->getDeviceInfo()->firmwareVersion());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OBCameraNodeFactory::startDevice() {
|
||||
if (ob_camera_node_) {
|
||||
ob_camera_node_.reset();
|
||||
}
|
||||
CHECK_NOTNULL(device_);
|
||||
ob_camera_node_ = std::make_unique<OBCameraNode>(this, device_);
|
||||
}
|
||||
} // namespace orbbec_camera
|
||||
@@ -0,0 +1,133 @@
|
||||
#include "orbbec_camera/utils.h"
|
||||
namespace orbbec_camera {
|
||||
sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
|
||||
OBCameraDistortion distortion) {
|
||||
sensor_msgs::msg::CameraInfo info;
|
||||
info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
|
||||
info.width = intrinsic.width;
|
||||
info.height = intrinsic.height;
|
||||
info.d.resize(5, 0.0);
|
||||
info.d[0] = distortion.k1;
|
||||
info.d[1] = distortion.k2;
|
||||
info.d[2] = distortion.k3;
|
||||
info.d[3] = distortion.k4;
|
||||
info.d[4] = distortion.k5;
|
||||
|
||||
info.k.fill(0.0);
|
||||
info.k[0] = intrinsic.fx;
|
||||
info.k[2] = intrinsic.cx;
|
||||
info.k[4] = intrinsic.fy;
|
||||
info.k[5] = intrinsic.cy;
|
||||
info.k[8] = 1.0;
|
||||
|
||||
info.r.fill(0.0);
|
||||
info.r[0] = 1;
|
||||
info.r[4] = 1;
|
||||
info.r[8] = 1;
|
||||
|
||||
info.p.fill(0.0);
|
||||
info.p[0] = info.k[0];
|
||||
info.p[2] = info.k[2];
|
||||
info.p[5] = info.k[4];
|
||||
info.p[6] = info.k[5];
|
||||
info.p[10] = 1.0;
|
||||
|
||||
return info;
|
||||
}
|
||||
|
||||
void saveRGBPointsToPly(std::shared_ptr<ob::Frame> frame, std::string fileName) {
|
||||
size_t point_size = frame->dataSize() / sizeof(OBColorPoint);
|
||||
FILE *fp = fopen(fileName.c_str(), "wb+");
|
||||
fprintf(fp, "ply\n");
|
||||
fprintf(fp, "format ascii 1.0\n");
|
||||
fprintf(fp, "element vertex %zu\n", point_size);
|
||||
fprintf(fp, "property float x\n");
|
||||
fprintf(fp, "property float y\n");
|
||||
fprintf(fp, "property float z\n");
|
||||
fprintf(fp, "property uchar red\n");
|
||||
fprintf(fp, "property uchar green\n");
|
||||
fprintf(fp, "property uchar blue\n");
|
||||
fprintf(fp, "end_header\n");
|
||||
|
||||
auto *point = (OBColorPoint *)frame->data();
|
||||
for (size_t i = 0; i < point_size; i++) {
|
||||
fprintf(fp, "%.3f %.3f %.3f %d %d %d\n", point->x, point->y, point->z, (int)point->r,
|
||||
(int)point->g, (int)point->b);
|
||||
point++;
|
||||
}
|
||||
|
||||
fflush(fp);
|
||||
fclose(fp);
|
||||
}
|
||||
|
||||
void savePointsToPly(std::shared_ptr<ob::Frame> frame, std::string fileName) {
|
||||
size_t point_size = frame->dataSize() / sizeof(OBPoint);
|
||||
FILE *fp = fopen(fileName.c_str(), "wb+");
|
||||
fprintf(fp, "ply\n");
|
||||
fprintf(fp, "format ascii 1.0\n");
|
||||
fprintf(fp, "element vertex %zu\n", point_size);
|
||||
fprintf(fp, "property float x\n");
|
||||
fprintf(fp, "property float y\n");
|
||||
fprintf(fp, "property float z\n");
|
||||
fprintf(fp, "end_header\n");
|
||||
|
||||
auto *points = (OBPoint *)frame->data();
|
||||
for (size_t i = 0; i < point_size; i++) {
|
||||
fprintf(fp, "%.3f %.3f %.3f\n", points->x, points->y, points->z);
|
||||
points++;
|
||||
}
|
||||
|
||||
fflush(fp);
|
||||
fclose(fp);
|
||||
}
|
||||
|
||||
tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]) {
|
||||
Eigen::Matrix3f m;
|
||||
// We need to be careful about the order, as RS2 rotation matrix is
|
||||
// column-major, while Eigen::Matrix3f expects row-major.
|
||||
m << rotation[0], rotation[3], rotation[6], rotation[1], rotation[4], rotation[7], rotation[2],
|
||||
rotation[5], rotation[8];
|
||||
Eigen::Quaternionf q(m);
|
||||
return {q.x(), q.y(), q.z(), q.w()};
|
||||
}
|
||||
std::ostream &operator<<(std::ostream &os, const OBCameraParam &rhs) {
|
||||
auto depth_intrinsic = rhs.depthIntrinsic;
|
||||
auto rgb_intrinsic = rhs.rgbIntrinsic;
|
||||
os << "=====depth intrinsic=====\n";
|
||||
os << "fx : " << depth_intrinsic.fx << "\n";
|
||||
os << "fy : " << depth_intrinsic.fy << "\n";
|
||||
os << "cx : " << depth_intrinsic.cx << "\n";
|
||||
os << "cy : " << depth_intrinsic.cy << "\n";
|
||||
os << "width : " << depth_intrinsic.width << "\n";
|
||||
os << "height : " << depth_intrinsic.height << "\n";
|
||||
os << "=====rgb intrinsic=====\n";
|
||||
os << "fx : " << rgb_intrinsic.fx << "\n";
|
||||
os << "fy : " << rgb_intrinsic.fy << "\n";
|
||||
os << "cx : " << rgb_intrinsic.cx << "\n";
|
||||
os << "cy : " << rgb_intrinsic.cy << "\n";
|
||||
os << "width : " << rgb_intrinsic.width << "\n";
|
||||
os << "height : " << rgb_intrinsic.height << "\n";
|
||||
return os;
|
||||
}
|
||||
std::string strToLowercase(const std::string &str) {
|
||||
std::string ret;
|
||||
for (char ch : str) {
|
||||
ret += tolower(ch);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
orbbec_camera_msgs::msg::Extrinsics obExtrinsicsToMsg(const OBD2CTransform &extrinsics,
|
||||
const std::string &frame_id) {
|
||||
orbbec_camera_msgs::msg::Extrinsics msg;
|
||||
for (int i = 0; i < 9; ++i) {
|
||||
msg.rotation[i] = extrinsics.rot[i];
|
||||
if (i < 3) {
|
||||
msg.translation[i] = extrinsics.trans[i];
|
||||
}
|
||||
}
|
||||
|
||||
msg.header.frame_id = frame_id;
|
||||
return msg;
|
||||
}
|
||||
} // namespace orbbec_camera
|
||||
Reference in New Issue
Block a user