AP_DDS: Status Message

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 = 1
+uint8 FS_BATTERY = 2
+uint8 FS_GCS = 3
+uint8 FS_EKF = 4
+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"
@@ -42,6 +43,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 +527,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<uint8_t>(AP::fwversion().vehicle_type);
+ 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 +1180,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 +1255,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);
 }
 

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)

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,
@@ -202,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),

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<uint8> failsafe;
+ };
+ };
+};