AP_DDS: publish relative true airspeed to /ap/airspeed

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -423,6 +423,34 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
  msg.twist.angular.z = -angular_velocity[2];
 }
 
+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;
+}
+
+
 void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
 {
  update_topic(msg.header.stamp);
@@ -1073,6 +1101,21 @@ void AP_DDS_Client::write_tx_local_velocity_topic()
  }
 }
 
+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");
+ }
+ }
+}
+
 #if AP_DDS_IMU_PUB_ENABLED
 void AP_DDS_Client::write_imu_topic()
 {
@@ -1167,6 +1210,9 @@ void AP_DDS_Client::update()
  update_topic(tx_local_velocity_topic);
  last_local_velocity_time_ms = cur_time_ms;
  write_tx_local_velocity_topic();
+ if (update_topic(tx_local_airspeed_topic)) {
+ write_tx_local_airspeed_topic();
+ }
  }
 #if AP_DDS_IMU_PUB_ENABLED
  if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) {

libraries/AP_DDS/AP_DDS_Client.h

@@ -19,6 +19,7 @@
 #include "sensor_msgs/msg/Joy.h"
 #include "geometry_msgs/msg/PoseStamped.h"
 #include "geometry_msgs/msg/TwistStamped.h"
+#include "geometry_msgs/msg/Vector3Stamped.h"
 #include "geographic_msgs/msg/GeoPointStamped.h"
 #include "geographic_msgs/msg/GeoPoseStamped.h"
 #include "rosgraph_msgs/msg/Clock.h"
@@ -65,6 +66,7 @@ class AP_DDS_Client
  geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
  geometry_msgs_msg_PoseStamped local_pose_topic;
  geometry_msgs_msg_TwistStamped tx_local_velocity_topic;
+ geometry_msgs_msg_Vector3Stamped tx_local_airspeed_topic;
  sensor_msgs_msg_BatteryState battery_state_topic;
  sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
 #if AP_DDS_IMU_PUB_ENABLED
@@ -94,6 +96,7 @@ class AP_DDS_Client
  static void update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance);
  static void update_topic(geometry_msgs_msg_PoseStamped& msg);
  static void update_topic(geometry_msgs_msg_TwistStamped& msg);
+ static bool update_topic(geometry_msgs_msg_Vector3Stamped& msg);
  static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg);
 #if AP_DDS_IMU_PUB_ENABLED
  static void update_topic(sensor_msgs_msg_Imu& msg);
@@ -203,6 +206,8 @@ class AP_DDS_Client
  void write_local_pose_topic();
  //! @brief Serialize the current local velocity and publish to the IO stream(s)
  void write_tx_local_velocity_topic();
+ //! @brief Serialize the current local airspeed and publish to the IO stream(s)
+ void write_tx_local_airspeed_topic();
  //! @brief Serialize the current geo_pose and publish to the IO stream(s)
  void write_geo_pose_topic();
 #if AP_DDS_IMU_PUB_ENABLED

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
@@ -23,6 +24,7 @@ enum class TopicIndex: uint8_t {
 #endif //AP_DDS_IMU_PUB_ENABLED
  LOCAL_POSE_PUB,
  LOCAL_VELOCITY_PUB,
+ LOCAL_AIRSPEED_PUB,
  GEOPOSE_PUB,
  CLOCK_PUB,
  GPS_GLOBAL_ORIGIN_PUB,
@@ -154,6 +156,22 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
  .depth = 5,
  },
  },
+ {
+ .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,
+ },
+ },
  {
  .topic_id = to_underlying(TopicIndex::GEOPOSE_PUB),
  .pub_id = to_underlying(TopicIndex::GEOPOSE_PUB),

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