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

Add lidar to imu static tf publisher

Browse Source
This commit is contained in:
xiexun
2025-09-17 12:22:23 +08:00
parent b3c134d693
commit a4abeecd72
3 changed files with 13 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/cloud_accumulated.h
-34
View File
@@ -1,34 +0,0 @@
/*******************************************************************************
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#pragma once
#include <rclcpp/rclcpp.hpp>
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "utils.h"
namespace orbbec_camera {
namespace orbbec_lidar {
class CloudAccumulated {
public:
explicit CloudAccumulated(rclcpp::Node* const node, rmw_qos_profile_t cloud_qos,
int cloud_accumulation_count);
~CloudAccumulated();
private:
rclcpp::Node* node_;
rclcpp::Logger logger_;
};
} // namespace orbbec_lidar
} // namespace orbbec_camera
orbbec_camera/src/cloud_accumulated.cpp
-78
View File
@@ -1,78 +0,0 @@
/*******************************************************************************
* Copyright (c) 2023 Orbbec 3D Technology, Inc
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/
#if defined(ROS_JAZZY) || defined(ROS_IRON)
#include <cv_bridge/cv_bridge.hpp>
#else
#include <cv_bridge/cv_bridge.h>
#endif
#include "orbbec_camera/cloud_accumulated.h"
namespace orbbec_camera {
namespace orbbec_lidar {
CloudAccumulated::CloudAccumulated(rclcpp::Node* const node, rmw_qos_profile_t cloud_qos,
int cloud_accumulation_count): node_(node), logger_(rclcpp::get_logger("cloud_accumulated")) {
cloud_sub_=this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/points", cloud_qos, std::bind(&CloudAccumulated::cloud_callback, this, _1));
}
} // namespace orbbec_lidar
D2CViewer::D2CViewer(rclcpp::Node* const node, rmw_qos_profile_t rgb_qos,
rmw_qos_profile_t depth_qos)
: node_(node), logger_(rclcpp::get_logger("d2c_viewer")) {
rgb_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
node_, "color/image_raw", rgb_qos);
depth_sub_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
node_, "depth/image_raw", depth_qos);
sync_ = std::make_shared<message_filters::Synchronizer<MySyncPolicy>>(MySyncPolicy(10), *rgb_sub_,
*depth_sub_);
sync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(1.0)); // 1s
using std::placeholders::_1;
using std::placeholders::_2;
sync_->registerCallback(std::bind(&D2CViewer::messageCallback, this, _1, _2));
d2c_viewer_pub_ =
node_->create_publisher<sensor_msgs::msg::Image>("depth_to_color/image_raw", rclcpp::QoS(1));
}
D2CViewer::~D2CViewer() = default;
void D2CViewer::messageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg,
const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg) {
if (rgb_msg->width != depth_msg->width || rgb_msg->height != depth_msg->height) {
RCLCPP_ERROR(logger_, "rgb and depth image size not match(%d, %d) vs (%d, %d)", rgb_msg->width,
rgb_msg->height, depth_msg->width, depth_msg->height);
return;
}
auto rgb_encode = (rgb_msg->step == 5760) ? sensor_msgs::image_encodings::RGB8
: sensor_msgs::image_encodings::RGBA8;
auto gray_type = (rgb_msg->step == 5760) ? cv::COLOR_GRAY2RGB : cv::COLOR_GRAY2RGBA;
auto rgb_img_ptr = cv_bridge::toCvCopy(rgb_msg, rgb_encode);
auto depth_img_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1);
cv::Mat gray_depth, depth_img, d2c_img;
depth_img_ptr->image.convertTo(gray_depth, CV_8UC1);
cv::cvtColor(gray_depth, depth_img, gray_type);
depth_img.setTo(cv::Scalar(255, 255, 0), depth_img);
cv::bitwise_or(rgb_img_ptr->image, depth_img, d2c_img);
sensor_msgs::msg::Image::SharedPtr d2c_msg =
cv_bridge::CvImage(std_msgs::msg::Header(), rgb_encode, d2c_img).toImageMsg();
if (d2c_msg != nullptr) {
d2c_msg->header = rgb_msg->header;
d2c_viewer_pub_->publish(*d2c_msg);
} else {
RCLCPP_ERROR(logger_,
"-----------------------d2c_viewer publishing failed-----------------------");
}
}
} // namespace orbbec_camera
orbbec_camera/src/ob_lidar_node.cpp
+13 -7
View File
@@ -55,11 +55,6 @@ OBLidarNode::OBLidarNode(rclcpp::Node *node, std::shared_ptr<ob::Device> device,
jpeg_decoder_ = std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR], height_[COLOR]);
#endif
is_camera_node_initialized_ = true;
// if (enable_cloud_accumulated_ && cloud_accumulation_count_ >= 1) {
// auto point_cloud_qos_profile = getRMWQosProfileFromString(point_cloud_qos_);
// cloud_accumulated_ = std::make_unique<CloudAccumulated>(node_, point_cloud_qos_profile,
// cloud_accumulation_count_);
// }
}
template <class T>
@@ -156,8 +151,6 @@ void OBLidarNode::getParameters() {
setAndGetNodeParameter<int>(repetitive_scan_mode_, "repetitive_scan_mode", -1);
setAndGetNodeParameter<int>(filter_level_, "filter_level", -1);
setAndGetNodeParameter<float>(vertical_fov_, "vertical_fov", -1.0);
setAndGetNodeParameter<bool>(enable_cloud_accumulated_, "enable_cloud_accumulated", false);
setAndGetNodeParameter<int>(cloud_accumulation_count_, "cloud_accumulation_count", -1);
setAndGetNodeParameter<bool>(enable_imu_, "enable_imu", false);
setAndGetNodeParameter<std::string>(imu_rate_, "imu_rate", "50hz");
setAndGetNodeParameter<std::string>(accel_range_, "accel_range", "2g");
@@ -1264,6 +1257,19 @@ void OBLidarNode::calcAndPublishStaticTransform() {
auto ex_msg = obExtrinsicsToMsg(ex, frame_id);
CHECK_NOTNULL(lidar_to_imu_extrinsics_publisher_);
lidar_to_imu_extrinsics_publisher_->publish(ex_msg);
// Publish static TF from lidar to IMU
auto Q = rotationMatrixToQuaternion(ex.rot);
Q = quaternion_optical * Q * quaternion_optical.inverse();
tf2::Vector3 trans(ex.trans[0], ex.trans[1], ex.trans[2]);
auto timestamp = node_->now();
publishStaticTF(timestamp, trans, Q, frame_id_[base_stream_], accel_gyro_frame_id_);
RCLCPP_INFO_STREAM(logger_, "Publishing static transform from " << frame_id_[base_stream_]
<< " to " << accel_gyro_frame_id_);
RCLCPP_INFO_STREAM(logger_, "Translation " << trans[0] << ", " << trans[1] << ", " << trans[2]);
RCLCPP_INFO_STREAM(logger_, "Rotation " << Q.getX() << ", " << Q.getY() << ", " << Q.getZ()
<< ", " << Q.getW());
}
}