Update accel_gyro_frame_id

orbbec · Dec 24, 2024 · 9752192 · 9752192
1 parent 056185e
commit 9752192
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 13 deletions.
diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
@@ -353,7 +353,7 @@ class OBCameraNode {
   std::unique_ptr<ob::Pipeline> imuPipeline_ = nullptr;
   std::atomic_bool pipeline_started_{false};
   std::string camera_name_ = "camera";
-  const std::string imu_optical_frame_id_ = "camera_gyro_optical_frame";
+  std::string accel_gyro_frame_id_ = "camera_accel_gyro_optical_frame";
   const std::string imu_frame_id_ = "camera_gyro_frame";
   std::shared_ptr<ob::Config> pipeline_config_ = nullptr;
   std::map<stream_index_pair, std::shared_ptr<ob::Sensor>> sensors_;

diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp
@@ -1039,6 +1039,8 @@ void OBCameraNode::getParameters() {
         camera_name_ + "_" + stream_name_[COLOR] + "_optical_frame";
   }
 
+  accel_gyro_frame_id_ = camera_name_ + "_accel_gyro_optical_frame";
+
   setAndGetNodeParameter(publish_tf_, "publish_tf", true);
   setAndGetNodeParameter(tf_publish_rate_, "tf_publish_rate", 0.0);
   setAndGetNodeParameter(depth_registration_, "depth_registration", false);
@@ -2208,32 +2210,32 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
   imu_msg.header.frame_id = optical_frame_id_[GYRO];
   auto frame_timestamp = getFrameTimestampUs(accelframe);
   auto timestamp = fromUsToROSTime(frame_timestamp);
-  imu_msg.header.stamp = timestamp; 
-  
+  imu_msg.header.stamp = timestamp;
+
   auto gyro_info = createIMUInfo(GYRO);
   gyro_info.header = imu_msg.header;
   imu_info_publishers_[GYRO]->publish(gyro_info);
-  
+
   auto accel_info = createIMUInfo(ACCEL);
   imu_msg.header.frame_id = optical_frame_id_[ACCEL];
   accel_info.header = imu_msg.header;
   imu_info_publishers_[ACCEL]->publish(accel_info);
-  
-  imu_msg.header.frame_id = imu_optical_frame_id_;
-  
+
+  imu_msg.header.frame_id = accel_gyro_frame_id_;
+
   auto gyro_frame = gryoframe->as<ob::GyroFrame>();
   auto gyroData = gyro_frame->value();
   imu_msg.angular_velocity.x = gyroData.x - gyro_info.bias[0];
   imu_msg.angular_velocity.y = gyroData.y - gyro_info.bias[1];
   imu_msg.angular_velocity.z = gyroData.z - gyro_info.bias[2];
-  
+
   auto accel_frame = accelframe->as<ob::AccelFrame>();
   auto accelData = accel_frame->value();
-  
+
   imu_msg.linear_acceleration.x = accelData.x - accel_info.bias[0];
   imu_msg.linear_acceleration.y = accelData.y - accel_info.bias[1];
   imu_msg.linear_acceleration.z = accelData.z - accel_info.bias[2];
-  
+
   imu_gyro_accel_publisher_->publish(imu_msg);
 }
 
@@ -2255,15 +2257,15 @@ void OBCameraNode::onNewIMUFrameCallback(const std::shared_ptr<ob::Frame> &frame
   }
   auto imu_msg = sensor_msgs::msg::Imu();
   setDefaultIMUMessage(imu_msg);
-  
+
   imu_msg.header.frame_id = optical_frame_id_[stream_index];
   auto timestamp = fromUsToROSTime(frame->timeStampUs());
   imu_msg.header.stamp = timestamp;
-  
+
   auto imu_info = createIMUInfo(stream_index);
   imu_info.header = imu_msg.header;
   imu_info_publishers_[stream_index]->publish(imu_info);
-  
+
   if (frame->type() == OB_FRAME_GYRO) {
     auto gyro_frame = frame->as<ob::GyroFrame>();
     auto data = gyro_frame->value();