add linear interpolation union method for IMU (#558)

Add linear interpolation method for union of IMU sensors. Thanks to Marius Fehr (@mfehr) for the idea.
Set the initial behavior to sending IMU sensors separately, since this is the raw data. Enabling union with option unite_imu_method as demonstrated in the file opensource_tracking.launch.
fix bug if initializing with unavailable imu profile.

realsense2_camera/CMakeLists.txt

@@ -65,14 +65,6 @@ include_directories(
     ${realsense_INCLUDE_DIR}
     )
 
-# Generate dynamic reconfigure options from .cfg files
-#generate_dynamic_reconfigure_options(
-#  cfg/base_d400_params.cfg
-#  cfg/sr300_params.cfg
-#  cfg/rs415_params.cfg
-#  cfg/rs435_params.cfg
-#)
-
 # RealSense ROS Node
 catkin_package(
     LIBRARIES ${PROJECT_NAME}

realsense2_camera/include/base_realsense_node.h

@@ -97,6 +97,9 @@ namespace realsense2_camera
         virtual void registerDynamicReconfigCb(ros::NodeHandle& nh) override;
         virtual ~BaseRealSenseNode() {}
 
+    public:
+        enum imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION};
+
     protected:
 
         const uint32_t set_default_dynamic_reconfig_values = 0xffffffff;
@@ -105,20 +108,68 @@ namespace realsense2_camera
         std::map<stream_index_pair, rs2::sensor> _sensors;
         std::vector<std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure>> _ddynrec;
 
-        static void callback(const ddynamic_reconfigure::DDMap& map, int, rs2::options sensor);
-        void registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name);
-
     private:
-        struct float3
+        class float3
         {
-            float x, y, z;
+            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);
+                }
         };
 
         struct quaternion
         {
             double x, y, z, w;
         };
 
+        class CIMUHistory
+        {
+            public:
+                enum sensor_name {mGYRO, mACCEL};
+                class imuData
+                {
+                    public:
+                        imuData(const imuData& other):
+                            imuData(other.m_reading, other.m_time)
+                            {};
+                        imuData(const float3 reading, double time):
+                            m_reading(reading),
+                            m_time(time)
+                            {};
+                        imuData operator*(const double factor);
+                        imuData operator+(const imuData& other);
+                    public:
+                        BaseRealSenseNode::float3 m_reading;
+                        double                    m_time;
+                };
+
+            private:
+                size_t m_max_size;
+                std::map<sensor_name, std::list<imuData> > m_map;
+
+            public:
+                CIMUHistory(size_t size);
+                void add_data(sensor_name module, imuData data);
+                bool is_all_data(sensor_name);
+                bool is_data(sensor_name);
+                const std::list<imuData>& get_data(sensor_name module);
+                imuData last_data(sensor_name module);
+        };
+
         static std::string getNamespaceStr();
         void getParameters();
         void setupDevice();
@@ -157,6 +208,13 @@ namespace realsense2_camera
                                   rs2_stream stream_type, int stream_index);
 
         void publishAlignedDepthToOthers(rs2::frameset frames, const ros::Time& t);
+        static void callback(const ddynamic_reconfigure::DDMap& map, int, rs2::options sensor);
+        double FillImuData_Copy(const stream_index_pair stream_index, const CIMUHistory::imuData imu_data, sensor_msgs::Imu& imu_msg);
+        double FillImuData_LinearInterpolation(const stream_index_pair stream_index, const CIMUHistory::imuData imu_data, sensor_msgs::Imu& imu_msg);
+        static void ConvertFromOpticalFrameToFrame(float3& data);
+        void imu_callback(rs2::frame frame);
+        void imu_callback_sync(rs2::frame frame, imu_sync_method sync_method=imu_sync_method::COPY);
+        void registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name);
         rs2_stream rs2_string_to_stream(std::string str);
 
         std::string _json_file_path;
@@ -199,14 +257,12 @@ namespace realsense2_camera
         bool _align_depth;
         bool _sync_frames;
         bool _pointcloud;
-        bool _unite_imu;
+        imu_sync_method _imu_sync_method;
         std::string _filters_str;
         stream_index_pair _pointcloud_texture;
         PipelineSyncer _syncer;
         std::vector<NamedFilter> _filters;
         std::vector<rs2::sensor> _dev_sensors;
-        // Declare pointcloud object, for calculating pointclouds and texture mappings
-        // rs2::pointcloud _pc_filter;
 
         std::map<stream_index_pair, cv::Mat> _depth_aligned_image;
         std::map<stream_index_pair, std::string> _depth_aligned_encoding;
@@ -220,6 +276,7 @@ namespace realsense2_camera
 
         std::map<stream_index_pair, bool> _is_frame_arrived;
         const std::string _namespace;
+
     };//end class
 
 }

realsense2_camera/include/constants.h

@@ -68,7 +68,6 @@ namespace realsense2_camera
     const bool ENABLE_COLOR   = true;
     const bool ENABLE_FISHEYE = true;
     const bool ENABLE_IMU     = true;
-    const bool UNITE_IMU      = true;
     const bool HOLD_BACK_IMU_FOR_FRAMES = false;
 
 
@@ -94,6 +93,7 @@ namespace realsense2_camera
     const std::string DEFAULT_ALIGNED_DEPTH_TO_INFRA2_FRAME_ID = "camera_aligned_depth_to_infra2_frame";
     const std::string DEFAULT_ALIGNED_DEPTH_TO_FISHEYE_FRAME_ID = "camera_aligned_depth_to_fisheye_frame";
 
+    const std::string DEFAULT_UNITE_IMU_METHOD         = "";
     const std::string DEFAULT_FILTERS                  = "";
 
     using stream_index_pair = std::pair<rs2_stream, int>;

realsense2_camera/launch/includes/nodelet.launch.xml

@@ -71,6 +71,7 @@
   <arg name="clip_distance"            default="-1"/>
   <arg name="linear_accel_cov"         default="0.01"/>
   <arg name="initial_reset"            default="false"/>
+  <arg name="unite_imu_method"         default="none"/> <!-- Options are: [none, copy, linear_interpolation] -->
 
 
   <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen"/>
@@ -137,6 +138,7 @@
     <param name="clip_distance"            type="double" value="$(arg clip_distance)"/>
     <param name="linear_accel_cov"         type="double" value="$(arg linear_accel_cov)"/>
     <param name="initial_reset"            type="bool"   value="$(arg initial_reset)"/>
+    <param name="unite_imu_method"         type="str"    value="$(arg unite_imu_method)"/>
 
   </node>
 </launch>

realsense2_camera/launch/opensource_tracking.launch

@@ -4,6 +4,7 @@
         file="$(find realsense2_camera)/launch/rs_camera.launch">
         <arg name="align_depth" value="true"/>
         <arg name="linear_accel_cov" value="1.0"/>
+        <arg name="unite_imu_method" value="linear_interpolation"/>
     </include>
 
     <node pkg="imu_filter_madgwick" type="imu_filter_node" name="ImuFilter">

realsense2_camera/launch/rs_camera.launch

@@ -44,6 +44,7 @@
   <arg name="clip_distance"         default="-2"/>
   <arg name="linear_accel_cov"      default="0.01"/>
   <arg name="initial_reset"         default="false"/>
+  <arg name="unite_imu_method"      default=""/>
 
   <group ns="$(arg camera)">
     <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
@@ -90,6 +91,7 @@
       <arg name="clip_distance"            value="$(arg clip_distance)"/>
       <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
       <arg name="initial_reset"            value="$(arg initial_reset)"/>
+      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
     </include>
   </group>
 </launch>