diff --git a/src/media/ros/ros_file_format.h b/src/media/ros/ros_file_format.h index 11c9c64a07..24a7c3a6c2 100644 --- 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 index ce5df08463..b98d2ec5e8 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) @@ -518,6 +520,24 @@ namespace librealsense data[2] = static_cast(msg->angular_velocity.z); LOG_DEBUG("RS2_STREAM_GYRO " << motion_frame); } + else if (stream_id.stream_type == RS2_STREAM_MOTION) + { + auto data = reinterpret_cast(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 index c55d38f7da..1c31701eda 100644 --- 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(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");