AP_DDS: Airspeed topic

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -46,6 +46,9 @@ static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33;
 #if AP_DDS_LOCAL_VEL_PUB_ENABLED
 static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33;
 #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
+#if AP_DDS_AIRSPEED_PUB_ENABLED
+static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = 33;
+#endif // AP_DDS_AIRSPEED_PUB_ENABLED
 #if AP_DDS_GEOPOSE_PUB_ENABLED
 static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33;
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
@@ -453,6 +456,34 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
     msg.twist.angular.z = -angular_velocity[2];
 }
 #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
+#if AP_DDS_AIRSPEED_PUB_ENABLED
+bool AP_DDS_Client::update_topic(geometry_msgs_msg_Vector3Stamped& msg)
+{
+    update_topic(msg.header.stamp);
+    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    auto &ahrs = AP::ahrs();
+    WITH_SEMAPHORE(ahrs.get_semaphore());
+    // In ROS REP 103, axis orientation uses the following convention:
+    // X - Forward
+    // Y - Left
+    // Z - Up
+    // https://www.ros.org/reps/rep-0103.html#axis-orientation
+    // The true airspeed data is received from AP_AHRS in body-frame
+    // X - Forward
+    // Y - Right
+    // Z - Down
+    // As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z
+    Vector3f true_airspeed_vec_bf;
+    bool is_airspeed_available {false};
+    if (ahrs.airspeed_vector_true(true_airspeed_vec_bf)) {
+        msg.vector.x = true_airspeed_vec_bf[0];
+        msg.vector.y = -true_airspeed_vec_bf[1];
+        msg.vector.z = -true_airspeed_vec_bf[2];
+        is_airspeed_available = true;
+    }
+    return is_airspeed_available;
+}
+#endif // AP_DDS_AIRSPEED_PUB_ENABLED
 
 #if AP_DDS_GEOPOSE_PUB_ENABLED
 void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
@@ -1128,7 +1159,22 @@ void AP_DDS_Client::write_tx_local_velocity_topic()
     }
 }
 #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
-
+#if AP_DDS_AIRSPEED_PUB_ENABLED
+void AP_DDS_Client::write_tx_local_airspeed_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub {};
+        const uint32_t topic_size = geometry_msgs_msg_Vector3Stamped_size_of_topic(&tx_local_airspeed_topic, 0);
+        uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB)].dw_id, &ub, topic_size);
+        const bool success = geometry_msgs_msg_Vector3Stamped_serialize_topic(&ub, &tx_local_airspeed_topic);
+        if (!success) {
+            // TODO sometimes serialization fails on bootup. Determine why.
+            // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
+        }
+    }
+}
+#endif // AP_DDS_AIRSPEED_PUB_ENABLED
 #if AP_DDS_IMU_PUB_ENABLED
 void AP_DDS_Client::write_imu_topic()
 {
@@ -1236,6 +1282,14 @@ void AP_DDS_Client::update()
         write_tx_local_velocity_topic();
     }
 #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
+#if AP_DDS_AIRSPEED_PUB_ENABLED
+    if (cur_time_ms - last_airspeed_time_ms > DELAY_AIRSPEED_TOPIC_MS) {
+        last_airspeed_time_ms = cur_time_ms;
+        if (update_topic(tx_local_airspeed_topic)) {
+            write_tx_local_airspeed_topic();
+        }
+    }
+#endif // AP_DDS_AIRSPEED_PUB_ENABLED
 #if AP_DDS_IMU_PUB_ENABLED
     if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) {
         update_topic(imu_topic);

libraries/AP_DDS/AP_DDS_Client.h

@@ -37,6 +37,9 @@
 #if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
 #include "geographic_msgs/msg/GeoPointStamped.h"
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
+#if AP_DDS_AIRSPEED_PUB_ENABLED
+#include "geometry_msgs/msg/Vector3Stamped.h"
+#endif // AP_DDS_AIRSPEED_PUB_ENABLED
 #if AP_DDS_GEOPOSE_PUB_ENABLED
 #include "geographic_msgs/msg/GeoPoseStamped.h"
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
@@ -127,6 +130,15 @@ class AP_DDS_Client
     static void update_topic(geometry_msgs_msg_TwistStamped& msg);
 #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
 
+#if AP_DDS_AIRSPEED_PUB_ENABLED
+    geometry_msgs_msg_Vector3Stamped tx_local_airspeed_topic;
+    // The last ms timestamp AP_DDS wrote a airspeed message
+    uint64_t last_airspeed_time_ms;
+    //! @brief Serialize the current local airspeed and publish to the IO stream(s)
+    void write_tx_local_airspeed_topic();
+    static bool update_topic(geometry_msgs_msg_Vector3Stamped& msg);
+#endif //AP_DDS_AIRSPEED_PUB_ENABLED
+
 #if AP_DDS_BATTERY_STATE_PUB_ENABLED
     sensor_msgs_msg_BatteryState battery_state_topic;
     // The last ms timestamp AP_DDS wrote a BatteryState message
@@ -193,8 +205,6 @@ class AP_DDS_Client
     bool status_ok{false};
     bool connected{false};
 
-
-
     // subscription callback function
     static void on_topic_trampoline(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length, void* args);
     void on_topic(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length);

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 "geometry_msgs/msg/Vector3Stamped.h"
 #if AP_DDS_IMU_PUB_ENABLED
 #include "sensor_msgs/msg/Imu.h"
 #endif //AP_DDS_IMU_PUB_ENABLED
@@ -35,6 +36,9 @@ enum class TopicIndex: uint8_t {
 #if AP_DDS_LOCAL_VEL_PUB_ENABLED
     LOCAL_VELOCITY_PUB,
 #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
+#if AP_DDS_AIRSPEED_PUB_ENABLED
+    LOCAL_AIRSPEED_PUB,
+#endif // AP_DDS_AIRSPEED_PUB_ENABLED
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     GEOPOSE_PUB,
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
@@ -192,6 +196,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         },
     },
 #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
+#if AP_DDS_AIRSPEED_PUB_ENABLED
+    {
+        .topic_id = to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB),
+        .pub_id = to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB),
+        .sub_id = to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB), .type=UXR_DATAREADER_ID},
+        .topic_rw = Topic_rw::DataWriter,
+        .topic_name = "rt/ap/airspeed",
+        .type_name = "geometry_msgs::msg::dds_::Vector3Stamped_",
+        .qos = {
+            .durability = UXR_DURABILITY_VOLATILE,
+            .reliability = UXR_RELIABILITY_BEST_EFFORT,
+            .history = UXR_HISTORY_KEEP_LAST,
+            .depth = 5,
+        },
+    },
+#endif // AP_DDS_AIRSPEED_PUB_ENABLED
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     {
         .topic_id = to_underlying(TopicIndex::GEOPOSE_PUB),

libraries/AP_DDS/AP_DDS_config.h

@@ -54,6 +54,10 @@
 #define AP_DDS_LOCAL_VEL_PUB_ENABLED 1
 #endif
 
+#ifndef AP_DDS_AIRSPEED_PUB_ENABLED
+#define AP_DDS_AIRSPEED_PUB_ENABLED 1
+#endif
+
 #ifndef AP_DDS_BATTERY_STATE_PUB_ENABLED
 #define AP_DDS_BATTERY_STATE_PUB_ENABLED 1
 #endif

libraries/AP_DDS/Idl/geometry_msgs/msg/Vector3Stamped.idl

@@ -0,0 +1,21 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from geometry_msgs/msg/Vector3Stamped.msg
+// generated code does not contain a copyright notice
+
+#include "geometry_msgs/msg/Vector3.idl"
+#include "std_msgs/msg/Header.idl"
+
+module geometry_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "This represents a Vector3 with reference coordinate frame and timestamp")
+    struct Vector3Stamped {
+      @verbatim (language="comment", text=
+        "Note that this follows vector semantics with it always anchored at the origin," "\n"
+        "so the rotational elements of a transform are the only parts applied when transforming.")
+      std_msgs::msg::Header header;
+
+      geometry_msgs::msg::Vector3 vector;
+    };
+  };
+};

libraries/AP_DDS/README.md

@@ -172,6 +172,7 @@ $ ros2 node list
 ```bash
 $ ros2 topic list -v
 Published topics:
+ * /ap/airspeed [geometry_msgs/msg/Vector3] 1 publisher
  * /ap/battery/battery0 [sensor_msgs/msg/BatteryState] 1 publisher
  * /ap/clock [rosgraph_msgs/msg/Clock] 1 publisher
  * /ap/geopose/filtered [geographic_msgs/msg/GeoPoseStamped] 1 publisher