From c46a4efb1baec9e6d1a27275e6be590ced12abfe Mon Sep 17 00:00:00 2001 From: fiorenzani Date: Mon, 7 Oct 2024 18:40:36 +0000 Subject: [PATCH 1/3] AP_DDS: added status enum --- libraries/AP_DDS/AP_DDS_Topic_Table.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 2329dde701964..e08768f2a42d4 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -26,6 +26,7 @@ enum class TopicIndex: uint8_t { GEOPOSE_PUB, CLOCK_PUB, GPS_GLOBAL_ORIGIN_PUB, + STATUS_PUB, JOY_SUB, DYNAMIC_TRANSFORMS_SUB, VELOCITY_CONTROL_SUB, From 070496919edfceaf59f35c660d61d2afea488fbd Mon Sep 17 00:00:00 2001 From: fiorenzani Date: Mon, 7 Oct 2024 22:19:54 +0000 Subject: [PATCH 2/3] AP_DDS: Status message --- Tools/ros2/ardupilot_msgs/CMakeLists.txt | 1 + Tools/ros2/ardupilot_msgs/msg/Status.msg | 26 +++++++ libraries/AP_DDS/AP_DDS_Client.cpp | 67 +++++++++++++++++++ libraries/AP_DDS/AP_DDS_Client.h | 10 +++ libraries/AP_DDS/AP_DDS_Topic_Table.h | 16 +++++ .../AP_DDS/Idl/ardupilot_msgs/msg/Status.idl | 52 ++++++++++++++ 6 files changed, 172 insertions(+) create mode 100644 Tools/ros2/ardupilot_msgs/msg/Status.msg create mode 100644 libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index d066a2d32b2e4..f8af788e8b81e 100644 --- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt +++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/GlobalPosition.msg" + "msg/Status.msg" "srv/ArmMotors.srv" "srv/ModeSwitch.srv" DEPENDENCIES geometry_msgs std_msgs diff --git a/Tools/ros2/ardupilot_msgs/msg/Status.msg b/Tools/ros2/ardupilot_msgs/msg/Status.msg new file mode 100644 index 0000000000000..6fd4a05617856 --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/msg/Status.msg @@ -0,0 +1,26 @@ +std_msgs/Header header + +uint8 APM_ROVER = 1 +uint8 APM_ARDUCOPTER = 2 +uint8 APM_ARDUPLANE = 3 +uint8 APM_ANTENNATRACKER = 4 +uint8 APM_UNKNOWN = 5 +uint8 APM_REPLAY = 6 +uint8 APM_ARDUSUB = 7 +uint8 APM_IOFIRMWARE = 8 +uint8 APM_AP_PERIPH = 9 +uint8 APM_AP_DAL_STANDALONE = 10 +uint8 APM_AP_BOOTLOADER = 11 +uint8 APM_BLIMP = 12 +uint8 APM_HELI = 13 +uint8 vehicle_type # From AP_Vehicle_Type.h + +bool armed # true if vehicle is armed. +uint8 mode # Vehicle mode, enum depending on vehicle type. +bool flying # True if flying/driving/diving/tracking. + +uint8 FS_RADIO = 1 +uint8 FS_BATTERY = 2 +uint8 FS_GCS = 3 +uint8 FS_EKF = 4 +uint8[] failsafe # Array containing all active failsafe. diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index d224a22551326..257f4ea373db6 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -42,6 +42,7 @@ static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33; static constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10; static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = 1000; static constexpr uint16_t DELAY_PING_MS = 500; +static constexpr uint16_t DELAY_STATUS_TOPIC_MS = 500; // Define the subscriber data members, which are static class scope. // If these are created on the stack in the subscriber, @@ -525,6 +526,50 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg) } } +bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg) +{ + // Fill the new message. + msg.vehicle_type = static_cast(APM_BUILD_DIRECTORY); + msg.armed = AP_Notify::flags.armed; + msg.mode = AP::vehicle()->get_mode(); + msg.flying = AP_Notify::flags.flying; + uint8_t fs_iter = 0; + msg.failsafe_size = 0; + if ( AP_Notify::flags.failsafe_radio ) { + msg.failsafe[fs_iter++] = FS_RADIO; + } + if ( AP_Notify::flags.failsafe_battery ) { + msg.failsafe[fs_iter++] = FS_BATTERY; + } + if ( AP_Notify::flags.failsafe_gcs ) { + msg.failsafe[fs_iter++] = FS_GCS; + } + if ( AP_Notify::flags.failsafe_ekf ) { + msg.failsafe[fs_iter++] = FS_EKF; + } + msg.failsafe_size = fs_iter; + + // Compare with the previous one. + bool is_message_changed {false}; + is_message_changed |= (last_status_msg_.flying != msg.flying); + is_message_changed |= (last_status_msg_.armed != msg.armed); + is_message_changed |= (last_status_msg_.mode != msg.mode); + is_message_changed |= (last_status_msg_.vehicle_type != msg.vehicle_type); + is_message_changed |= (last_status_msg_.failsafe_size != msg.failsafe_size); + + if ( is_message_changed ) { + last_status_msg_.flying = msg.flying; + last_status_msg_.armed = msg.armed; + last_status_msg_.mode = msg.mode; + last_status_msg_.vehicle_type = msg.vehicle_type; + last_status_msg_.failsafe_size = msg.failsafe_size; + update_topic(msg.header.stamp); + return true; + } else { + return false; + } +} + /* start the DDS thread */ @@ -1134,6 +1179,21 @@ void AP_DDS_Client::write_gps_global_origin_topic() } } +void AP_DDS_Client::write_status_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub {}; + const uint32_t topic_size = ardupilot_msgs_msg_Status_size_of_topic(&status_topic, 0); + uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::STATUS_PUB)].dw_id, &ub, topic_size); + const bool success = ardupilot_msgs_msg_Status_serialize_topic(&ub, &status_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); @@ -1194,6 +1254,13 @@ void AP_DDS_Client::update() write_gps_global_origin_topic(); } + if (cur_time_ms - last_status_check_time_ms > DELAY_STATUS_TOPIC_MS) { + if (update_topic(status_topic)) { + write_status_topic(); + } + last_status_check_time_ms = cur_time_ms; + } + status_ok = uxr_run_session_time(&session, 1); } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index c020eae17933c..de1f95038403a 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -8,6 +8,7 @@ #include "ucdr/microcdr.h" #include "ardupilot_msgs/msg/GlobalPosition.h" +#include "ardupilot_msgs/msg/Status.h" #include "builtin_interfaces/msg/Time.h" #include "sensor_msgs/msg/NavSatFix.h" @@ -63,6 +64,7 @@ class AP_DDS_Client builtin_interfaces_msg_Time time_topic; geographic_msgs_msg_GeoPointStamped gps_global_origin_topic; geographic_msgs_msg_GeoPoseStamped geo_pose_topic; + ardupilot_msgs_msg_Status status_topic; geometry_msgs_msg_PoseStamped local_pose_topic; geometry_msgs_msg_TwistStamped tx_local_velocity_topic; sensor_msgs_msg_BatteryState battery_state_topic; @@ -95,6 +97,7 @@ class AP_DDS_Client 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); + bool update_topic(ardupilot_msgs_msg_Status& msg); #if AP_DDS_IMU_PUB_ENABLED static void update_topic(sensor_msgs_msg_Imu& msg); #endif // AP_DDS_IMU_PUB_ENABLED @@ -137,6 +140,8 @@ class AP_DDS_Client uint64_t last_clock_time_ms; // The last ms timestamp AP_DDS wrote a gps global origin message uint64_t last_gps_global_origin_time_ms; + // The last ms timestamp AP_DDS wrote/checked a status message + uint64_t last_status_check_time_ms; // functions for serial transport bool ddsSerialInit(); @@ -172,6 +177,9 @@ class AP_DDS_Client // client key we present static constexpr uint32_t key = 0xAAAABBBB; + // last status values; + ardupilot_msgs_msg_Status last_status_msg_; + public: ~AP_DDS_Client(); @@ -201,6 +209,8 @@ class AP_DDS_Client void write_battery_state_topic(); //! @brief Serialize the current local_pose and publish to the IO stream(s) void write_local_pose_topic(); + //! @brief Serialize the current status and publish to the IO stream(s) + void write_status_topic(); //! @brief Serialize the current local velocity and publish to the IO stream(s) void write_tx_local_velocity_topic(); //! @brief Serialize the current geo_pose and publish to the IO stream(s) diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index e08768f2a42d4..b78fba3add629 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -203,6 +203,22 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .depth = 5, }, }, + { + .topic_id = to_underlying(TopicIndex::STATUS_PUB), + .pub_id = to_underlying(TopicIndex::STATUS_PUB), + .sub_id = to_underlying(TopicIndex::STATUS_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::STATUS_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::STATUS_PUB), .type=UXR_DATAREADER_ID}, + .topic_rw = Topic_rw::DataWriter, + .topic_name = "rt/ap/status", + .type_name = "ardupilot_msgs::msg::dds_::Status_", + .qos = { + .durability = UXR_DURABILITY_TRANSIENT_LOCAL, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 1, + }, + }, { .topic_id = to_underlying(TopicIndex::JOY_SUB), .pub_id = to_underlying(TopicIndex::JOY_SUB), diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl new file mode 100644 index 0000000000000..f0a74dcab9ca1 --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl @@ -0,0 +1,52 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from ardupilot_msgs/msg/Status.msg +// generated code does not contain a copyright notice + +#include "std_msgs/msg/Header.idl" + +module ardupilot_msgs { + module msg { + module Status_Constants { + const uint8 APM_ROVER = 1; + const uint8 APM_ARDUCOPTER = 2; + const uint8 APM_ARDUPLANE = 3; + const uint8 APM_ANTENNATRACKER = 4; + const uint8 APM_UNKNOWN = 5; + const uint8 APM_REPLAY = 6; + const uint8 APM_ARDUSUB = 7; + const uint8 APM_IOFIRMWARE = 8; + const uint8 APM_AP_PERIPH = 9; + const uint8 APM_AP_DAL_STANDALONE = 10; + const uint8 APM_AP_BOOTLOADER = 11; + const uint8 APM_BLIMP = 12; + const uint8 APM_HELI = 13; + const uint8 FS_RADIO = 1; + const uint8 FS_BATTERY = 2; + const uint8 FS_GCS = 3; + const uint8 FS_EKF = 4; + }; + struct Status { + std_msgs::msg::Header header; + + @verbatim (language="comment", text= + "From AP_Vehicle_Type.h") + uint8 vehicle_type; + + @verbatim (language="comment", text= + "true if vehicle is armed.") + boolean armed; + + @verbatim (language="comment", text= + "Vehicle mode, enum depending on vehicle type.") + uint8 mode; + + @verbatim (language="comment", text= + "True if flying/driving/diving/tracking.") + boolean flying; + + @verbatim (language="comment", text= + "Array containing all active failsafe.") + sequence failsafe; + }; + }; +}; From 31ca9c3d769c92d5489eec41d9c59828122e3e7b Mon Sep 17 00:00:00 2001 From: fiorenzani Date: Mon, 7 Oct 2024 23:35:49 +0000 Subject: [PATCH 3/3] AP_DDS: vehicle_type opbtained from fwversion() --- libraries/AP_DDS/AP_DDS_Client.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 257f4ea373db6..48e215477c789 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include "ardupilot_msgs/srv/ArmMotors.h" @@ -529,7 +530,7 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg) bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg) { // Fill the new message. - msg.vehicle_type = static_cast(APM_BUILD_DIRECTORY); + msg.vehicle_type = static_cast(AP::fwversion().vehicle_type); msg.armed = AP_Notify::flags.armed; msg.mode = AP::vehicle()->get_mode(); msg.flying = AP_Notify::flags.flying;