diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index d066a2d32b2e43..f8af788e8b81ec 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 00000000000000..2b569f0dd77709 --- /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 = 21 +uint8 FS_BATTERY = 22 +uint8 FS_GCS = 23 +uint8 FS_EKF = 24 +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 9ba608c20290d6..96a73140712ec7 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" @@ -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(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); diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 005b87f30aa213..f60bc38475a30d 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/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(); diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 92d546dd51290f..6bf7d58bf174d7 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/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), diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 2e0d3d6264f8c5..22ab9f2315324a 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/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 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 00000000000000..555aed78bc7bb1 --- /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 = 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 failsafe; + }; + }; +};