From ca1b9f423c97841bf4e0c581a2fcb758e79657ec Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Mon, 18 Nov 2024 12:15:58 +0200 Subject: [PATCH] enabling record and playback --- src/media/ros/ros_reader.cpp | 26 ++++++++++++++------------ src/media/ros/ros_writer.cpp | 24 ++++++++++++------------ 2 files changed, 26 insertions(+), 24 deletions(-) diff --git a/src/media/ros/ros_reader.cpp b/src/media/ros/ros_reader.cpp index 99b4125bd67..6c6e6e0642e 100644 --- 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(motion_frame->data.data()); // orientation part - data[0] = static_cast(msg->orientation.x); - data[1] = static_cast(msg->orientation.y); - data[2] = static_cast(msg->orientation.z); - data[3] = static_cast(msg->orientation.w); - // ACCEL part - data[4] = static_cast(msg->linear_acceleration.x); - data[5] = static_cast(msg->linear_acceleration.y); - data[6] = static_cast(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(msg->angular_velocity.x); - data[8] = static_cast(msg->angular_velocity.y); - data[9] = static_cast(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 index 74642ad910f..2fe7ec0786a 100644 --- 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(data_ptr); + auto data_imu = *reinterpret_cast(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 {