AP_DDS: Add velocity control DDS subscriber

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -27,6 +27,7 @@ static char BASE_LINK_FRAME_ID[] = "base_link";
 // the AP_DDS_Client::on_topic frame size is exceeded.
 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 {};
 
 const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
 
@@ -430,6 +431,7 @@ void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, ui
         if (rx_joy_topic.axes_size >= 4) {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: %f, %f, %f, %f",
                           rx_joy_topic.axes[0], rx_joy_topic.axes[1], rx_joy_topic.axes[2], rx_joy_topic.axes[3]);
+            // TODO implement joystick RC control to AP
         } else {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: Insufficient axes size ");
         }
@@ -445,9 +447,22 @@ void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, ui
         if (rx_dynamic_transforms_topic.transforms_size > 0) {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage of length: %u",
                           static_cast<unsigned>(rx_dynamic_transforms_topic.transforms_size));
+            // TODO implement external odometry to AP
         } else {
             GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage: Insufficient size ");
         }
+        break;
+    }
+    case topics[to_underlying(TopicIndex::VELOCITY_CONTROL_SUB)].dr_id.id: {
+        const bool success = geometry_msgs_msg_TwistStamped_deserialize_topic(ub, &rx_velocity_control_topic);
+        if (success == false) {
+            break;
+        }
+        uint32_t* count_ptr = (uint32_t*) args;
+        (*count_ptr)++;
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received geometry_msgs/TwistStamped");
+        // TODO implement the velocity control to AP
+        break;
     }
     }
 
@@ -695,14 +710,14 @@ void AP_DDS_Client::write_local_pose_topic()
     }
 }
 
-void AP_DDS_Client::write_local_velocity_topic()
+void AP_DDS_Client::write_tx_local_velocity_topic()
 {
     WITH_SEMAPHORE(csem);
     if (connected) {
         ucdrBuffer ub {};
-        const uint32_t topic_size = geometry_msgs_msg_TwistStamped_size_of_topic(&local_velocity_topic, 0);
+        const uint32_t topic_size = geometry_msgs_msg_TwistStamped_size_of_topic(&tx_local_velocity_topic, 0);
         uxr_prepare_output_stream(&session,reliable_out,topics[5].dw_id,&ub,topic_size);
-        const bool success = geometry_msgs_msg_TwistStamped_serialize_topic(&ub, &local_velocity_topic);
+        const bool success = geometry_msgs_msg_TwistStamped_serialize_topic(&ub, &tx_local_velocity_topic);
         if (!success) {
             // TODO sometimes serialization fails on bootup. Determine why.
             // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
@@ -770,9 +785,9 @@ void AP_DDS_Client::update()
     }
 
     if (cur_time_ms - last_local_velocity_time_ms > DELAY_LOCAL_VELOCITY_TOPIC_MS) {
-        update_topic(local_velocity_topic);
+        update_topic(tx_local_velocity_topic);
         last_local_velocity_time_ms = cur_time_ms;
-        write_local_velocity_topic();
+        write_tx_local_velocity_topic();
     }
 
     if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) {

libraries/AP_DDS/AP_DDS_Client.h

@@ -58,12 +58,14 @@ private:
     builtin_interfaces_msg_Time time_topic;
     geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
     geometry_msgs_msg_PoseStamped local_pose_topic;
-    geometry_msgs_msg_TwistStamped local_velocity_topic;
+    geometry_msgs_msg_TwistStamped tx_local_velocity_topic;
     sensor_msgs_msg_BatteryState battery_state_topic;
     sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
     rosgraph_msgs_msg_Clock clock_topic;
     // incoming joystick data
     static sensor_msgs_msg_Joy rx_joy_topic;
+    // incoming REP147 velocity control
+    static geometry_msgs_msg_TwistStamped rx_velocity_control_topic;
     // outgoing transforms
     tf2_msgs_msg_TFMessage tx_static_transforms_topic;
     tf2_msgs_msg_TFMessage tx_dynamic_transforms_topic;
@@ -169,7 +171,7 @@ public:
     //! @brief Serialize the current local_pose and publish to the IO stream(s)
     void write_local_pose_topic();
     //! @brief Serialize the current local velocity and publish to the IO stream(s)
-    void write_local_velocity_topic();
+    void write_tx_local_velocity_topic();
     //! @brief Serialize the current geo_pose and publish to the IO stream(s)
     void write_geo_pose_topic();
     //! @brief Serialize the current clock and publish to the IO stream(s)

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -21,6 +21,7 @@ enum class TopicIndex: uint8_t {
     CLOCK_PUB,
     JOY_SUB,
     DYNAMIC_TRANSFORMS_SUB,
+    VELOCITY_CONTROL_SUB,
 };
 
 static inline constexpr uint8_t to_underlying(const TopicIndex index)
@@ -131,4 +132,14 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         .dw_profile_label = "",
         .dr_profile_label = "dynamictf__dr",
     },
+    {
+        .topic_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB),
+        .pub_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB),
+        .sub_id = to_underlying(TopicIndex::VELOCITY_CONTROL_SUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::VELOCITY_CONTROL_SUB), .type=UXR_DATAREADER_ID},
+        .topic_profile_label = "velocitycontrol__t",
+        .dw_profile_label = "",
+        .dr_profile_label = "velocitycontrol__dr",
+    },
 };

libraries/AP_DDS/README.md

@@ -214,6 +214,7 @@ Next, follow the associated section for your chosen transport, and finally you c
   Subscribed topics:
   * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber
   * /ap/tf [tf2_msgs/msg/TFMessage] 1 subscriber
+  * /ap/velocity_control [geometry_msgs/msg/TwistStamped] 1 subscriber
 
   $ ros2 topic hz /ap/time
   average rate: 50.115

libraries/AP_DDS/dds_xrce_profile.xml

@@ -253,4 +253,19 @@
       <dataType>tf2_msgs::msg::dds_::TFMessage_</dataType>
     </topic>
   </data_reader>
+  <topic profile_name="velocitycontrol__t">
+    <name>rt/ap/velocity_control</name>
+    <dataType>geometry_msgs::msg::dds_::TwistStamped_</dataType>
+    <historyQos>
+      <kind>KEEP_LAST</kind>
+      <depth>5</depth>
+    </historyQos>
+  </topic>
+  <data_reader profile_name="velocitycontrol__dr">
+    <topic>
+      <kind>NO_KEY</kind>
+      <name>rt/ap/velocity_control</name>
+      <dataType>geometry_msgs::msg::dds_::TwistStamped_</dataType>
+    </topic>
+  </data_reader>
 </profiles>