AP_DDS: Stub out external odom

* Implement frame ID checking and test it
* Implement the visual odom function that does narrowing to floats
* Normalize quaternions from ROS
* Supply 0 error to EKF
* Handle external odomo only if HAL_VISUALODOM_ENABLED is defined
* Implement odom timestamping and improve docs
* Add unit tests
* Add a CONFIG file for DDS

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -19,10 +19,10 @@
 #include "AP_DDS_Client.h"
 #include "AP_DDS_Topic_Table.h"
 #include "AP_DDS_Service_Table.h"
+#include "AP_DDS_External_Odom.h"
 
 // Enable DDS at runtime by default
 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_LOCAL_POSE_TOPIC_MS = 33;
@@ -461,9 +461,10 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
 
         subscribe_sample_count++;
         if (rx_dynamic_transforms_topic.transforms_size > 0) {
-            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage of length: %u",
-                          static_cast<unsigned>(rx_dynamic_transforms_topic.transforms_size));
-            // TODO implement external odometry to AP
+#if AP_DDS_VISUALODOM_ENABLED
+            AP_DDS_External_Odom::handle_external_odom(rx_dynamic_transforms_topic);
+#endif // AP_DDS_VISUALODOM_ENABLED
+
         } else {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage: Insufficient size ");
         }
@@ -935,7 +936,7 @@ int clock_gettime(clockid_t clockid, struct timespec *ts)
     ts->tv_nsec = (utc_usec % 1000000ULL) * 1000UL;
     return 0;
 }
-#endif // CONFIG_HAL_BOARD
+#endif // CONFIG_HAL_BOARD != HAL_BOARD_SITL
 
 #endif // AP_DDS_ENABLED
 

libraries/AP_DDS/AP_DDS_External_Odom.cpp

@@ -0,0 +1,76 @@
+
+
+#include "AP_DDS_External_Odom.h"
+#include "AP_DDS_Type_Conversions.h"
+
+#if AP_DDS_VISUALODOM_ENABLED
+
+#include <AP_VisualOdom/AP_VisualOdom.h>
+#include <GCS_MAVLink/GCS.h>
+
+void AP_DDS_External_Odom::handle_external_odom(const tf2_msgs_msg_TFMessage& msg)
+{
+    auto *visual_odom = AP::visualodom();
+    if (visual_odom == nullptr) {
+        return;
+    }
+
+    for (size_t i = 0; i < msg.transforms_size; i++) {
+        const auto& ros_transform_stamped = msg.transforms[i];
+        if (!is_odometry_frame(ros_transform_stamped)) {
+            continue;
+        }
+        const uint64_t remote_time_us {AP_DDS_Type_Conversions::time_u64_micros(ros_transform_stamped.header.stamp)};
+
+        Vector3f ap_position;
+        Quaternion ap_rotation;
+
+        convert_transform(ros_transform_stamped.transform, ap_position, ap_rotation);
+        // Although ROS convention states quaternions in ROS messages should be normalized, it's not guaranteed.
+        // Before propogating a potentially inaccurate quaternion to the rest of AP, normalize it here.
+        // TODO what if the quaternion is NaN?
+        ap_rotation.normalize();
+
+        // No error is available in TF, trust the data as-is
+        const float posErr {0.0};
+        const float angErr {0.0};
+        // The odom to base_link transform used is locally consistent per ROS REP-105.
+        // https://www.ros.org/reps/rep-0105.html#id16
+        // Thus, there will not be any resets.
+        const uint8_t reset_counter {0};
+        // TODO imlement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg));
+        const uint32_t time_ms {static_cast<uint32_t>(remote_time_us * 1E-3)};
+        visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter);
+
+    }
+}
+
+bool AP_DDS_External_Odom::is_odometry_frame(const geometry_msgs_msg_TransformStamped& msg)
+{
+    char odom_parent[] = "odom";
+    char odom_child[] = "base_link";
+    // Assume the frame ID's are null terminated.
+    return (strcmp(msg.header.frame_id, odom_parent) == 0) &&
+           (strcmp(msg.child_frame_id, odom_child) == 0);
+}
+
+void AP_DDS_External_Odom::convert_transform(const geometry_msgs_msg_Transform& ros_transform, Vector3f& translation, Quaternion& rotation)
+{
+    // convert from x-forward, y-left, z-up to NED
+    // https://github.com/mavlink/mavros/issues/49#issuecomment-51614130
+    translation = {
+        static_cast<float>(ros_transform.translation.x),
+        static_cast<float>(-ros_transform.translation.y),
+        static_cast<float>(-ros_transform.translation.z)
+    };
+
+    // In AP, q1 is the quaternion's scalar component.
+    // In ROS, w is the quaternion's scalar component.
+    // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html#components-of-a-quaternion
+    rotation.q1 = ros_transform.rotation.w;
+    rotation.q2 = ros_transform.rotation.x;
+    rotation.q3 = -ros_transform.rotation.y;
+    rotation.q4 = -ros_transform.rotation.z;
+}
+
+#endif // AP_DDS_VISUALODOM_ENABLED

libraries/AP_DDS/AP_DDS_External_Odom.h

@@ -0,0 +1,33 @@
+// Class for handling external localization data.
+// For historical reasons, it's called odometry to match AP_VisualOdom.
+
+#pragma once
+
+#include "AP_DDS_config.h"
+#if AP_DDS_VISUALODOM_ENABLED
+
+#include "geometry_msgs/msg/TransformStamped.h"
+#include "tf2_msgs/msg/TFMessage.h"
+#include "AP_Math/vector3.h"
+#include "AP_Math/quaternion.h"
+
+class AP_DDS_External_Odom
+{
+public:
+
+    // Handler for external position localization
+    static void handle_external_odom(const tf2_msgs_msg_TFMessage& msg);
+
+    // Checks the child and parent frames match a set needed for external odom.
+    // Since multiple different transforms can be sent, this validates the specific transform is
+    // for odometry.
+    static bool is_odometry_frame(const geometry_msgs_msg_TransformStamped& msg);
+
+    // Helper to convert from ROS transform to AP datatypes
+    // ros_transform is in ENU
+    // translation is in NED
+    static void convert_transform(const geometry_msgs_msg_Transform& ros_transform, Vector3f& translation, Quaternion& rotation);
+
+};
+
+#endif // AP_DDS_VISUALODOM_ENABLED

libraries/AP_DDS/AP_DDS_Type_Conversions.cpp

@@ -0,0 +1,13 @@
+#include "AP_DDS_Type_Conversions.h"
+#if AP_DDS_ENABLED
+
+#include "builtin_interfaces/msg/Time.h"
+
+
+uint64_t AP_DDS_Type_Conversions::time_u64_micros(const builtin_interfaces_msg_Time& ros_time)
+{
+    return (uint64_t(ros_time.sec) * 1000000ULL) + (ros_time.nanosec / 1000ULL);
+}
+
+
+#endif // AP_DDS_ENABLED

libraries/AP_DDS/AP_DDS_Type_Conversions.h

@@ -0,0 +1,17 @@
+// Class for handling type conversions for DDS.
+
+#pragma once
+
+#if AP_DDS_ENABLED
+
+#include "builtin_interfaces/msg/Time.h"
+
+class AP_DDS_Type_Conversions
+{
+public:
+
+    // Convert ROS time to a uint64_t [μS]
+    static uint64_t time_u64_micros(const builtin_interfaces_msg_Time& ros_time);
+};
+
+#endif // AP_DDS_ENABLED

libraries/AP_DDS/AP_DDS_config.h

@@ -2,7 +2,16 @@
 
 #include <AP_HAL/AP_HAL_Boards.h>
 
+#ifndef AP_DDS_ENABLED
+#define AP_DDS_ENABLED 1
+#endif
+
 // UDP only on SITL for now
 #ifndef AP_DDS_UDP_ENABLED
-#define AP_DDS_UDP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#define AP_DDS_UDP_ENABLED AP_DDS_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif
+
+#include <AP_VisualOdom/AP_VisualOdom_config.h>
+#ifndef AP_DDS_VISUALODOM_ENABLED
+#define AP_DDS_VISUALODOM_ENABLED HAL_VISUALODOM_ENABLED && AP_DDS_ENABLED
 #endif

libraries/AP_DDS/tests/test_ap_dds_external_odom.cpp

@@ -0,0 +1,38 @@
+#include <AP_gtest.h>
+
+#include <AP_DDS/AP_DDS_External_Odom.h>
+#include "geometry_msgs/msg/TransformStamped.h"
+#include <AP_HAL/AP_HAL.h>
+
+const AP_HAL::HAL &hal = AP_HAL::get_HAL();
+
+TEST(AP_DDS_EXTERNAL_ODOM, test_is_odometry_success)
+{
+    geometry_msgs_msg_TransformStamped msg {};
+
+    strncpy(msg.header.frame_id, "odom", strlen("odom") + 1);
+    strncpy(msg.child_frame_id, "base_link", strlen("base_link") + 1);
+    ASSERT_TRUE(AP_DDS_External_Odom::is_odometry_frame(msg));
+
+    strncpy(msg.header.frame_id, "invalid", strlen("invalid") + 1);
+    strncpy(msg.child_frame_id, "base_link", strlen("base_link") + 1);
+    ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
+
+    strncpy(msg.header.frame_id, "odom", strlen("odom") + 1);
+    strncpy(msg.child_frame_id, "invalid", strlen("invalid") + 1);
+    ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
+
+    strncpy(msg.header.frame_id, "odom_with_invalid_extra", strlen("odom_with_invalid_extra") + 1);
+    strncpy(msg.child_frame_id, "base_link", strlen("base_link") + 1);
+    ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
+
+    strncpy(msg.header.frame_id, "odom", strlen("odom") + 1);
+    strncpy(msg.child_frame_id, "base_link_with_invalid_extra", strlen("base_link_with_invalid_extra") + 1);
+    ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
+
+    strncpy(msg.header.frame_id, "x", strlen("x") + 1);
+    strncpy(msg.child_frame_id, "base_link", strlen("base_link") + 1);
+    ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
+}
+
+AP_GTEST_MAIN()

libraries/AP_DDS/tests/test_ap_dds_type_conversions.cpp

@@ -0,0 +1,28 @@
+#include <AP_gtest.h>
+
+#include <AP_DDS/AP_DDS_Type_Conversions.h>
+#include "builtin_interfaces/msg/Time.h"
+#include <AP_HAL/AP_HAL.h>
+
+
+const AP_HAL::HAL &hal = AP_HAL::get_HAL();
+
+TEST(AP_DDS_TYPE_CONVERSIONS, test_time_u64_micros)
+{
+    builtin_interfaces_msg_Time ros_time {};
+    ASSERT_EQ(AP_DDS_Type_Conversions::time_u64_micros(ros_time), 0UL);
+
+    ros_time.sec = 5;
+    ASSERT_EQ(AP_DDS_Type_Conversions::time_u64_micros(ros_time), uint64_t(5E6));
+
+    ros_time.nanosec = 1000;
+    const uint64_t expected5 = uint64_t(5E6) + 1; 
+    ASSERT_EQ(AP_DDS_Type_Conversions::time_u64_micros(ros_time), expected5);
+
+    ros_time.sec = 7 * 24 * 60 * 60; // 1 week of runtime
+    ros_time.nanosec = 1000;
+    const uint64_t expected_long_runtime = uint64_t(ros_time.sec) * 1000000 + 1; 
+    ASSERT_EQ(AP_DDS_Type_Conversions::time_u64_micros(ros_time), expected_long_runtime);   
+}
+
+AP_GTEST_MAIN()

libraries/AP_DDS/tests/wscript

@@ -0,0 +1,10 @@
+#!/usr/bin/env python3
+
+def build(bld):
+    if not bld.env.ENABLE_DDS:
+        return
+
+    bld.ap_find_tests(
+        use='ap',
+        DOUBLE_PRECISION_SOURCES = ['test_ap_dds_external_odom.cpp']
+    )