From a5ab5ec2d03dbfc9b3131e9d0bbd3cd4f11cf3e4 Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 17 Nov 2024 15:45:49 +0200 Subject: [PATCH 1/4] adding read and write data to bagfile --- src/media/ros/ros_file_format.h | 1 + src/media/ros/ros_reader.cpp | 18 ++++++++++++++++++ src/media/ros/ros_writer.cpp | 17 +++++++++++++++++ 3 files changed, 36 insertions(+) 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..99b4125bd6 100644 --- a/src/media/ros/ros_reader.cpp +++ b/src/media/ros/ros_reader.cpp @@ -518,6 +518,24 @@ namespace librealsense data[2] = static_cast(msg->angular_velocity.z); LOG_DEBUG("RS2_STREAM_GYRO " << motion_frame); } + if (stream_id.stream_type == RS2_STREAM_MOTION) + { + 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); + // 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); + 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..d07c887e54 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) + { + const double* data_double = reinterpret_cast(data_ptr); + // 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]; + // 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]; + } else { throw io_exception("Unsupported stream type for a motion frame"); From ddd994bc4b12a460382fde04be69367b1d8e5335 Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Mon, 18 Nov 2024 12:15:58 +0200 Subject: [PATCH 2/4] 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 99b4125bd6..6c6e6e0642 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 d07c887e54..1c31701eda 100644 --- a/src/media/ros/ros_writer.cpp +++ b/src/media/ros/ros_writer.cpp @@ -229,20 +229,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 { From 36336ff2ab1974b2de3790097616dfdcf20cd4b1 Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Mon, 18 Nov 2024 14:24:36 +0200 Subject: [PATCH 3/4] semantic change --- src/media/ros/ros_reader.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/media/ros/ros_reader.cpp b/src/media/ros/ros_reader.cpp index 6c6e6e0642..299b233f1f 100644 --- a/src/media/ros/ros_reader.cpp +++ b/src/media/ros/ros_reader.cpp @@ -522,20 +522,20 @@ namespace librealsense } if (stream_id.stream_type == RS2_STREAM_MOTION) { - auto data = reinterpret_cast(motion_frame->data.data()); + auto data = reinterpret_cast(motion_frame->data.data()); // orientation part - data[0] = msg->orientation.x; - data[1] = msg->orientation.y; - data[2] = msg->orientation.z; - data[3] = msg->orientation.w; + 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[4] = msg->angular_velocity.x; - data[5] = msg->angular_velocity.y; - data[6] = msg->angular_velocity.z; + 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[7] = msg->linear_acceleration.x; - data[8] = msg->linear_acceleration.y; - data[9] = msg->linear_acceleration.z; + 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 From 82b51b1ee591945aafa0d8fa06cfada7b7066366 Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Tue, 19 Nov 2024 09:56:55 +0200 Subject: [PATCH 4/4] bug correction --- src/media/ros/ros_reader.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/media/ros/ros_reader.cpp b/src/media/ros/ros_reader.cpp index 299b233f1f..b98d2ec5e8 100644 --- a/src/media/ros/ros_reader.cpp +++ b/src/media/ros/ros_reader.cpp @@ -520,7 +520,7 @@ namespace librealsense data[2] = static_cast(msg->angular_velocity.z); LOG_DEBUG("RS2_STREAM_GYRO " << motion_frame); } - if (stream_id.stream_type == RS2_STREAM_MOTION) + else if (stream_id.stream_type == RS2_STREAM_MOTION) { auto data = reinterpret_cast(motion_frame->data.data()); // orientation part