From 5286d263860a1e732f05dde9e8c140918171ed32 Mon Sep 17 00:00:00 2001 From: SamerKhshiboun Date: Thu, 10 Oct 2024 10:56:47 +0300 Subject: [PATCH] use new APIs for motion, accel and gryo streams --- realsense2_camera/include/ros_utils.h | 2 +- realsense2_camera/src/base_realsense_node.cpp | 65 +++++++++---------- 2 files changed, 30 insertions(+), 37 deletions(-) diff --git a/realsense2_camera/include/ros_utils.h b/realsense2_camera/include/ros_utils.h index 937162dcb..6980d2bf3 100644 --- a/realsense2_camera/include/ros_utils.h +++ b/realsense2_camera/include/ros_utils.h @@ -33,7 +33,7 @@ namespace realsense2_camera const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; const stream_index_pair GYRO{RS2_STREAM_GYRO, 0}; const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0}; - const stream_index_pair IMU{RS2_STREAM_MOTION, 0}; + const stream_index_pair MOTION{RS2_STREAM_MOTION, 0}; bool isValidCharInName(char c); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 72cc1c3ee..11126452b 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -490,9 +490,9 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) { stream_index = ACCEL; } - else if(stream == IMU.first) + else if(stream == MOTION.first) { - stream_index = IMU; + stream_index = MOTION; } else { @@ -514,47 +514,40 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) ImuMessage_AddDefaultValues(imu_msg); imu_msg.header.frame_id = OPTICAL_FRAME_ID(stream_index); - const float3 *crnt_reading = reinterpret_cast(frame.get_data()); - size_t data_size = frame.get_data_size(); - - if (IMU == stream_index) + if (MOTION == stream_index) { - // Expecting two float3 objects: first for accel, second for gyro - // Check if we have at least two float3s - if (data_size >= 2 * sizeof(float3)) - { - const float3 &accel_data = crnt_reading[0]; - const float3 &gyro_data = crnt_reading[1]; + auto combined_motion_data = frame.as().get_combined_motion_data(); + + imu_msg.linear_acceleration.x = combined_motion_data.linear_acceleration.x; + imu_msg.linear_acceleration.y = combined_motion_data.linear_acceleration.y; + imu_msg.linear_acceleration.z = combined_motion_data.linear_acceleration.z; + + imu_msg.angular_velocity.x = combined_motion_data.angular_velocity.x; + imu_msg.angular_velocity.y = combined_motion_data.angular_velocity.y; + imu_msg.angular_velocity.z = combined_motion_data.angular_velocity.z; - // Fill the IMU ROS2 message with both accel and gyro data - imu_msg.linear_acceleration.x = accel_data.x; - imu_msg.linear_acceleration.y = accel_data.y; - imu_msg.linear_acceleration.z = accel_data.z; + imu_msg.orientation.x = combined_motion_data.orientation.x; + imu_msg.orientation.y = combined_motion_data.orientation.y; + imu_msg.orientation.z = combined_motion_data.orientation.z; + imu_msg.orientation.w = combined_motion_data.orientation.w; - imu_msg.angular_velocity.x = gyro_data.x; - imu_msg.angular_velocity.y = gyro_data.y; - imu_msg.angular_velocity.z = gyro_data.z; + } + else + { + auto motion_data = frame.as().get_motion_data(); + if (GYRO == stream_index) + { + imu_msg.angular_velocity.x = motion_data.x; + imu_msg.angular_velocity.y = motion_data.y; + imu_msg.angular_velocity.z = motion_data.z; } - else + else // ACCEL == stream_index { - ROS_ERROR_STREAM("Insufficient data for IMU (Motion) frame: expected at least " - << 2 * sizeof(float3) << " bytes, but got " - << data_size << " bytes."); - return; + imu_msg.linear_acceleration.x = motion_data.x; + imu_msg.linear_acceleration.y = motion_data.y; + imu_msg.linear_acceleration.z = motion_data.z; } } - else if (GYRO == stream_index) - { - imu_msg.angular_velocity.x = crnt_reading->x; - imu_msg.angular_velocity.y = crnt_reading->y; - imu_msg.angular_velocity.z = crnt_reading->z; - } - else // ACCEL == stream_index - { - imu_msg.linear_acceleration.x = crnt_reading->x; - imu_msg.linear_acceleration.y = crnt_reading->y; - imu_msg.linear_acceleration.z = crnt_reading->z; - } imu_msg.header.stamp = t; _imu_publishers[stream_index]->publish(imu_msg);