AP_DDS: Add IMU publisher

* Using NED frame

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -1,13 +1,13 @@
 #include <AP_HAL/AP_HAL_Boards.h>
 
 #if AP_DDS_ENABLED
-
 #include <uxr/client/util/ping.h>
 
 #include <AP_GPS/AP_GPS.h>
 #include <AP_HAL/AP_HAL.h>
 #include <AP_RTC/AP_RTC.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
 #include <GCS_MAVLink/GCS.h>
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_AHRS/AP_AHRS.h>
@@ -32,6 +32,7 @@
 static constexpr uint8_t ENABLED_BY_DEFAULT = 1;
 static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10;
 static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000;
+static constexpr uint16_t DELAY_IMU_TOPIC_MS = 5;
 static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33;
 static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33;
 static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33;
@@ -417,6 +418,42 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
     }
 }
 
+void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg)
+{
+    update_topic(msg.header.stamp);
+    strcpy(msg.header.frame_id, BASE_LINK_NED_FRAME_ID);
+
+    auto &imu = AP::ins();
+    auto &ahrs = AP::ahrs();
+
+    WITH_SEMAPHORE(ahrs.get_semaphore());
+
+    Quaternion orientation;
+    if (ahrs.get_quaternion(orientation)) {
+        msg.orientation.x = orientation[0];
+        msg.orientation.y = orientation[1];
+        msg.orientation.z = orientation[2];
+        msg.orientation.w = orientation[3];
+    }
+    msg.orientation_covariance[0] = -1;
+
+    uint8_t accel_index = ahrs.get_primary_accel_index();
+    uint8_t gyro_index = ahrs.get_primary_gyro_index();
+    const Vector3f accel_data = imu.get_accel(accel_index);
+    const Vector3f gyro_data = imu.get_gyro(gyro_index);
+
+    // Populate the message fields
+    msg.linear_acceleration.x = accel_data.x;
+    msg.linear_acceleration.y = accel_data.y;
+    msg.linear_acceleration.z = accel_data.z;
+
+    msg.angular_velocity.x = gyro_data.x;
+    msg.angular_velocity.y = gyro_data.y;
+    msg.angular_velocity.z = gyro_data.z;
+    msg.angular_velocity_covariance[0] = -1;
+    msg.linear_acceleration_covariance[0] = -1;
+}
+
 void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
 {
     update_topic(msg.clock);
@@ -958,6 +995,21 @@ void AP_DDS_Client::write_tx_local_velocity_topic()
     }
 }
 
+void AP_DDS_Client::write_imu_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub {};
+        const uint32_t topic_size = sensor_msgs_msg_Imu_size_of_topic(&imu_topic, 0);
+        uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::IMU_PUB)].dw_id, &ub, topic_size);
+        const bool success = sensor_msgs_msg_Imu_serialize_topic(&ub, &imu_topic);
+        if (!success) {
+            // TODO sometimes serialization fails on bootup. Determine why.
+            // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
+        }
+    }
+}
+
 void AP_DDS_Client::write_geo_pose_topic()
 {
     WITH_SEMAPHORE(csem);
@@ -1023,6 +1075,12 @@ void AP_DDS_Client::update()
         write_tx_local_velocity_topic();
     }
 
+    if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) {
+        update_topic(imu_topic);
+        last_imu_time_ms = cur_time_ms;
+        write_imu_topic();
+    }
+
     if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) {
         update_topic(geo_pose_topic);
         last_geo_pose_time_ms = cur_time_ms;

libraries/AP_DDS/AP_DDS_Client.h

@@ -11,6 +11,7 @@
 #include "sensor_msgs/msg/NavSatFix.h"
 #include "tf2_msgs/msg/TFMessage.h"
 #include "sensor_msgs/msg/BatteryState.h"
+#include "sensor_msgs/msg/Imu.h"
 #include "sensor_msgs/msg/Joy.h"
 #include "geometry_msgs/msg/PoseStamped.h"
 #include "geometry_msgs/msg/TwistStamped.h"
@@ -62,6 +63,7 @@ private:
     geometry_msgs_msg_TwistStamped tx_local_velocity_topic;
     sensor_msgs_msg_BatteryState battery_state_topic;
     sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
+    sensor_msgs_msg_Imu imu_topic;
     rosgraph_msgs_msg_Clock clock_topic;
     // incoming joystick data
     static sensor_msgs_msg_Joy rx_joy_topic;
@@ -88,6 +90,7 @@ private:
     static void update_topic(geometry_msgs_msg_PoseStamped& msg);
     static void update_topic(geometry_msgs_msg_TwistStamped& msg);
     static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg);
+    static void update_topic(sensor_msgs_msg_Imu& msg);
     static void update_topic(rosgraph_msgs_msg_Clock& msg);
 
     // subscription callback function
@@ -112,6 +115,8 @@ private:
     uint64_t last_nav_sat_fix_time_ms;
     // The last ms timestamp AP_DDS wrote a BatteryState message
     uint64_t last_battery_state_time_ms;
+    // The last ms timestamp AP_DDS wrote an IMU message
+    uint64_t last_imu_time_ms;
     // The last ms timestamp AP_DDS wrote a Local Pose message
     uint64_t last_local_pose_time_ms;
     // The last ms timestamp AP_DDS wrote a Local Velocity message
@@ -188,6 +193,8 @@ public:
     void write_tx_local_velocity_topic();
     //! @brief Serialize the current geo_pose and publish to the IO stream(s)
     void write_geo_pose_topic();
+    //! @brief Serialize the current IMU data and publish to the IO stream(s)
+    void write_imu_topic();
     //! @brief Serialize the current clock and publish to the IO stream(s)
     void write_clock_topic();
     //! @brief Update the internally stored DDS messages with latest data

libraries/AP_DDS/AP_DDS_Frames.h

@@ -3,5 +3,6 @@
 static constexpr char WGS_84_FRAME_ID[] = "WGS-84";
 // https://www.ros.org/reps/rep-0105.html#base-link
 static constexpr char BASE_LINK_FRAME_ID[] = "base_link";
+static constexpr char BASE_LINK_NED_FRAME_ID[] = "base_link_ned";
 // https://www.ros.org/reps/rep-0105.html#map
 static constexpr char MAP_FRAME[] = "map";

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -3,6 +3,7 @@
 #include "tf2_msgs/msg/TFMessage.h"
 #include "sensor_msgs/msg/BatteryState.h"
 #include "geographic_msgs/msg/GeoPoseStamped.h"
+#include "sensor_msgs/msg/Imu.h"
 
 #include "uxr/client/client.h"
 
@@ -15,6 +16,7 @@ enum class TopicIndex: uint8_t {
     NAV_SAT_FIX_PUB,
     STATIC_TRANSFORMS_PUB,
     BATTERY_STATE_PUB,
+    IMU_PUB,
     LOCAL_POSE_PUB,
     LOCAL_VELOCITY_PUB,
     GEOPOSE_PUB,
@@ -73,6 +75,16 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .dw_profile_label = "batterystate0__dw",
         .dr_profile_label = "",
     },
+    {
+        .topic_id = to_underlying(TopicIndex::IMU_PUB),
+        .pub_id = to_underlying(TopicIndex::IMU_PUB),
+        .sub_id = to_underlying(TopicIndex::IMU_PUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAREADER_ID},
+        .topic_profile_label = "imu__t",
+        .dw_profile_label = "imu__dw",
+        .dr_profile_label = "",
+    },
     {
         .topic_id = to_underlying(TopicIndex::LOCAL_POSE_PUB),
         .pub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB),

libraries/AP_DDS/Idl/sensor_msgs/msg/Imu.idl

@@ -0,0 +1,48 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from sensor_msgs/msg/Imu.msg
+// generated code does not contain a copyright notice
+
+#include "geometry_msgs/msg/Quaternion.idl"
+#include "geometry_msgs/msg/Vector3.idl"
+#include "std_msgs/msg/Header.idl"
+
+module sensor_msgs {
+  module msg {
+    typedef double double__9[9];
+    @verbatim (language="comment", text=
+      "This is a message to hold data from an IMU (Inertial Measurement Unit)" "\n"
+      "" "\n"
+      "Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec" "\n"
+      "" "\n"
+      "If the covariance of the measurement is known, it should be filled in (if all you know is the" "\n"
+      "variance of each measurement, e.g. from the datasheet, just put those along the diagonal)" "\n"
+      "A covariance matrix of all zeros will be interpreted as \"covariance unknown\", and to use the" "\n"
+      "data a covariance will have to be assumed or gotten from some other source" "\n"
+      "" "\n"
+      "If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an" "\n"
+      "orientation estimate), please set element 0 of the associated covariance matrix to -1" "\n"
+      "If you are interpreting this message, please check for a value of -1 in the first element of each" "\n"
+      "covariance matrix, and disregard the associated estimate.")
+    struct Imu {
+      std_msgs::msg::Header header;
+
+      geometry_msgs::msg::Quaternion orientation;
+
+      @verbatim (language="comment", text=
+        "Row major about x, y, z axes")
+      double__9 orientation_covariance;
+
+      geometry_msgs::msg::Vector3 angular_velocity;
+
+      @verbatim (language="comment", text=
+        "Row major about x, y, z axes")
+      double__9 angular_velocity_covariance;
+
+      geometry_msgs::msg::Vector3 linear_acceleration;
+
+      @verbatim (language="comment", text=
+        "Row major x, y z")
+      double__9 linear_acceleration_covariance;
+    };
+  };
+};

libraries/AP_DDS/dds_xrce_profile.xml

@@ -114,6 +114,29 @@
       </historyQos>
     </topic>
   </data_writer>
+  <topic profile_name="imu__t">
+    <name>rt/ap/imu/experimental/data</name>
+    <dataType>sensor_msgs::msg::dds_::Imu_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>20</depth>
+    </historyQos>
+  </topic>
+  <data_writer profile_name="imu__dw">
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ap/imu/experimental/data</name>
+      <dataType>sensor_msgs::msg::dds_::Imu_</dataType>
+    </topic>
+    <qos>
+      <reliability>
+        <kind>RELIABLE</kind>
+      </reliability>
+      <durability>
+        <kind>TRANSIENT_LOCAL</kind>
+      </durability>
+    </qos>
+  </data_writer>
   <topic profile_name="localpose__t">
     <name>rt/ap/pose/filtered</name>
     <dataType>geometry_msgs::msg::dds_::PoseStamped_</dataType>