enabling record and playback

IntelRealSense · Nov 18, 2024 · ca1b9f4 · ca1b9f4
1 parent bc2312e
commit ca1b9f4
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 24 deletions.
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)
@@ -522,18 +524,18 @@ namespace librealsense
         {
             auto data = reinterpret_cast<double*>(motion_frame->data.data());
             // orientation part
-            data[0] = static_cast<float>(msg->orientation.x);
-            data[1] = static_cast<float>(msg->orientation.y);
-            data[2] = static_cast<float>(msg->orientation.z);
-            data[3] = static_cast<float>(msg->orientation.w);
-            // ACCEL part
-            data[4] = static_cast<float>(msg->linear_acceleration.x);
-            data[5] = static_cast<float>(msg->linear_acceleration.y);
-            data[6] = static_cast<float>(msg->linear_acceleration.z);
+            data[0] = msg->orientation.x;
+            data[1] = msg->orientation.y;
+            data[2] = msg->orientation.z;
+            data[3] = msg->orientation.w;
             // GYRO part
-            data[7] = static_cast<float>(msg->angular_velocity.x);
-            data[8] = static_cast<float>(msg->angular_velocity.y);
-            data[9] = static_cast<float>(msg->angular_velocity.z);
+            data[4] = msg->angular_velocity.x;
+            data[5] = msg->angular_velocity.y;
+            data[6] = msg->angular_velocity.z;
+            // ACCEL part
+            data[7] = msg->linear_acceleration.x;
+            data[8] = msg->linear_acceleration.y;
+            data[9] = msg->linear_acceleration.z;
             LOG_DEBUG("RS2_STREAM_MOTION " << motion_frame);
         }
         else

diff --git a/src/media/ros/ros_writer.cpp b/src/media/ros/ros_writer.cpp
@@ -228,20 +228,20 @@ namespace librealsense
         }
         else if (stream_id.stream_type == RS2_STREAM_MOTION)
         {
-            const double* data_double = reinterpret_cast<const double*>(data_ptr);
+            auto data_imu = *reinterpret_cast<const rs2_combined_motion*>(frame.frame->get_frame_data());
             // orientation part
-            imu_msg.orientation.x = data_double[0];
-            imu_msg.orientation.y = data_double[1];
-            imu_msg.orientation.z = data_double[2];
-            imu_msg.orientation.w = data_double[3];
-            // ACCEL part
-            imu_msg.linear_acceleration.x = data_double[4];
-            imu_msg.linear_acceleration.y = data_double[5];
-            imu_msg.linear_acceleration.z = data_double[6];
+            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_double[7];
-            imu_msg.angular_velocity.y = data_double[8];
-            imu_msg.angular_velocity.z = data_double[9];
+            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
         {