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

Update the point cloud y axis

Browse Source
This commit is contained in:
jj
2025-06-11 14:07:11 +08:00
parent 86669cfdc7
commit 721d07d1fb
3 changed files with 29 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node_driver.cpp
+23 -5
View File
@@ -105,7 +105,9 @@ OBCameraNodeDriver::~OBCameraNodeDriver() {
reset_device_cond_.notify_all();
reset_device_thread_->join();
}
ob_camera_node_->stopGmslTrigger();
if (ob_camera_node_) {
ob_camera_node_->stopGmslTrigger();
}
if (orb_device_lock_shm_fd_ != -1) {
close(orb_device_lock_shm_fd_);
orb_device_lock_shm_fd_ = -1;
@@ -247,6 +249,8 @@ void OBCameraNodeDriver::checkConnectTimer() {
return;
} else if (!ob_camera_node_) {
device_connected_.store(false);
} else if (!ob_lidar_node_) {
device_connected_.store(false);
}
}
@@ -278,8 +282,11 @@ void OBCameraNodeDriver::resetDevice() {
RCLCPP_INFO_STREAM(logger_, "resetDevice : Reset device uid: " << device_unique_id_);
std::lock_guard<decltype(device_lock_)> device_lock(device_lock_);
{
ob_camera_node_.reset();
ob_lidar_node_.reset();
if (ob_camera_node_) {
ob_camera_node_.reset();
} else if (ob_lidar_node_) {
ob_lidar_node_.reset();
}
device_.reset();
device_info_.reset();
device_connected_ = false;
@@ -300,7 +307,12 @@ void OBCameraNodeDriver::rebootDeviceCallback(
return;
}
RCLCPP_INFO(logger_, "Reboot device");
ob_camera_node_->rebootDevice();
if (ob_camera_node_) {
ob_camera_node_->rebootDevice();
} else if (ob_lidar_node_) {
ob_lidar_node_->rebootDevice();
}
device_connected_ = false;
device_ = nullptr;
}
@@ -438,6 +450,8 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
CHECK_NOTNULL(device_.get());
if (ob_camera_node_) {
ob_camera_node_.reset();
} else if (ob_lidar_node_) {
ob_lidar_node_.reset();
}
int retry_count = 0;
constexpr int max_retries = 3;
@@ -757,7 +771,11 @@ void OBCameraNodeDriver::firmwareUpdateCallback(OBFwUpdateState state, const cha
std::cout << "Message : " << message << std::endl << std::flush;
if (state == STAT_DONE) {
RCLCPP_INFO(logger_, "Reboot device");
ob_camera_node_->rebootDevice();
if (ob_camera_node_) {
ob_camera_node_.reset();
} else if (ob_lidar_node_) {
ob_lidar_node_.reset();
}
device_connected_ = false;
upgrade_firmware_ = "";
}
orbbec_camera/src/ob_lidar_node.cpp
+3 -3
View File
@@ -454,7 +454,7 @@ void OBLidarNode::publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set) {
for (size_t i = 0; i < point_count;
++i, ++iter_x, ++iter_y, ++iter_z, ++iter_reflectivity, ++iter_tag) {
*iter_x = static_cast<float>(point_data[i].x / 1000.0);
*iter_y = static_cast<float>(point_data[i].y / 1000.0);
*iter_y = static_cast<float>(point_data[i].y / -1000.0);
*iter_z = static_cast<float>(point_data[i].z / 1000.0);
*iter_reflectivity = point_data[i].reflectivity;
*iter_tag = point_data[i].tag;
@@ -498,7 +498,7 @@ void OBLidarNode::publishSpherePointCloud(std::shared_ptr<ob::FrameSet> frame_se
for (size_t i = 0; i < point_count;
++i, ++iter_x, ++iter_y, ++iter_z, ++iter_reflectivity, ++iter_tag) {
*iter_x = static_cast<float>(result_point[i].x / 1000.0);
*iter_y = static_cast<float>(result_point[i].y / 1000.0);
*iter_y = static_cast<float>(result_point[i].y / -1000.0);
*iter_z = static_cast<float>(result_point[i].z / 1000.0);
*iter_reflectivity = result_point[i].reflectivity;
*iter_tag = result_point[i].tag;
@@ -529,7 +529,7 @@ std::vector<OBLiDARPoint> OBLidarNode::spherePointToPoint(OBLiDARSpherePoint *sp
for (uint32_t i = 0; i < point_count; ++i) {
double theta_rad = sphere_point->theta * M_PI / 180.0f; // to unit rad
double phi_rad = sphere_point->phi * M_PI / 180.0f; // to unit rad
auto distance = sphere_point->distance / 1000; // mm to m
auto distance = sphere_point->distance;
auto x = static_cast<float>(distance * cos(theta_rad) * cos(phi_rad));
auto y = static_cast<float>(distance * sin(theta_rad) * cos(phi_rad));
auto z = static_cast<float>(distance * sin(phi_rad));