From 602e825237df256819c0a466a9ed0b4b0cb6be2b Mon Sep 17 00:00:00 2001 From: Tiziano Fiorenzani Date: Tue, 8 Oct 2024 15:58:31 +0000 Subject: [PATCH] AP_DDS: Airspeed topic --- libraries/AP_DDS/AP_DDS_Client.cpp | 56 ++++++++++++++++++- libraries/AP_DDS/AP_DDS_Client.h | 14 ++++- libraries/AP_DDS/AP_DDS_Topic_Table.h | 22 ++++++++ libraries/AP_DDS/AP_DDS_config.h | 4 ++ .../Idl/geometry_msgs/msg/Vector3Stamped.idl | 21 +++++++ libraries/AP_DDS/README.md | 1 + 6 files changed, 115 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_DDS/Idl/geometry_msgs/msg/Vector3Stamped.idl diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 926d4304b701b..5ef37ca24895b 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -46,6 +46,9 @@ static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33; #if AP_DDS_LOCAL_VEL_PUB_ENABLED static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33; #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED +#if AP_DDS_AIRSPEED_PUB_ENABLED +static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = 33; +#endif // AP_DDS_AIRSPEED_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33; #endif // AP_DDS_GEOPOSE_PUB_ENABLED @@ -453,6 +456,34 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg) msg.twist.angular.z = -angular_velocity[2]; } #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED +#if AP_DDS_AIRSPEED_PUB_ENABLED +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; +} +#endif // AP_DDS_AIRSPEED_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) @@ -1128,7 +1159,22 @@ void AP_DDS_Client::write_tx_local_velocity_topic() } } #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED - +#if AP_DDS_AIRSPEED_PUB_ENABLED +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"); + } + } +} +#endif // AP_DDS_AIRSPEED_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED void AP_DDS_Client::write_imu_topic() { @@ -1236,6 +1282,14 @@ void AP_DDS_Client::update() write_tx_local_velocity_topic(); } #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED +#if AP_DDS_AIRSPEED_PUB_ENABLED + if (cur_time_ms - last_airspeed_time_ms > DELAY_AIRSPEED_TOPIC_MS) { + last_airspeed_time_ms = cur_time_ms; + if (update_topic(tx_local_airspeed_topic)) { + write_tx_local_airspeed_topic(); + } + } +#endif // AP_DDS_AIRSPEED_PUB_ENABLED #if AP_DDS_IMU_PUB_ENABLED if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) { update_topic(imu_topic); diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 2d2ec5a27f0cc..005b87f30aa21 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -37,6 +37,9 @@ #if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED #include "geographic_msgs/msg/GeoPointStamped.h" #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_AIRSPEED_PUB_ENABLED +#include "geometry_msgs/msg/Vector3Stamped.h" +#endif // AP_DDS_AIRSPEED_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED #include "geographic_msgs/msg/GeoPoseStamped.h" #endif // AP_DDS_GEOPOSE_PUB_ENABLED @@ -127,6 +130,15 @@ class AP_DDS_Client static void update_topic(geometry_msgs_msg_TwistStamped& msg); #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED +#if AP_DDS_AIRSPEED_PUB_ENABLED + geometry_msgs_msg_Vector3Stamped tx_local_airspeed_topic; + // The last ms timestamp AP_DDS wrote a airspeed message + uint64_t last_airspeed_time_ms; + //! @brief Serialize the current local airspeed and publish to the IO stream(s) + void write_tx_local_airspeed_topic(); + static bool update_topic(geometry_msgs_msg_Vector3Stamped& msg); +#endif //AP_DDS_AIRSPEED_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 @@ -193,8 +205,6 @@ class AP_DDS_Client bool status_ok{false}; bool connected{false}; - - // subscription callback function static void on_topic_trampoline(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length, void* args); void on_topic(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length); diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 4864a15b672e9..05d68e0c78e3f 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/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 @@ -35,6 +36,9 @@ enum class TopicIndex: uint8_t { #if AP_DDS_LOCAL_VEL_PUB_ENABLED LOCAL_VELOCITY_PUB, #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED +#if AP_DDS_AIRSPEED_PUB_ENABLED + LOCAL_AIRSPEED_PUB, +#endif // AP_DDS_AIRSPEED_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED GEOPOSE_PUB, #endif // AP_DDS_GEOPOSE_PUB_ENABLED @@ -192,6 +196,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { }, }, #endif // AP_DDS_LOCAL_VEL_PUB_ENABLED +#if AP_DDS_AIRSPEED_PUB_ENABLED + { + .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, + }, + }, +#endif // AP_DDS_AIRSPEED_PUB_ENABLED #if AP_DDS_GEOPOSE_PUB_ENABLED { .topic_id = to_underlying(TopicIndex::GEOPOSE_PUB), diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 01f168ea217ee..2e0d3d6264f8c 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -54,6 +54,10 @@ #define AP_DDS_LOCAL_VEL_PUB_ENABLED 1 #endif +#ifndef AP_DDS_AIRSPEED_PUB_ENABLED +#define AP_DDS_AIRSPEED_PUB_ENABLED 1 +#endif + #ifndef AP_DDS_BATTERY_STATE_PUB_ENABLED #define AP_DDS_BATTERY_STATE_PUB_ENABLED 1 #endif diff --git a/libraries/AP_DDS/Idl/geometry_msgs/msg/Vector3Stamped.idl b/libraries/AP_DDS/Idl/geometry_msgs/msg/Vector3Stamped.idl new file mode 100644 index 0000000000000..edb586e2138d3 --- /dev/null +++ b/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; + }; + }; +}; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 3b163a3948ea9..3a037f9b9782d 100644 --- a/libraries/AP_DDS/README.md +++ b/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