diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 0d7b26d7b4..2e9fe20239 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/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(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 diff --git a/libraries/AP_DDS/AP_DDS_External_Odom.cpp b/libraries/AP_DDS/AP_DDS_External_Odom.cpp new file mode 100644 index 0000000000..cf121f4085 --- /dev/null +++ b/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 +#include + +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(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(ros_transform.translation.x), + static_cast(-ros_transform.translation.y), + static_cast(-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 diff --git a/libraries/AP_DDS/AP_DDS_External_Odom.h b/libraries/AP_DDS/AP_DDS_External_Odom.h new file mode 100644 index 0000000000..2cba26a16d --- /dev/null +++ b/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 diff --git a/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp b/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp new file mode 100644 index 0000000000..2aa4caf543 --- /dev/null +++ b/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 diff --git a/libraries/AP_DDS/AP_DDS_Type_Conversions.h b/libraries/AP_DDS/AP_DDS_Type_Conversions.h new file mode 100644 index 0000000000..71b2e3182d --- /dev/null +++ b/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 diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index e854e21596..febd1a7542 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -2,7 +2,16 @@ #include +#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 +#ifndef AP_DDS_VISUALODOM_ENABLED +#define AP_DDS_VISUALODOM_ENABLED HAL_VISUALODOM_ENABLED && AP_DDS_ENABLED #endif diff --git a/libraries/AP_DDS/tests/test_ap_dds_external_odom.cpp b/libraries/AP_DDS/tests/test_ap_dds_external_odom.cpp new file mode 100644 index 0000000000..804d891e16 --- /dev/null +++ b/libraries/AP_DDS/tests/test_ap_dds_external_odom.cpp @@ -0,0 +1,38 @@ +#include + +#include +#include "geometry_msgs/msg/TransformStamped.h" +#include + +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() diff --git a/libraries/AP_DDS/tests/test_ap_dds_type_conversions.cpp b/libraries/AP_DDS/tests/test_ap_dds_type_conversions.cpp new file mode 100644 index 0000000000..f0b89f207d --- /dev/null +++ b/libraries/AP_DDS/tests/test_ap_dds_type_conversions.cpp @@ -0,0 +1,28 @@ +#include + +#include +#include "builtin_interfaces/msg/Time.h" +#include + + +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() diff --git a/libraries/AP_DDS/tests/wscript b/libraries/AP_DDS/tests/wscript new file mode 100644 index 0000000000..5f82c39f83 --- /dev/null +++ b/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'] + )