Fix the imu_info frame_id bug and initialize the quaternion's w value to 1.0

Author: obdatean

orbbec_camera/src/ob_camera_node.cpp

@@ -2304,26 +2304,34 @@ void OBCameraNode::onNewIMUFrameSyncOutputCallback(const std::shared_ptr<ob::Fra
   auto imu_msg = sensor_msgs::msg::Imu();
   setDefaultIMUMessage(imu_msg);
 
-  imu_msg.header.frame_id = imu_optical_frame_id_;
+  imu_msg.header.frame_id = optical_frame_id_[GYRO];
   auto frame_timestamp = getFrameTimestampUs(accelframe);
   auto timestamp = fromUsToROSTime(frame_timestamp);
   imu_msg.header.stamp = timestamp;
-  auto gyro_frame = gryoframe->as<ob::GyroFrame>();
+
   auto gyro_info = createIMUInfo(GYRO);
   gyro_info.header = imu_msg.header;
-  gyro_info.header.frame_id = imu_optical_frame_id_;
   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_;
+
+  auto gyro_frame = gryoframe->as<ob::GyroFrame>();
   auto gyroData = gyro_frame->getValue();
   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->getValue();
-  auto accel_info = createIMUInfo(ACCEL);
   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_info_publishers_[ACCEL]->publish(accel_info);
+
   imu_gyro_accel_publisher_->publish(imu_msg);
 }
 
@@ -2345,14 +2353,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->getTimeStampUs());
-
   imu_msg.header.stamp = timestamp;
+
   auto imu_info = createIMUInfo(stream_index);
   imu_info.header = imu_msg.header;
-  imu_info.header.frame_id = imu_optical_frame_id_;
   imu_info_publishers_[stream_index]->publish(imu_info);
+
   if (frame->getType() == OB_FRAME_GYRO) {
     auto gyro_frame = frame->as<ob::GyroFrame>();
     auto data = gyro_frame->getValue();
@@ -2377,7 +2386,7 @@ void OBCameraNode::setDefaultIMUMessage(sensor_msgs::msg::Imu &imu_msg) {
   imu_msg.orientation.x = 0.0;
   imu_msg.orientation.y = 0.0;
   imu_msg.orientation.z = 0.0;
-  imu_msg.orientation.w = 0.0;
+  imu_msg.orientation.w = 1.0;
 
   imu_msg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
   imu_msg.linear_acceleration_covariance = {
@@ -2761,7 +2770,7 @@ orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo(
   orbbec_camera_msgs::msg::IMUInfo imu_info;
   imu_info.header.frame_id = optical_frame_id_[stream_index];
   imu_info.header.stamp = node_->now();
-  auto imu_profile = stream_profile_[stream_index];
+
   if (stream_index == GYRO) {
     auto gyro_profile = stream_profile_[stream_index]->as<ob::GyroStreamProfile>();
     auto gyro_intrinsics = gyro_profile->getIntrinsic();