AP_DDS: Vehicle Status publisher

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

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 = 21
+uint8 FS_BATTERY = 22
+uint8 FS_GCS = 23
+uint8 FS_EKF = 24
+uint8[] failsafe # Array containing all active failsafe.

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -14,6 +14,7 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Arming/AP_Arming.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_Common/AP_FWVersion.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
 
 #include "ardupilot_msgs/srv/ArmMotors.h"
@@ -59,6 +60,9 @@ static constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10;
 static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = 1000;
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
 static constexpr uint16_t DELAY_PING_MS = 500;
+#ifdef AP_DDS_STATUS_PUB_ENABLED
+static constexpr uint16_t DELAY_STATUS_TOPIC_MS = 100;
+#endif // AP_DDS_STATUS_PUB_ENABLED
 
 // Define the subscriber data members, which are static class scope.
 // If these are created on the stack in the subscriber,
@@ -596,6 +600,53 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg)
 }
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
 
+#if AP_DDS_STATUS_PUB_ENABLED
+bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg)
+{
+    // Fill the new message.
+    const auto &vehicle = AP::vehicle();
+    const auto &battery = AP::battery();
+    msg.vehicle_type = static_cast<uint8_t>(AP::fwversion().vehicle_type);
+    msg.armed = hal.util->get_soft_armed();
+    msg.mode = vehicle->get_mode();
+    msg.flying = vehicle->get_likely_flying();
+    uint8_t fs_iter = 0;
+    msg.failsafe_size = 0;
+    if (AP_Notify::flags.failsafe_radio) {
+        msg.failsafe[fs_iter++] = FS_RADIO;
+    }
+    if (battery.has_failsafed()) {
+        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;
+    }
+}
+#endif // AP_DDS_STATUS_PUB_ENABLED
 /*
   start the DDS thread
  */
@@ -1245,6 +1296,23 @@ void AP_DDS_Client::write_gps_global_origin_topic()
 }
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
 
+#if AP_DDS_STATUS_PUB_ENABLED
+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");
+        }
+    }
+}
+#endif // AP_DDS_STATUS_PUB_ENABLED
+
 void AP_DDS_Client::update()
 {
     WITH_SEMAPHORE(csem);
@@ -1324,10 +1392,17 @@ void AP_DDS_Client::update()
         write_gps_global_origin_topic();
     }
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
+#if AP_DDS_STATUS_PUB_ENABLED
+    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);
 }
-
+#endif // AP_DDS_STATUS_PUB_ENABLED
 #if CONFIG_HAL_BOARD != HAL_BOARD_SITL
 extern "C" {
     int clock_gettime(clockid_t clockid, struct timespec *ts);

libraries/AP_DDS/AP_DDS_Client.h

@@ -25,6 +25,9 @@
 #if AP_DDS_IMU_PUB_ENABLED
 #include "sensor_msgs/msg/Imu.h"
 #endif // AP_DDS_IMU_PUB_ENABLED
+#if AP_DDS_STATUS_PUB_ENABLED
+#include "ardupilot_msgs/msg/Status.h"
+#endif // AP_DDS_STATUS_PUB_ENABLED
 #if AP_DDS_JOY_SUB_ENABLED
 #include "sensor_msgs/msg/Joy.h"
 #endif // AP_DDS_JOY_SUB_ENABLED
@@ -175,6 +178,17 @@ class AP_DDS_Client
     static void update_topic(rosgraph_msgs_msg_Clock& msg);
 #endif // AP_DDS_CLOCK_PUB_ENABLED
 
+#if AP_DDS_STATUS_PUB_ENABLED
+    ardupilot_msgs_msg_Status status_topic;
+    bool update_topic(ardupilot_msgs_msg_Status& msg);
+    // The last ms timestamp AP_DDS wrote/checked a status message
+    uint64_t last_status_check_time_ms;
+    // last status values;
+    ardupilot_msgs_msg_Status last_status_msg_;
+    //! @brief Serialize the current status and publish to the IO stream(s)
+    void write_status_topic();
+#endif // AP_DDS_STATUS_PUB_ENABLED
+
 #if AP_DDS_STATIC_TF_PUB_ENABLED
     // outgoing transforms
     tf2_msgs_msg_TFMessage tx_static_transforms_topic;
@@ -255,6 +269,7 @@ class AP_DDS_Client
     // client key we present
     static constexpr uint32_t key = 0xAAAABBBB;
 
+
 public:
     ~AP_DDS_Client();
 

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -48,6 +48,9 @@ enum class TopicIndex: uint8_t {
 #if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
     GPS_GLOBAL_ORIGIN_PUB,
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
+#if AP_DDS_STATUS_PUB_ENABLED
+    STATUS_PUB,
+#endif // AP_DDS_STATUS_PUB_ENABLED
 #if AP_DDS_JOY_SUB_ENABLED
     JOY_SUB,
 #endif // AP_DDS_JOY_SUB_ENABLED
@@ -268,6 +271,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         },
     },
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
+#if AP_DDS_STATUS_PUB_ENABLED
+    {
+        .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,
+        },
+    },
+#endif // AP_DDS_STATUS_PUB_ENABLED
 #if AP_DDS_JOY_SUB_ENABLED
     {
         .topic_id = to_underlying(TopicIndex::JOY_SUB),

libraries/AP_DDS/AP_DDS_config.h

@@ -66,6 +66,10 @@
 #define AP_DDS_CLOCK_PUB_ENABLED 1
 #endif
 
+#ifndef AP_DDS_STATUS_PUB_ENABLED
+#define AP_DDS_STATUS_PUB_ENABLED 1
+#endif
+
 #ifndef AP_DDS_JOY_SUB_ENABLED
 #define AP_DDS_JOY_SUB_ENABLED 1
 #endif

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 = 21;
+      const uint8 FS_BATTERY = 22;
+      const uint8 FS_GCS = 23;
+      const uint8 FS_EKF = 24;
+    };
+    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<uint8> failsafe;
+    };
+  };
+};