Enable linear interpolate of Gyro and Accel data over IMU topic

realsense_ros/CMakeLists.txt

@@ -65,6 +65,7 @@ set(CMAKE_CXX_FLAGS "-fPIE -fPIC -D_FORTIFY_SOURCE=2 -fstack-protector -Wformat
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(rcl REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(nav_msgs REQUIRED)
@@ -100,6 +101,7 @@ target_link_libraries(${PROJECT_NAME}
 )
 
 ament_target_dependencies(${PROJECT_NAME}
+  rcl
   rclcpp
   rclcpp_components
   nav_msgs
@@ -133,6 +135,7 @@ install(
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+  find_package(rcl REQUIRED)
   find_package(rclcpp REQUIRED)
   find_package(nav_msgs REQUIRED)
   find_package(sensor_msgs REQUIRED)
@@ -162,6 +165,7 @@ if(BUILD_TESTING)
         realsense2
 	${PROJECT_NAME})
       ament_target_dependencies(${target}
+        "rcl"
         "rclcpp"
         "nav_msgs"
         "sensor_msgs"

realsense_ros/include/realsense/rs_base.hpp

@@ -80,6 +80,8 @@ class RealSenseBase
   Result toggleStream(const stream_index_pair & stream, const rclcpp::Parameter & param);
   Result changeResolution(const stream_index_pair & stream, const rclcpp::Parameter & param);
   Result changeFPS(const stream_index_pair & stream, const rclcpp::Parameter & param);
+  rclcpp::Time frameToTime(const rs2::frame & frame);
+  rclcpp::Time msToTime(const double &ms);
 
   typedef struct VideoStreamInfo
   {
@@ -106,7 +108,7 @@ class RealSenseBase
   rclcpp::TimerBase::SharedPtr timer_;
   std::map<stream_index_pair, bool> enable_ = {{COLOR, false}, {DEPTH, false},
                                                {INFRA1, false}, {INFRA2, false},
-                                               {ACCEL, false}, {GYRO, false},
+                                               {ACCEL, false}, {GYRO, false}, {IMU, false},
                                                {FISHEYE1, false}, {FISHEYE2, false},
                                                {POSE, false}};
 

realsense_ros/include/realsense/rs_constants.hpp

@@ -32,8 +32,9 @@ namespace realsense
   const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2};
   const stream_index_pair FISHEYE1{RS2_STREAM_FISHEYE, 1};
   const stream_index_pair FISHEYE2{RS2_STREAM_FISHEYE, 2};
-  const stream_index_pair GYRO{RS2_STREAM_GYRO, 0};
   const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0};
+  const stream_index_pair GYRO{RS2_STREAM_GYRO, 0};
+  const stream_index_pair IMU{RS2_STREAM_ANY, 0};
   const stream_index_pair POSE{RS2_STREAM_POSE, 0};
 
   const bool ALIGN_DEPTH = true;
@@ -66,11 +67,12 @@ namespace realsense
   const std::string DEFAULT_FISHEYE2_OPTICAL_FRAME_ID = "camera_fisheye2_optical_frame";
   const std::string DEFAULT_ACCEL_OPTICAL_FRAME_ID    = "camera_accel_optical_frame";
   const std::string DEFAULT_GYRO_OPTICAL_FRAME_ID     = "camera_gyro_optical_frame";
+  const std::string DEFAULT_IMU_OPTICAL_FRAME_ID      = "camera_imu_optical_frame";
   const std::string DEFAULT_POSE_OPTICAL_FRAME_ID     = "camera_pose_optical_frame";
 
   const std::string DEFAULT_ALIGNED_DEPTH_TO_COLOR_FRAME_ID = "camera_aligned_depth_to_color_frame";
 
-  const std::string DEFAULT_UNITE_IMU_METHOD         = "";
+  const std::string DEFAULT_UNITE_IMU_METHOD         = "linear_interpolation";
   const std::string DEFAULT_FILTERS                  = "";
   const std::string DEFAULT_TOPIC_ODOM_IN            = "";
 
@@ -90,13 +92,15 @@ namespace realsense
                                                           {RS2_STREAM_FISHEYE, RS2_FORMAT_Y8},
                                                           {RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F},
                                                           {RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F},
+                                                          {RS2_STREAM_ANY, RS2_FORMAT_MOTION_XYZ32F},
                                                           {RS2_STREAM_POSE, RS2_FORMAT_6DOF}};
 
   const std::map<rs2_stream, std::string> STREAM_NAME = {{RS2_STREAM_COLOR, "color"},
                                                          {RS2_STREAM_DEPTH, "depth"},
                                                          {RS2_STREAM_INFRARED, "infra"},
                                                          {RS2_STREAM_ACCEL, "accel"},
                                                          {RS2_STREAM_GYRO, "gyro"},
+                                                         {RS2_STREAM_ANY, "imu"},
                                                          {RS2_STREAM_FISHEYE, "fisheye"},
                                                          {RS2_STREAM_POSE, "pose"}};
 
@@ -108,6 +112,7 @@ namespace realsense
                                                                      {FISHEYE2, DEFAULT_FISHEYE2_OPTICAL_FRAME_ID},
                                                                      {ACCEL, DEFAULT_ACCEL_OPTICAL_FRAME_ID},
                                                                      {GYRO, DEFAULT_GYRO_OPTICAL_FRAME_ID},
+                                                                     {IMU, DEFAULT_IMU_OPTICAL_FRAME_ID},
                                                                      {POSE, DEFAULT_POSE_OPTICAL_FRAME_ID}};
 
   const std::map<stream_index_pair, std::string> SAMPLE_TOPIC = {{COLOR, "camera/color/image_raw"},
@@ -118,6 +123,7 @@ namespace realsense
                                                                  {FISHEYE2, "camera/fisheye2/image_raw"},
                                                                  {ACCEL, "camera/accel/sample"},
                                                                  {GYRO, "camera/gyro/sample"},
+                                                                 {IMU, "camera/imu/sample"},
                                                                  {POSE, "camera/odom/sample"}};
 
   const std::map<stream_index_pair, std::string> INFO_TOPIC = {{COLOR, "camera/color/camera_info"},
@@ -128,6 +134,7 @@ namespace realsense
                                                                {FISHEYE2, "camera/fisheye2/camera_info"},
                                                                {ACCEL, "camera/accel/imu_info"},
                                                                {GYRO, "camera/gyro/imu_info"},
+                                                               {IMU, "camera/imu/imu_info"},
                                                                {POSE, ""}};
 
   const std::string ALIGNED_DEPTH_IMAGE_TOPIC = "camera/aligned_depth_to_color/image_raw";

realsense_ros/include/realsense/rs_d435i.hpp

@@ -17,6 +17,8 @@
 
 #include "realsense/rs_d435.hpp"
 
+#include <queue>
+
 using IMUInfo = realsense_msgs::msg::IMUInfo;
 
 namespace realsense
@@ -28,15 +30,66 @@ class RealSenseD435I : public RealSenseD435
   virtual ~RealSenseD435I() = default;
   virtual void publishTopicsCallback(const rs2::frame & frame) override;
   virtual Result paramChangeCallback(const std::vector<rclcpp::Parameter> & params) override;
-  void publishIMUTopic(const rs2::frame & frame, const rclcpp::Time & time);
+  void publishIMUTopic(const rs2::frame & frame);
   IMUInfo getIMUInfo(const rs2::frame & frame, const stream_index_pair & stream_index);
 
+ public:
+  enum imu_sync_method{COPY, LINEAR_INTERPOLATION};
+
+ protected:
+  class float3
+  {
+   public:
+    float x, y, z;
+
+   public:
+    float3& operator*=(const float& factor)
+    {
+      x*=factor;
+      y*=factor;
+      z*=factor;
+      return (*this);
+    }
+    float3& operator+=(const float3& other)
+    {
+      x+=other.x;
+      y+=other.y;
+      z+=other.z;
+      return (*this);
+    }
+  };
+
+ private:
+  class CimuData
+  {
+   public:
+    CimuData() : m_time(-1) {};
+    CimuData(const stream_index_pair type, Eigen::Vector3d data, double time):
+        m_type(type),
+        m_data(data),
+        m_time(time){};
+    bool is_set() {return m_time > 0;};
+   public:
+    stream_index_pair m_type;
+    Eigen::Vector3d m_data;
+    double          m_time;
+  };
+
+private:
+  sensor_msgs::msg::Imu CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data);
+
+  void FillImuData_Copy(const CimuData imu_data, std::deque<sensor_msgs::msg::Imu>& imu_msgs);
+  void ImuMessage_AddDefaultValues(sensor_msgs::msg::Imu &imu_msg);
+  void imu_callback_sync(const rs2::frame & frame, imu_sync_method sync_method);
+  void FillImuData_LinearInterpolation(const CimuData imu_data, std::deque<sensor_msgs::msg::Imu>& imu_msgs);
+
 private:
   const std::vector<stream_index_pair> IMAGE_STREAMS = {COLOR, DEPTH, INFRA1, INFRA2};
-  const std::vector<stream_index_pair> MOTION_STREAMS = {ACCEL, GYRO};
+  const std::vector<stream_index_pair> MOTION_STREAMS = {ACCEL, GYRO, IMU};
   double linear_accel_cov_;
   double angular_velocity_cov_;
   bool initialized_ = false;
+  imu_sync_method imu_sync_method_;
 };
 }  // namespace perception
 #endif // REALSENSE__RS_D435I_HPP_

realsense_ros/package.xml

@@ -21,6 +21,8 @@
   <depend>realsense_msgs</depend>
   <depend>image_transport</depend>
 
+  <build_depend>rcl</build_depend>
+
   <exec_depend>launch_ros</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

realsense_ros/src/rs_base.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <sstream>
+#include "rcl/time.h"
 #include "realsense/rs_base.hpp"
 
 namespace realsense
@@ -94,14 +95,16 @@ void RealSenseBase::setupStream(const stream_index_pair & stream)
     enable = node_.declare_parameter(os.str(), DEFAULT_ENABLE_STREAM);
   }
 
-  if (stream == ACCEL || stream == GYRO) {
+  if (stream == ACCEL || stream == GYRO || stream == IMU) {
     imu_pub_.insert(std::pair<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr>
       (stream, node_.create_publisher<sensor_msgs::msg::Imu>(SAMPLE_TOPIC.at(stream), rclcpp::QoS(1))));
     imu_info_pub_.insert(std::pair<stream_index_pair, rclcpp::Publisher<realsense_msgs::msg::IMUInfo>::SharedPtr>
       (stream, node_.create_publisher<realsense_msgs::msg::IMUInfo>(INFO_TOPIC.at(stream), rclcpp::QoS(1))));
     if (enable == true) {
       enable_[stream] = true;
-      cfg_.enable_stream(stream.first, stream.second);
+      if (stream != IMU) {
+        cfg_.enable_stream(stream.first, stream.second);
+      }
     }
   } else if (stream == POSE) {
     odom_pub_ = node_.create_publisher<nav_msgs::msg::Odometry>(SAMPLE_TOPIC.at(stream), rclcpp::QoS(1));
@@ -457,4 +460,12 @@ Result RealSenseBase::changeFPS(const stream_index_pair & stream, const rclcpp::
   }
   return result;
 }
+
+rclcpp::Time RealSenseBase::frameToTime(const rs2::frame & frame) {
+  return rclcpp::Time(RCL_MS_TO_NS(frame.get_timestamp()));
+}
+
+rclcpp::Time RealSenseBase::msToTime(const double & ms) {
+  return rclcpp::Time(RCL_MS_TO_NS(ms));
+}
 }  // namespace realsense

realsense_ros/src/rs_d435.cpp

@@ -60,7 +60,7 @@ RealSenseD435::RealSenseD435(rs2::context ctx, rs2::device dev, rclcpp::Node & n
 void RealSenseD435::publishTopicsCallback(const rs2::frame & frame)
 {
   rs2::frameset frameset = frame.as<rs2::frameset>();
-  rclcpp::Time t = node_.now();
+  rclcpp::Time t = frameToTime(frame);
   if (enable_[COLOR] && (image_pub_[COLOR]->get_subscription_count() > 0 || camera_info_pub_[COLOR]->get_subscription_count() > 0)){
     auto frame = frameset.get_color_frame();
     publishImageTopic(frame, t);