AP_DDS: Add GeoPose support

* Add GeoPoint.idl
* Add GeoPose.idl
* Add GeoPoseStamped.idl
* Update geopose topic name to follow #23603
* Add GeoPose topic support

Co-authored-by: Pedro Fuoco <pedrofuoco6@gmail.com>
Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -16,6 +16,7 @@ static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10;
 static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000;
 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;
 static char WGS_84_FRAME_ID[] = "WGS-84";
 // https://www.ros.org/reps/rep-0105.html#base-link
 static char BASE_LINK_FRAME_ID[] = "base_link";
@@ -338,6 +339,41 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
     msg.twist.angular.z = -angular_velocity[2];
 }
 
+void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
+{
+    update_topic(msg.header.stamp);
+    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+
+    auto &ahrs = AP::ahrs();
+    WITH_SEMAPHORE(ahrs.get_semaphore());
+
+    Location loc;
+    if (ahrs.get_location(loc)) {
+        msg.pose.position.latitude = loc.lat * 1E-7;
+        msg.pose.position.longitude = loc.lng * 1E-7;
+        msg.pose.position.altitude = loc.alt / 100.0; // Transform from cm to m
+    }
+
+    // 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
+    // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
+    // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis
+    // for x to point forward
+    Quaternion orientation;
+    if (ahrs.get_quaternion(orientation)) {
+        Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation
+        Quaternion transformation(sqrt(2) / 2, 0, 0, sqrt(2) / 2); // Z axis 90 degree rotation
+        orientation = aux * transformation;
+        msg.pose.orientation.w = orientation[0];
+        msg.pose.orientation.x = orientation[1];
+        msg.pose.orientation.y = orientation[2];
+        msg.pose.orientation.z = orientation[3];
+    }
+}
+
 /*
   start the DDS thread
  */
@@ -576,6 +612,21 @@ void AP_DDS_Client::write_local_velocity_topic()
     }
 }
 
+void AP_DDS_Client::write_geo_pose_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub;
+        const uint32_t topic_size = geographic_msgs_msg_GeoPoseStamped_size_of_topic(&geo_pose_topic, 0);
+        uxr_prepare_output_stream(&session,reliable_out,topics[6].dw_id,&ub,topic_size);
+        const bool success = geographic_msgs_msg_GeoPoseStamped_serialize_topic(&ub, &geo_pose_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::update()
 {
     WITH_SEMAPHORE(csem);
@@ -592,8 +643,6 @@ void AP_DDS_Client::update()
         write_nav_sat_fix_topic();
     }
 
-
-
     if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) {
         constexpr uint8_t battery_instance = 0;
         update_topic(battery_state_topic, battery_instance);
@@ -613,6 +662,12 @@ void AP_DDS_Client::update()
         write_local_velocity_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;
+        write_geo_pose_topic();
+    }
+
     connected = uxr_run_session_time(&session, 1);
 }
 

libraries/AP_DDS/AP_DDS_Client.h

@@ -12,6 +12,7 @@
 #include "sensor_msgs/msg/BatteryState.h"
 #include "geometry_msgs/msg/PoseStamped.h"
 #include "geometry_msgs/msg/TwistStamped.h"
+#include "geographic_msgs/msg/GeoPoseStamped.h"
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/Scheduler.h>
@@ -58,6 +59,7 @@ private:
     sensor_msgs_msg_BatteryState battery_state_topic;
     geometry_msgs_msg_PoseStamped local_pose_topic;
     geometry_msgs_msg_TwistStamped local_velocity_topic;
+    geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
 
     HAL_Semaphore csem;
 
@@ -70,6 +72,7 @@ private:
     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 void update_topic(geographic_msgs_msg_GeoPoseStamped& msg);
 
     // The last ms timestamp AP_DDS wrote a Time message
     uint64_t last_time_time_ms;
@@ -81,6 +84,8 @@ private:
     uint64_t last_local_pose_time_ms;
     // The last ms timestamp AP_DDS wrote a Local Velocity message
     uint64_t last_local_velocity_time_ms;
+    // The last ms timestamp AP_DDS wrote a GeoPose message
+    uint64_t last_geo_pose_time_ms;
 
     // functions for serial transport
     bool ddsSerialInit();
@@ -135,10 +140,12 @@ public:
     void write_static_transforms();
     //! @brief Serialize the current nav_sat_fix state and publish it to the IO stream(s)
     void write_battery_state_topic();
-    //! @brief Serialize the current local pose and publish to the IO stream(s)
+    //! @brief Serialize the current local_pose and publish to the IO stream(s)
     void write_local_pose_topic();
     //! @brief Serialize the current local velocity and publish to the IO stream(s)
     void write_local_velocity_topic();
+    //! @brief Serialize the current geo_pose and publish to the IO stream(s)
+    void write_geo_pose_topic();
     //! @brief Update the internally stored DDS messages with latest data
     void update();
 

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -2,6 +2,7 @@
 #include "sensor_msgs/msg/NavSatFix.h"
 #include "tf2_msgs/msg/TFMessage.h"
 #include "sensor_msgs/msg/BatteryState.h"
+#include "geographic_msgs/msg/GeoPoseStamped.h"
 
 #include "AP_DDS_Generic_Fn_T.h"
 #include "uxr/client/client.h"
@@ -71,5 +72,15 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .serialize = Generic_serialize_topic_fn_t(&geometry_msgs_msg_TwistStamped_serialize_topic),
         .deserialize = Generic_deserialize_topic_fn_t(&geometry_msgs_msg_TwistStamped_deserialize_topic),
         .size_of = Generic_size_of_topic_fn_t(&geometry_msgs_msg_TwistStamped_size_of_topic),
-    }
+    },
+    {
+        .topic_id = 0x07,
+        .pub_id = 0x07,
+        .dw_id = uxrObjectId{.id=0x07, .type=UXR_DATAWRITER_ID},
+        .topic_profile_label = "geopose__t",
+        .dw_profile_label = "geopose__dw",
+        .serialize = Generic_serialize_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_serialize_topic),
+        .deserialize = Generic_deserialize_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_deserialize_topic),
+        .size_of = Generic_size_of_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_size_of_topic),
+    },
 };

libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPoint.idl

@@ -0,0 +1,30 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from geographic_msgs/msg/GeoPoint.msg
+// generated code does not contain a copyright notice
+
+
+module geographic_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "Geographic point, using the WGS 84 reference ellipsoid.")
+    struct GeoPoint {
+      @verbatim (language="comment", text=
+        "Latitude. Positive is north of equator; negative is south" "\n"
+        "(-90 <= latitude <= +90).")
+      @unit (value="degrees")
+      double latitude;
+
+      @verbatim (language="comment", text=
+        "Longitude. Positive is east of prime meridian; negative is" "\n"
+        "west (-180 <= longitude <= +180). At the poles, latitude is -90 or" "\n"
+        "+90, and longitude is irrelevant, but must be in range.")
+      @unit (value="degrees")
+      double longitude;
+
+      @verbatim (language="comment", text=
+        "Altitude. Positive is above the WGS 84 ellipsoid (NaN if unspecified).")
+      @unit (value="m")
+      double altitude;
+    };
+  };
+};

libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPose.idl

@@ -0,0 +1,21 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from geographic_msgs/msg/GeoPose.msg
+// generated code does not contain a copyright notice
+
+#include "geometry_msgs/msg/Quaternion.idl"
+#include "geographic_msgs/msg/GeoPoint.idl"
+
+module geographic_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "Geographic pose, using the WGS 84 reference ellipsoid." "\n"
+      "" "\n"
+      "Orientation uses the East-North-Up (ENU) frame of reference." "\n"
+      "(But, what about singularities at the poles?)")
+    struct GeoPose {
+      geographic_msgs::msg::GeoPoint position;
+
+      geometry_msgs::msg::Quaternion orientation;
+    };
+  };
+};

libraries/AP_DDS/Idl/geographic_msgs/msg/GeoPoseStamped.idl

@@ -0,0 +1,16 @@
+// generated from rosidl_adapter/resource/msg.idl.em
+// with input from geographic_msgs/msg/GeoPoseStamped.msg
+// generated code does not contain a copyright notice
+
+#include "geographic_msgs/msg/GeoPose.idl"
+#include "std_msgs/msg/Header.idl"
+
+module geographic_msgs {
+  module msg {
+    struct GeoPoseStamped {
+      std_msgs::msg::Header header;
+
+      geographic_msgs::msg::GeoPose pose;
+    };
+  };
+};

libraries/AP_DDS/README.md

@@ -105,6 +105,11 @@ Follow the steps to use the microROS Agent
 
   - https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html
 
+- Install geographic_msgs
+  ```bash
+  sudo apt install ros-humble-geographic-msgs
+  ```
+
 - Install and run the microROS agent (as descibed here). Make sure to use the `humble` branch.
   - Follow [the instructions](https://micro.ros.org/docs/tutorials/core/first_application_linux/) for the following:
 

libraries/AP_DDS/dds_xrce_profile.xml

@@ -170,4 +170,32 @@
       </historyQos>
     </topic>
   </data_writer>
+  <topic profile_name="geopose__t">
+    <name>rt/ap/geopose/filtered</name>
+    <dataType>geographic_msgs::msg::dds_::GeoPoseStamped_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>5</depth>
+    </historyQos>
+  </topic>
+  <data_writer profile_name="geopose__dw">
+    <historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
+    <qos>
+      <reliability>
+        <kind>BEST_EFFORT</kind>
+      </reliability>
+      <durability>
+        <kind>VOLATILE</kind>
+      </durability>
+    </qos>
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ap/geopose/filtered</name>
+      <dataType>geographic_msgs::msg::dds_::GeoPoseStamped_</dataType>
+      <historyQos>
+        <kind>KEEP_LAST</kind>
+        <depth>5</depth>
+      </historyQos>
+    </topic>
+  </data_writer>
 </profiles>