use new APIs for motion, accel and gryo streams

SamerKhshiboun · Oct 10, 2024 · 5286d26 · 5286d26
1 parent c586628
commit 5286d26
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 37 deletions.
diff --git 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
@@ -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<const float3 *>(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<rs2::motion_frame>().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<rs2::motion_frame>().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);