Add lidar to imu static tf publisher

orbbec_camera/include/orbbec_camera/cloud_accumulated.h

@@ -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

@@ -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

@@ -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());
   }
 }