custom msg for angular goals

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/AngularVelandAccn.msg"
   "srv/ArmMotors.srv"
   "srv/ModeSwitch.srv"
   DEPENDENCIES geometry_msgs std_msgs

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -15,6 +15,7 @@
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
 
+#include "ardupilot_msgs/msg/AngularVelandAccn.h"
 #include "ardupilot_msgs/srv/ArmMotors.h"
 #include "ardupilot_msgs/srv/ModeSwitch.h"
 
@@ -46,6 +47,7 @@ static constexpr uint16_t DELAY_PING_MS = 500;
 sensor_msgs_msg_Joy AP_DDS_Client::rx_joy_topic {};
 tf2_msgs_msg_TFMessage AP_DDS_Client::rx_dynamic_transforms_topic {};
 geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {};
+custom_msgs_msg_AngularVelandAccn AP_DDS_Client::rx_angular_control_topic {};
 ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {};
 
 
@@ -564,6 +566,21 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
 #endif // AP_EXTERNAL_CONTROL_ENABLED
         break;
     }
+
+    case topics[to_underlying(TopicIndex::ANGULAR_CONTROL_SUB)].dr_id.id: {
+        const bool success = custom_msgs_msg_AngularVelandAccn_deserialize_topic(ub, &rx_angular_control_topic);
+        if (success == false) {
+            break;
+        }
+
+#if AP_EXTERNAL_CONTROL_ENABLED
+        if (!AP_DDS_External_Control::handle_angular_control(rx_angular_control_topic)) {
+            // TODO #23430 handle velocity control failure through rosout, throttled.
+        }
+#endif // AP_EXTERNAL_CONTROL_ENABLED
+        break;
+    }
+
     case topics[to_underlying(TopicIndex::GLOBAL_POSITION_SUB)].dr_id.id: {
         const bool success = ardupilot_msgs_msg_GlobalPosition_deserialize_topic(ub, &rx_global_position_control_topic);
         if (success == false) {

libraries/AP_DDS/AP_DDS_Client.h

@@ -17,6 +17,7 @@
 #include "sensor_msgs/msg/Joy.h"
 #include "geometry_msgs/msg/PoseStamped.h"
 #include "geometry_msgs/msg/TwistStamped.h"
+#include "ardupilot_msgs/msg/AngularVelandAccn.h"
 #include "geographic_msgs/msg/GeoPointStamped.h"
 #include "geographic_msgs/msg/GeoPoseStamped.h"
 #include "rosgraph_msgs/msg/Clock.h"
@@ -72,6 +73,8 @@ class AP_DDS_Client
     static sensor_msgs_msg_Joy rx_joy_topic;
     // incoming REP147 velocity control
     static geometry_msgs_msg_TwistStamped rx_velocity_control_topic;
+    // incoming orientation, angular velocity, angular acceleration and thrust goals
+    static custom_msgs_msg_AngularVelandAccn rx_angular_control_topic;
     // incoming REP147 goal interface global position
     static ardupilot_msgs_msg_GlobalPosition rx_global_position_control_topic;
     // outgoing transforms

libraries/AP_DDS/AP_DDS_ExternalControl.cpp

@@ -85,6 +85,24 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
     return false;
 }
 
+bool AP_DDS_External_Control::handle_angular_control(custom_msgs_msg_AngularVelandAccn& cmd_angular_goals)
+{
+    auto *external_control = AP::externalcontrol();
+    if (external_control == nullptr) {
+        return false;
+    }
+
+    // Convert commands from body frame (x-forward, y-left, z-up) to NED.
+    //Quaternion orientation;
+    //Vector3f angular_velocity;
+    //Vector3f angular_acceleration;
+    //double thrust;
+
+    GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "angular control");
+
+    return true;
+}
+
 bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out)
 {
 

libraries/AP_DDS/AP_DDS_ExternalControl.h

@@ -3,6 +3,7 @@
 #if AP_DDS_ENABLED
 #include "ardupilot_msgs/msg/GlobalPosition.h"
 #include "geometry_msgs/msg/TwistStamped.h"
+#include "ardupilot_msgs/msg/AngularVelandAccn.h"
 
 #include <AP_Common/Location.h>
 
@@ -13,6 +14,7 @@ class AP_DDS_External_Control
     // https://ros.org/reps/rep-0147.html#goal-interface
     static bool handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos);
     static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel);
+    static bool handle_angular_control(custom_msgs_msg_AngularVelandAccn& cmd_angular_goals);
 private:
     static bool convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out);
 };

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -25,6 +25,7 @@ enum class TopicIndex: uint8_t {
     JOY_SUB,
     DYNAMIC_TRANSFORMS_SUB,
     VELOCITY_CONTROL_SUB,
+    ANGULAR_CONTROL_SUB,
     GLOBAL_POSITION_SUB,
 };
 
@@ -166,6 +167,16 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .dw_profile_label = "",
         .dr_profile_label = "velocitycontrol__dr",
     },
+    {
+        .topic_id = to_underlying(TopicIndex::ANGULAR_CONTROL_SUB),
+        .pub_id = to_underlying(TopicIndex::ANGULAR_CONTROL_SUB),
+        .sub_id = to_underlying(TopicIndex::ANGULAR_CONTROL_SUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::ANGULAR_CONTROL_SUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::ANGULAR_CONTROL_SUB), .type=UXR_DATAREADER_ID},
+        .topic_profile_label = "angularcontrol__t",
+        .dw_profile_label = "",
+        .dr_profile_label = "angularcontrol__dr",
+    },
     {
         .topic_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
         .pub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),

libraries/AP_DDS/Idl/ardupilot_msgs/msg/AngularVelandAccn.idl

@@ -0,0 +1,34 @@
+// This is an example and does not exist in the default ROS 2 message sets.
+
+#include "geometry_msgs/msg/Vector3.idl"
+#include "geometry_msgs/msg/Quaternion.idl"
+#include "std_msgs/msg/Header.idl"
+
+module custom_msgs {
+  module msg {
+    @verbatim (language="comment", text=
+      "This message contains angular rate, angular acceleration, orientation as a quaternion, and thrust, each with a reference frame and timestamp. \
+       The angular rate and angular acceleration are represented in free space, anchored at the origin, and only the rotational component applies when transformed. \
+       The orientation is represented as a quaternion. \
+       The thrust is represented as a single floating-point number.")
+    struct AngularVelandAccn {
+      std_msgs::msg::Header header;
+
+      @verbatim (language="comment", text="Angular rate represented by a Vector3. \
+                                           It describes the angular velocity around each of the three principal axes.")
+      geometry_msgs::msg::Vector3 angular_rate;
+
+      @verbatim (language="comment", text="Angular acceleration represented by a Vector3. \
+                                           It describes the rate of change of angular velocity around each of the three principal axes.")
+      geometry_msgs::msg::Vector3 angular_acceleration;
+
+      @verbatim (language="comment", text="Orientation represented by a quaternion. \
+                                           It describes the orientation in space as a rotation from a reference orientation.")
+      geometry_msgs::msg::Quaternion orientation;
+
+      @verbatim (language="comment", text="Thrust represented as a single floating-point number. \
+                                           It describes the magnitude of the thrust force.")
+      double thrust;
+    };
+  };
+};

libraries/AP_DDS/dds_xrce_profile.xml

@@ -319,6 +319,21 @@
       <dataType>geometry_msgs::msg::dds_::TwistStamped_</dataType>
     </topic>
   </data_reader>
+  <topic profile_name="angularcontrol__t">
+    <name>rt/ap/cmd_ang_goals</name>
+    <dataType>ardupilot_msgs::msg::dds_::AngularVelandAccn_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>5</depth>
+    </historyQos>
+  </topic>
+  <data_reader profile_name="angularcontrol__dr">
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ap/cmd_ang_goals</name>
+      <dataType>ardupilot_msgs::msg::dds_::AngularVelandAccn_</dataType>
+    </topic>
+  </data_reader>
   <replier profile_name="arm_motors__replier" service_name="rs/ap/arm_motorsService" request_type="ardupilot_msgs::srv::dds_::ArmMotors_Request_" reply_type="ardupilot_msgs::srv::dds_::ArmMotors_Response_">
     <request_topic_name>rq/ap/arm_motorsRequest</request_topic_name>
     <reply_topic_name>rr/ap/arm_motorsReply</reply_topic_name>