ROS-82: Add an option to populate orientation field in sensor_msgs::Imu with an estimated value

src/imu_model.h

@@ -0,0 +1,56 @@
+/**
+ * Copyright (c) 2024, Ouster, Inc.
+ * All rights reserved.
+ *
+ * @file imu-model.h
+ * @brief provide orientation esimtate based on imu data
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+
+// A class that can estimates the current orientation based on imu telemetry
+class ImuModel {
+   public:
+    /*
+     * TODO: Add description
+     * param: orientation represented as euler
+     * TODO: add initial angular velocity
+    */
+    virtual void set_initial_state(const Eigen::Vector3d& initial_orientation) = 0;
+
+    /*
+     * TODO: Update
+     * params la: linear acceleration
+     * params av: angular velocity
+    */
+    virtual Eigen::Quaterniond update(uint64_t ts, const Eigen::Vector3d& la, const Eigen::Vector3d& av) = 0;
+};
+
+
+class SimpleImuModel : public ImuModel {
+   public:
+    void set_initial_state(const Eigen::Vector3d& initial_orientation) override {
+        this->initial_orientation = initial_orientation;
+    }
+
+    Eigen::Quaterniond update(uint64_t ts, const Eigen::Vector3d& la, const Eigen::Vector3d& av) override {
+        // TODO: compute dt from consecutive ts
+        double dt = 0.01;
+
+        Eigen::Vector3d orientation = initial_orientation + av * dt;
+        orientation.x() += atan2(la.y(), la.z());
+        orientation.y() += atan2(-la.x(), sqrt(la.y() * la.y() + la.z() * la.z()));
+
+        Eigen::Quaterniond q =
+            Eigen::AngleAxisd(orientation.x(), Eigen::Vector3d::UnitX())
+            * Eigen::AngleAxisd(orientation.y(), Eigen::Vector3d::UnitY())
+            * Eigen::AngleAxisd(orientation.z(), Eigen::Vector3d::UnitZ());
+        return q;
+    }
+
+private:
+    // using euler angles for now
+    Eigen::Vector3d initial_orientation;
+};

src/imu_packet_handler.h

@@ -14,6 +14,8 @@
 #include "ouster_ros/os_ros.h"
 // clang-format on
 
+
+#include "imu_model.h"
 namespace ouster_ros {
 
 class ImuPacketHandler {
@@ -25,7 +27,8 @@ class ImuPacketHandler {
     static HandlerType create_handler(const sensor::sensor_info& info,
                                       const std::string& frame,
                                       const std::string& timestamp_mode,
-                                      int64_t ptp_utc_tai_offset) {
+                                      int64_t ptp_utc_tai_offset,
+                                      bool estimate_orientation) {
         const auto& pf = sensor::get_format(info);
         using Timestamper = std::function<ros::Time(const sensor::ImuPacket&)>;
         Timestamper timestamper;
@@ -49,9 +52,26 @@ class ImuPacketHandler {
                 }};
         }
 
-        return [&pf, &frame, timestamper](const sensor::ImuPacket& imu_packet) {
-            return packet_to_imu_msg(pf, timestamper(imu_packet), frame,
-                                     imu_packet.buf.data());
+        std::shared_ptr<ImuModel> imu_model;
+        if (estimate_orientation) {
+            imu_model = std::make_shared<SimpleImuModel>();
+            imu_model->set_initial_state(Eigen::Vector3d(0.0, 0.0, 0.0));
+        }
+
+        return [&pf, &frame, timestamper, estimate_orientation, imu_model](const sensor::ImuPacket& imu_packet) {
+            sensor_msgs::Imu msg = packet_to_imu_msg(pf, timestamper(imu_packet),
+                                                     frame, imu_packet.buf.data());
+            if (estimate_orientation) {
+                auto ts = pf.imu_gyro_ts(imu_packet.buf.data());
+                Eigen::Vector3d la(msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z);
+                Eigen::Vector3d av(msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z);
+                Eigen::Quaternion q = imu_model->update(ts, la, av);
+                msg.orientation.x = q.x();
+                msg.orientation.y = q.y();
+                msg.orientation.z = q.z();
+                msg.orientation.w = q.w();
+            }
+            return msg;
         };
     }
 };

src/os_cloud_nodelet.cpp

@@ -154,11 +154,13 @@ class OusterCloud : public nodelet::Nodelet {
 
         auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
         double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
+        auto estimate_imu_orientation = pnh.param("estimate_imu_orientation", false);
 
         if (impl::check_token(tokens, "IMU")) {
             imu_packet_handler = ImuPacketHandler::create_handler(
                 info, tf_bcast.imu_frame_id(), timestamp_mode,
-                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
+                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
+                estimate_imu_orientation);
         }
 
         std::vector<LidarScanProcessor> processors;

src/os_driver_nodelet.cpp

@@ -115,11 +115,13 @@ class OusterDriver : public OusterSensor {
 
         auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
         double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
+        auto estimate_imu_orientation = pnh.param("estimate_imu_orientation", false);
 
         if (impl::check_token(tokens, "IMU")) {
             imu_packet_handler = ImuPacketHandler::create_handler(
                 info, tf_bcast.imu_frame_id(), timestamp_mode,
-                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
+                static_cast<int64_t>(ptp_utc_tai_offset * 1e+9),
+                estimate_imu_orientation);
         }
 
         std::vector<LidarScanProcessor> processors;