i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 12:07:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

remove debug msg

Browse Source
This commit is contained in:
Joe Dong
2022-06-08 13:41:31 +08:00
parent fffab59240
commit ad4cb96b0c
4 changed files with 66 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+9 -20
View File
@@ -38,10 +38,17 @@ OBCameraNode::OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> devic
startPipeline();
}
OBCameraNode::~OBCameraNode() {
OBCameraNode::~OBCameraNode() { clean(); }
void OBCameraNode::clean() {
RCLCPP_WARN_STREAM(logger_, "Do destroy ~OBCameraNode");
is_running_.store(false);
if (tf_thread_->joinable()) {
tf_thread_->join();
}
RCLCPP_WARN_STREAM(logger_, "stop pipeline");
pipeline_->stop();
RCLCPP_WARN_STREAM(logger_, "Destroy ~OBCameraNode DONE");
}
void OBCameraNode::setupDevices() {
@@ -181,6 +188,7 @@ void OBCameraNode::setupPublishers() {
void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
#if 0
// NOTE: This block code only for debug, it will be crazy slowly
static int cnt = 0;
const std::string home_dir = std::getenv("HOME");
const std::string pc_file_name = home_dir + "/pc/point_cloud.ply";
@@ -195,11 +203,6 @@ void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
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);
}
}
#endif
@@ -207,7 +210,6 @@ void OBCameraNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
auto start = rclcpp::Clock().now();
publishColorPointCloud(frame_set);
auto end = rclcpp::Clock().now();
RCLCPP_INFO_STREAM(logger_, "process point cloud cost " << (end - start).seconds());
}
}
@@ -307,32 +309,19 @@ void OBCameraNode::publishColorPointCloud(std::shared_ptr<ob::FrameSet> frame_se
point_cloud_publisher_->publish(point_cloud_msg_);
}
void OBCameraNode::frameSetCallback(std::shared_ptr<ob::FrameSet> frame_set) {
auto start = rclcpp::Clock().now();
rclcpp::Time t = rclcpp::Clock().now();
auto color_frame = frame_set->colorFrame();
auto depth_frame = frame_set->depthFrame();
auto ir_frame = frame_set->irFrame();
if (color_frame && enable_[COLOR]) {
auto color_start = rclcpp::Clock().now();
publishColorFrame(color_frame);
RCLCPP_INFO_STREAM(
logger_, "publishColorFrame cost " << (rclcpp::Clock().now() - color_start).seconds());
}
if (depth_frame && enable_[DEPTH]) {
auto depth_start = rclcpp::Clock().now();
publishDepthFrame(depth_frame);
RCLCPP_INFO_STREAM(
logger_, "publishDepthFrame cost " << (rclcpp::Clock().now() - depth_start).seconds());
}
if (ir_frame && enable_[IR0]) {
auto ir_start = rclcpp::Clock().now();
publishIRFrame(ir_frame);
RCLCPP_INFO_STREAM(logger_,
"publishIRFrame cost " << (rclcpp::Clock().now() - ir_start).seconds());
}
publishPointCloud(frame_set);
auto end = rclcpp::Clock().now();
RCLCPP_INFO_STREAM(logger_, "frameSetCallback cost " << (end - start).seconds());
}
std::optional<OBCameraParam> OBCameraNode::findDefaultCameraParam() {
auto camera_params = device_->getCalibrationCameraParamList();
orbbec_camera/src/ob_camera_node_factory.cpp
+51 -20
View File
@@ -21,8 +21,9 @@ OBCameraNodeFactory::~OBCameraNodeFactory() {
}
}
void OBCameraNodeFactory::init() {
ctx_->setLoggerSeverity(OB_LOG_SEVERITY_NONE);
is_alive_.store(true);
serial_number_ = declare_parameter<std::string>("serial_number", "BX4NC10000S");
serial_number_ = declare_parameter<std::string>("serial_number", "");
query_thread_ = std::thread([=]() {
std::chrono::milliseconds timespan(static_cast<int>(reconnect_timeout_ * 1e3));
rclcpp::Time first_try_time = this->now();
@@ -61,6 +62,10 @@ void OBCameraNodeFactory::init() {
void OBCameraNodeFactory::deviceConnectCallback(
const std::shared_ptr<ob::DeviceList> &device_list) {
if (device_list->deviceCount() == 0) {
return;
}
RCLCPP_ERROR_STREAM(logger_, "deviceConnectCallback");
CHECK_NOTNULL(device_list);
if (!device_) {
getDevice(device_list);
@@ -73,13 +78,38 @@ void OBCameraNodeFactory::deviceConnectCallback(
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();
if (device_list->deviceCount() == 0) {
return;
}
RCLCPP_ERROR_STREAM(logger_, "deviceDisconnectCallback");
CHECK_NOTNULL(device_list);
ob_camera_node_.reset(nullptr);
device_.reset();
// try {
// for (size_t i = 0; i < device_list->deviceCount(); i++) {
// auto dev = device_list->getDevice(i);
// std::string sn1 = dev->getDeviceInfo()->serialNumber();
// std::string sn2 = device_->getDeviceInfo()->serialNumber();
// if (sn1 == sn2) {
// RCLCPP_ERROR(logger_, "The device with SN %s was disconnected!", sn1.c_str());
// ob_camera_node_.reset(nullptr);
// device_.reset();
// }
// }
// } catch (const ob::Error &e) {
// RCLCPP_ERROR_STREAM(logger_, e.getMessage());
// }
}
void OBCameraNodeFactory::printDeviceInfo(const std::shared_ptr<ob::DeviceInfo> &device_info) {
RCLCPP_INFO_STREAM(logger_, "name " << device_info->name());
RCLCPP_INFO_STREAM(logger_, "pid " << device_info->pid());
RCLCPP_INFO_STREAM(logger_, "vid " << device_info->vid());
RCLCPP_INFO_STREAM(logger_, "serial_name " << device_info->serialNumber());
RCLCPP_INFO_STREAM(logger_, "firmware version " << device_info->firmwareVersion());
RCLCPP_INFO_STREAM(logger_,
"supported min sdk version " << device_info->supportedMinSdkVersion());
RCLCPP_INFO_STREAM(logger_, "hardware version " << device_info->hardwareVersion());
}
void OBCameraNodeFactory::getDevice(const std::shared_ptr<ob::DeviceList> &list) {
@@ -90,19 +120,20 @@ void OBCameraNodeFactory::getDevice(const std::shared_ptr<ob::DeviceList> &list)
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_, "device name " << dev->getDeviceInfo()->name());
RCLCPP_INFO_STREAM(logger_, "device pid " << dev->getDeviceInfo()->pid());
RCLCPP_INFO_STREAM(logger_, "device vid " << dev->getDeviceInfo()->vid());
RCLCPP_INFO_STREAM(logger_, "device serial_name " << dev->getDeviceInfo()->serialNumber());
RCLCPP_INFO_STREAM(logger_,
"device firmware version " << dev->getDeviceInfo()->firmwareVersion());
RCLCPP_INFO_STREAM(logger_, "device supported min sdk version "
<< dev->getDeviceInfo()->supportedMinSdkVersion());
RCLCPP_INFO_STREAM(logger_, "device hardware version " << dev->getDeviceInfo()->hardwareVersion());
if (serial_number_.empty()) {
for (size_t i = 0; i < list->deviceCount(); i++) {
auto dev = list->getDevice(i);
if (dev != nullptr) {
device_ = dev;
printDeviceInfo(device_->getDeviceInfo());
}
}
} else {
device_ = list->getDeviceBySN(serial_number_.c_str());
if (device_ == nullptr) {
RCLCPP_ERROR_STREAM(logger_, "can not found device with SN " << serial_number_);
} else {
printDeviceInfo(device_->getDeviceInfo());
}
}
}