DDS: Add new publisher for SITL state

Tools/ros2/ardupilot_msgs/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/GlobalPosition.msg"
   "msg/Rc.msg"
+  "msg/GroundTruth.msg"
   "msg/Status.msg"
   "msg/Airspeed.msg"
   "srv/ArmMotors.srv"

Tools/ros2/ardupilot_msgs/msg/GroundTruth.msg

@@ -0,0 +1,20 @@
+std_msgs/Header header
+
+# Geographical position
+float64 latitude
+float64 longitude
+float64 altitude
+
+
+# Local ENU position (meters)
+geometry_msgs/Vector3 position
+
+
+# Orientation (ENU)
+geometry_msgs/Quaternion orientation
+
+# Linear velocity (ENU, m/s)
+geometry_msgs/Vector3 linear_velocity
+
+# Angular velocity ( body frame, rad/s)
+geometry_msgs/Vector3 angular_velocity

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -73,6 +73,9 @@ static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = AP_DDS_DELAY_AIRSPEED_TOPIC_
 #if AP_DDS_RC_PUB_ENABLED
 static constexpr uint16_t DELAY_RC_TOPIC_MS = AP_DDS_DELAY_RC_TOPIC_MS;
 #endif // AP_DDS_RC_PUB_ENABLED
+#ifndef AP_DDS_DELAY_GROUNDTRUTH_TOPIC_MS
+static constexpr uint16_t DELAY_RC_TOPIC_MS = AP_DDS_DELAY_GROUNDTRUTH_TOPIC_MS;
+#endif
 #if AP_DDS_GEOPOSE_PUB_ENABLED
 static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = AP_DDS_DELAY_GEO_POSE_TOPIC_MS;
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
@@ -565,6 +568,105 @@ bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Rc& msg)
 }
 #endif // AP_DDS_RC_PUB_ENABLED
 
+#if AP_DDS_GROUNDTRUTH_PUB_ENABLED
+void AP_DDS_Client::update_topic(ardupilot_msgs_msg_GroundTruth& msg)
+{
+#if AP_SIM_ENABLED
+    const SITL::SIM *sitl = AP::sitl();
+    if (!sitl) {
+        return;
+    }
+    const auto &s = sitl->state;
+
+    update_topic(msg.header.stamp);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
+
+    // Geographic position
+    msg.latitude  = s.latitude;
+    msg.longitude = s.longitude;
+    msg.altitude  = s.altitude;
+
+    // Cartesian position in ENU frame relative to home
+    // ROS REP 103 uses the ENU convention:
+    // X - East
+    // Y - North
+    // Z - Up
+    // https://www.ros.org/reps/rep-0103.html#axis-orientation
+    // SITL state uses the NED convention:
+    // X - North
+    // Y - East
+    // Z - Down
+    // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
+    // as well as invert Z
+
+    // Location stores lat/lon in 1E7 degrees, altitude in centimeters, conversion on the go
+    Location current_loc(s.latitude * 1e7,s.longitude * 1e7,s.altitude * 100,Location::AltFrame::ABSOLUTE);
+    // get_distance_NED returns vector FROM home TO current location
+    Vector3f pos_ned = s.home.get_distance_NED(current_loc);
+
+    // Cartesion position in ENU frame
+    msg.position.x = pos_ned.y;   // East
+    msg.position.y = pos_ned.x;   // North
+    msg.position.z = -pos_ned.z;  // Up
+
+    // Orientation quaternion
+    // 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
+    // SITL provides orientation in NED body frame (FRD):
+    // X - Forward
+    // Y - Right
+    // Z - Down
+    // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
+    // as well as invert Z (NED to ENU conversion) as well as a 90 degree rotation in the Z axis
+    // for x to point forward
+    Quaternion orientation(s.quaternion.q1, s.quaternion.q2, s.quaternion.q3, s.quaternion.q4);
+    Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); // NED to ENU transformation
+    Quaternion transformation(sqrtF(2) * 0.5, 0, 0, sqrtF(2) * 0.5); // Z axis 90 degree rotation
+    orientation = aux * transformation;
+    msg.orientation.w = orientation[0];
+    msg.orientation.x = orientation[1];
+    msg.orientation.y = orientation[2];
+    msg.orientation.z = orientation[3];
+
+    // Linear velocity in ENU frame
+    // ROS REP 103 uses the ENU convention:
+    // X - East
+    // Y - North
+    // Z - Up
+    // https://www.ros.org/reps/rep-0103.html#axis-orientation
+    // SITL state uses the NED convention:
+    // X - North (speedN)
+    // Y - East (speedE)
+    // Z - Down (speedD)
+    // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
+    // as well as invert Z
+    msg.linear_velocity.x = s.speedE;   // East
+    msg.linear_velocity.y = s.speedN;   // North
+    msg.linear_velocity.z = -s.speedD;  // Up
+
+    // Angular velocity in body frame
+    // 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 angular rate data is received from SITL in body-frame (FRD):
+    // X - Forward (rollRate)
+    // Y - Right (pitchRate)
+    // Z - Down (yawRate)
+    // As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z
+    // Also convert from degrees/s to radians/s
+    msg.angular_velocity.x = radians(s.rollRate);   // Forward
+    msg.angular_velocity.y = -radians(s.pitchRate); // Left (inverted from Right)
+    msg.angular_velocity.z = -radians(s.yawRate);   // Up (inverted from Down)
+
+#endif // AP_SIM_ENABLED
+}
+#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED
+
 #if AP_DDS_GEOPOSE_PUB_ENABLED
 void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
 {
@@ -1669,6 +1771,22 @@ void AP_DDS_Client::write_tx_local_rc_topic()
     }
 }
 #endif // AP_DDS_RC_PUB_ENABLED
+#if AP_DDS_GROUNDTRUTH_PUB_ENABLED
+void AP_DDS_Client::write_groundtruth_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub{};
+        const uint32_t topic_size = ardupilot_msgs_msg_GroundTruth_size_of_topic(&groundtruth_topic, 0);
+        uxr_prepare_output_stream(&session,reliable_out,topics[to_underlying(TopicIndex::GROUNDTRUTH_PUB)].dw_id,&ub,topic_size);
+        const bool success = ardupilot_msgs_msg_GroundTruth_serialize_topic(&ub, &groundtruth_topic);
+        if (!success) {
+            // TODO sometimes serialization fails on bootup. Determine why.
+            // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
+        }
+    }
+}
+#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED
 #if AP_DDS_IMU_PUB_ENABLED
 void AP_DDS_Client::write_imu_topic()
 {
@@ -1836,6 +1954,13 @@ void AP_DDS_Client::update()
         write_imu_topic();
     }
 #endif // AP_DDS_IMU_PUB_ENABLED
+#if AP_DDS_GROUNDTRUTH_PUB_ENABLED
+    if (cur_time_ms - last_groundtruth_time_ms > AP_DDS_DELAY_GROUNDTRUTH_TOPIC_MS) {
+        update_topic(groundtruth_topic);
+        last_groundtruth_time_ms = cur_time_ms;
+        write_groundtruth_topic();
+    }
+#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) {
         update_topic(geo_pose_topic);

libraries/AP_DDS/AP_DDS_Client.h

@@ -47,6 +47,9 @@
 #if AP_DDS_RC_PUB_ENABLED
 #include "ardupilot_msgs/msg/Rc.h"
 #endif // AP_DDS_RC_PUB_ENABLED
+#if AP_DDS_GROUNDTRUTH_PUB_ENABLED
+#include "ardupilot_msgs/msg/GroundTruth.h"
+#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED
 #if AP_DDS_GEOPOSE_PUB_ENABLED
 #include "geographic_msgs/msg/GeoPoseStamped.h"
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
@@ -173,6 +176,15 @@ class AP_DDS_Client
     static bool update_topic(ardupilot_msgs_msg_Rc& msg);
 #endif //AP_DDS_RC_PUB_ENABLED
 
+#if AP_DDS_GROUNDTRUTH_PUB_ENABLED
+    ardupilot_msgs_msg_GroundTruth groundtruth_topic;
+    // The last ms timestamp AP_DDS wrote a groundtruth message
+    uint64_t last_groundtruth_time_ms;
+    //! @brief Serialize the current ground truth of sim vehicle and publish to the IO stream(s)
+    void write_groundtruth_topic();
+    static void update_topic(ardupilot_msgs_msg_GroundTruth& msg);
+#endif // AP_DDS_GROUNDTRUTH_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

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -42,6 +42,9 @@ enum class TopicIndex: uint8_t {
 #if AP_DDS_RC_PUB_ENABLED
     LOCAL_RC_PUB,
 #endif // AP_DDS_RC_PUB_ENABLED
+#if AP_DDS_GROUNDTRUTH_PUB_ENABLED
+    GROUNDTRUTH_PUB,
+#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLED
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     GEOPOSE_PUB,
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
@@ -241,6 +244,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         },
     },
 #endif // AP_DDS_RC_PUB_ENABLED
+#if AP_DDS_GROUNDTRUTH_PUB_ENABLED
+    {
+        .topic_id = to_underlying(TopicIndex::GROUNDTRUTH_PUB),
+        .pub_id   = to_underlying(TopicIndex::GROUNDTRUTH_PUB),
+        .sub_id   = to_underlying(TopicIndex::GROUNDTRUTH_PUB),
+        .dw_id    = uxrObjectId{.id = to_underlying(TopicIndex::GROUNDTRUTH_PUB), .type = UXR_DATAWRITER_ID},
+        .dr_id    = uxrObjectId{.id = to_underlying(TopicIndex::GROUNDTRUTH_PUB), .type = UXR_DATAREADER_ID},
+        .topic_rw = Topic_rw::DataWriter,
+        .topic_name = "rt/ap/sim/groundtruth",
+        .type_name  = "ardupilot_msgs::msg::dds_::GroundTruth_",
+        .qos = {
+            .durability = UXR_DURABILITY_VOLATILE,
+            .reliability = UXR_RELIABILITY_RELIABLE,
+            .history = UXR_HISTORY_KEEP_LAST,
+            .depth = 1,
+        },
+    },
+#endif // AP_DDS_GROUNDTRUTH_PUB_ENABLEDs
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     {
         .topic_id = to_underlying(TopicIndex::GEOPOSE_PUB),

libraries/AP_DDS/AP_DDS_config.h

@@ -98,6 +98,14 @@
 #define AP_DDS_DELAY_RC_TOPIC_MS 100
 #endif
 
+#ifndef AP_DDS_GROUNDTRUTH_PUB_ENABLED
+#define AP_DDS_GROUNDTRUTH_PUB_ENABLED AP_SIM_ENABLED
+#endif
+
+#ifndef AP_DDS_DELAY_GROUNDTRUTH_TOPIC_MS
+#define AP_DDS_DELAY_GROUNDTRUTH_TOPIC_MS 20
+#endif
+
 #ifndef AP_DDS_BATTERY_STATE_PUB_ENABLED
 #define AP_DDS_BATTERY_STATE_PUB_ENABLED 1
 #endif

libraries/AP_DDS/Idl/ardupilot_msgs/msg/GroundTruth.idl

@@ -0,0 +1,39 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from ardupilot_msgs/msg/GroundTruth.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 ardupilot_msgs {
+  module msg {
+    struct GroundTruth {
+      std_msgs::msg::Header header;
+
+      @verbatim (language="comment", text=
+        "Geographical position")
+      double latitude;
+
+      double longitude;
+
+      double altitude;
+
+      @verbatim (language="comment", text=
+        "Local ENU position (meters)")
+      geometry_msgs::msg::Vector3 position;
+
+      @verbatim (language="comment", text=
+        "Orientation (ENU)")
+      geometry_msgs::msg::Quaternion orientation;
+
+      @verbatim (language="comment", text=
+        "Linear velocity (ENU, m/s)")
+      geometry_msgs::msg::Vector3 linear_velocity;
+
+      @verbatim (language="comment", text=
+        "Angular velocity ( body frame, rad/s)")
+      geometry_msgs::msg::Vector3 angular_velocity;
+    };
+  };
+};

libraries/AP_DDS/README.md

@@ -173,6 +173,7 @@ Published topics:
  * /ap/pose/filtered [geometry_msgs/msg/PoseStamped] 1 publisher
  * /ap/rc [ardupilot_msgs/msg/Rc] 1 publisher
  * /ap/status [ardupilot_msgs/msg/Status] 1 publisher
+ * /ap/sim/groundtruth [ardupilot_msgs/msg/GroundTruth] 1 publisher
  * /ap/tf_static [tf2_msgs/msg/TFMessage] 1 publisher
  * /ap/time [builtin_interfaces/msg/Time] 1 publisher
  * /ap/twist/filtered [geometry_msgs/msg/TwistStamped] 1 publisher