PR #13519 from remibettan: D555 IMU record and playback

IntelRealSense · Nov 19, 2024 · f8f2b49 · f8f2b49
2 parents 3074e3c + 82b51b1
commit f8f2b49
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 1 deletion.
diff --git a/src/media/ros/ros_file_format.h b/src/media/ros/ros_file_format.h
@@ -417,6 +417,7 @@ namespace librealsense
 
             case RS2_STREAM_GYRO:
             case RS2_STREAM_ACCEL:
+            case RS2_STREAM_MOTION:
                 return ros_imu_type_str();
 
             case RS2_STREAM_POSE:

diff --git a/src/media/ros/ros_reader.cpp b/src/media/ros/ros_reader.cpp
@@ -486,9 +486,11 @@ namespace librealsense
             get_frame_metadata(m_file, info_topic, stream_id, motion_data, additional_data);
         }
 
+        size_t size_of_imu_data = (stream_id.stream_type == RS2_STREAM_MOTION) ? sizeof(rs2_combined_motion) : 3 * sizeof(float);
+
         frame_interface * frame = m_frame_source->alloc_frame(
             { stream_id.stream_type, stream_id.stream_index, RS2_EXTENSION_MOTION_FRAME },
-            3 * sizeof( float ),
+            size_of_imu_data,
             std::move( additional_data ),
             true );
         if (frame == nullptr)
@@ -518,6 +520,24 @@ namespace librealsense
             data[2] = static_cast<float>(msg->angular_velocity.z);
             LOG_DEBUG("RS2_STREAM_GYRO " << motion_frame);
         }
+        else if (stream_id.stream_type == RS2_STREAM_MOTION)
+        {
+            auto data = reinterpret_cast<rs2_combined_motion*>(motion_frame->data.data());
+            // orientation part
+            data->orientation.x = msg->orientation.x;
+            data->orientation.y = msg->orientation.y;
+            data->orientation.z = msg->orientation.z;
+            data->orientation.w = msg->orientation.w;
+            // GYRO part
+            data->angular_velocity.x = msg->angular_velocity.x;
+            data->angular_velocity.y = msg->angular_velocity.y;
+            data->angular_velocity.z = msg->angular_velocity.z;
+            // ACCEL part
+            data->linear_acceleration.x = msg->linear_acceleration.x;
+            data->linear_acceleration.y = msg->linear_acceleration.y;
+            data->linear_acceleration.z = msg->linear_acceleration.z;
+            LOG_DEBUG("RS2_STREAM_MOTION " << motion_frame);
+        }
         else
         {
             throw io_exception( rsutils::string::from() << "Unsupported stream type " << stream_id.stream_type );

diff --git a/src/media/ros/ros_writer.cpp b/src/media/ros/ros_writer.cpp
@@ -227,6 +227,23 @@ namespace librealsense
             imu_msg.angular_velocity.y = data_ptr[1];
             imu_msg.angular_velocity.z = data_ptr[2];
         }
+        else if (stream_id.stream_type == RS2_STREAM_MOTION)
+        {
+            auto data_imu = *reinterpret_cast<const rs2_combined_motion*>(frame.frame->get_frame_data());
+            // orientation part
+            imu_msg.orientation.x = data_imu.orientation.x;
+            imu_msg.orientation.y = data_imu.orientation.y;
+            imu_msg.orientation.z = data_imu.orientation.z;
+            imu_msg.orientation.w = data_imu.orientation.w;
+            // GYRO part
+            imu_msg.angular_velocity.x = data_imu.angular_velocity.x;
+            imu_msg.angular_velocity.y = data_imu.angular_velocity.y;
+            imu_msg.angular_velocity.z = data_imu.angular_velocity.z;
+            // ACCEL part
+            imu_msg.linear_acceleration.x = data_imu.linear_acceleration.x;
+            imu_msg.linear_acceleration.y = data_imu.linear_acceleration.y;
+            imu_msg.linear_acceleration.z = data_imu.linear_acceleration.z;
+        }
         else
         {
             throw io_exception("Unsupported stream type for a motion frame");