diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 22a48f21e7..c5d31cc244 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -410,21 +410,27 @@ void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, ui 1) Store the read contents into the ucdr buffer 2) Deserialize the said contents into the topic instance */ - sensor_msgs_msg_Joy* topic = nullptr; - const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, topic); - if (success == false || topic == nullptr) { - return; + switch (object_id.id) { + case topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id: + sensor_msgs_msg_Joy topic; + const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, &topic); + + if (success == false) { + break; + } + + uint32_t* count_ptr = (uint32_t*) args; + (*count_ptr)++; + if (topic.axes_size >= 4) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: %f, %f, %f, %f", + topic.axes[0], topic.axes[1], topic.axes[2], topic.axes[3]); + } else { + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: Insufficient axes size "); + } + break; } - uint32_t* count_ptr = (uint32_t*) args; - (*count_ptr)++; - if (topic->axes_size >= 4) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Receivied sensor_msgs/Joy: %f, %f, %f, %f", - topic->axes[0], topic->axes[1], topic->axes[2], topic->axes[3]); - } else { - GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Receivied sensor_msgs/Joy: Insufficient axes size "); - } } /* diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 493744f438..e987e20732 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -5,7 +5,6 @@ #include "uxr/client/client.h" #include "ucdr/microcdr.h" #include "builtin_interfaces/msg/Time.h" -#include "AP_DDS_Generic_Fn_T.h" #include "sensor_msgs/msg/NavSatFix.h" #include "tf2_msgs/msg/TFMessage.h" @@ -185,9 +184,6 @@ public: const char* topic_profile_label; const char* dw_profile_label; const char* dr_profile_label; - Generic_serialize_topic_fn_t serialize; - Generic_deserialize_topic_fn_t deserialize; - Generic_size_of_topic_fn_t size_of; }; static const struct Topic_table topics[]; diff --git a/libraries/AP_DDS/AP_DDS_Generic_Fn_T.h b/libraries/AP_DDS/AP_DDS_Generic_Fn_T.h deleted file mode 100644 index b6a66f6ee4..0000000000 --- a/libraries/AP_DDS/AP_DDS_Generic_Fn_T.h +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once - -// table maps between string names and pointer to element -// Function pointer that matches signature of generated topics -typedef bool (*Generic_serialize_topic_fn_t)(struct ucdrBuffer*, const void*); -typedef bool (*Generic_deserialize_topic_fn_t)(struct ucdrBuffer*, void*); -typedef uint32_t (*Generic_size_of_topic_fn_t)(struct ucdrBuffer*, uint32_t); diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 930bfa019b..376bf9dfc1 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -4,130 +4,120 @@ #include "sensor_msgs/msg/BatteryState.h" #include "geographic_msgs/msg/GeoPoseStamped.h" -#include "AP_DDS_Generic_Fn_T.h" #include "uxr/client/client.h" // Code generated table based on the enabled topics. // Mavgen is using python, loops are not readable. // Can use jinja to template (like Flask) +enum class TopicIndex: uint8_t { + TIME_PUB, + NAV_SAT_FIX_PUB, + STATIC_TRANSFORMS_PUB, + BATTERY_STATE_PUB, + LOCAL_POSE_PUB, + LOCAL_VELOCITY_PUB, + GEOPOSE_PUB, + CLOCK_PUB, + JOY_SUB, +}; -const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { +static inline constexpr uint8_t to_underlying(const TopicIndex index) +{ + static_assert(sizeof(index) == sizeof(uint8_t)); + return static_cast(index); +} + + +constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { { - .topic_id = 0x01, - .pub_id = 0x01, - .sub_id = 0x01, - .dw_id = uxrObjectId{.id=0x01, .type=UXR_DATAWRITER_ID}, - .dr_id = uxrObjectId{.id=0x01, .type=UXR_DATAREADER_ID}, + .topic_id = to_underlying(TopicIndex::TIME_PUB), + .pub_id = to_underlying(TopicIndex::TIME_PUB), + .sub_id = to_underlying(TopicIndex::TIME_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::TIME_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::TIME_PUB), .type=UXR_DATAREADER_ID}, .topic_profile_label = "time__t", .dw_profile_label = "time__dw", .dr_profile_label = "", - .serialize = Generic_serialize_topic_fn_t(&builtin_interfaces_msg_Time_serialize_topic), - .deserialize = Generic_deserialize_topic_fn_t(&builtin_interfaces_msg_Time_deserialize_topic), - .size_of = Generic_size_of_topic_fn_t(&builtin_interfaces_msg_Time_size_of_topic), }, { - .topic_id = 0x02, - .pub_id = 0x02, - .sub_id = 0x02, - .dw_id = uxrObjectId{.id=0x02, .type=UXR_DATAWRITER_ID}, - .dr_id = uxrObjectId{.id=0x02, .type=UXR_DATAREADER_ID}, + .topic_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB), + .pub_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB), + .sub_id = to_underlying(TopicIndex::NAV_SAT_FIX_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::NAV_SAT_FIX_PUB), .type=UXR_DATAREADER_ID}, .topic_profile_label = "navsatfix0__t", .dw_profile_label = "navsatfix0__dw", .dr_profile_label = "", - .serialize = Generic_serialize_topic_fn_t(&sensor_msgs_msg_NavSatFix_serialize_topic), - .deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_NavSatFix_deserialize_topic), - .size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_NavSatFix_size_of_topic), }, { - .topic_id = 0x03, - .pub_id = 0x03, - .sub_id = 0x03, - .dw_id = uxrObjectId{.id=0x03, .type=UXR_DATAWRITER_ID}, - .dr_id = uxrObjectId{.id=0x03, .type=UXR_DATAREADER_ID}, + .topic_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), + .pub_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), + .sub_id = to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB), .type=UXR_DATAREADER_ID}, .topic_profile_label = "statictransforms__t", .dw_profile_label = "statictransforms__dw", .dr_profile_label = "", - .serialize = Generic_serialize_topic_fn_t(&tf2_msgs_msg_TFMessage_serialize_topic), - .deserialize = Generic_deserialize_topic_fn_t(&tf2_msgs_msg_TFMessage_deserialize_topic), - .size_of = Generic_size_of_topic_fn_t(&tf2_msgs_msg_TFMessage_size_of_topic), }, { - .topic_id = 0x04, - .pub_id = 0x04, - .sub_id = 0x04, - .dw_id = uxrObjectId{.id=0x04, .type=UXR_DATAWRITER_ID}, - .dr_id = uxrObjectId{.id=0x04, .type=UXR_DATAREADER_ID}, + .topic_id = to_underlying(TopicIndex::BATTERY_STATE_PUB), + .pub_id = to_underlying(TopicIndex::BATTERY_STATE_PUB), + .sub_id = to_underlying(TopicIndex::BATTERY_STATE_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::BATTERY_STATE_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::BATTERY_STATE_PUB), .type=UXR_DATAREADER_ID}, .topic_profile_label = "batterystate0__t", .dw_profile_label = "batterystate0__dw", .dr_profile_label = "", - .serialize = Generic_serialize_topic_fn_t(&sensor_msgs_msg_BatteryState_serialize_topic), - .deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_BatteryState_deserialize_topic), - .size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_BatteryState_size_of_topic), }, { - .topic_id = 0x05, - .pub_id = 0x05, - .sub_id = 0x05, - .dw_id = uxrObjectId{.id=0x05, .type=UXR_DATAWRITER_ID}, - .dr_id = uxrObjectId{.id=0x05, .type=UXR_DATAREADER_ID}, + .topic_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), + .pub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), + .sub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_POSE_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_POSE_PUB), .type=UXR_DATAREADER_ID}, .topic_profile_label = "localpose__t", .dw_profile_label = "localpose__dw", .dr_profile_label = "", - .serialize = Generic_serialize_topic_fn_t(&geometry_msgs_msg_PoseStamped_serialize_topic), - .deserialize = Generic_deserialize_topic_fn_t(&geometry_msgs_msg_PoseStamped_deserialize_topic), - .size_of = Generic_size_of_topic_fn_t(&geometry_msgs_msg_PoseStamped_size_of_topic), }, { - .topic_id = 0x06, - .pub_id = 0x06, - .sub_id = 0x06, - .dw_id = uxrObjectId{.id=0x06, .type=UXR_DATAWRITER_ID}, - .dr_id = uxrObjectId{.id=0x06, .type=UXR_DATAREADER_ID}, + .topic_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), + .pub_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), + .sub_id = to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::LOCAL_VELOCITY_PUB), .type=UXR_DATAREADER_ID}, .topic_profile_label = "localvelocity__t", .dw_profile_label = "localvelocity__dw", .dr_profile_label = "", - .serialize = Generic_serialize_topic_fn_t(&geometry_msgs_msg_TwistStamped_serialize_topic), - .deserialize = Generic_deserialize_topic_fn_t(&geometry_msgs_msg_TwistStamped_deserialize_topic), - .size_of = Generic_size_of_topic_fn_t(&geometry_msgs_msg_TwistStamped_size_of_topic), }, { - .topic_id = 0x07, - .pub_id = 0x07, - .sub_id = 0x07, - .dw_id = uxrObjectId{.id=0x07, .type=UXR_DATAWRITER_ID}, - .dr_id = uxrObjectId{.id=0x07, .type=UXR_DATAREADER_ID}, + .topic_id = to_underlying(TopicIndex::GEOPOSE_PUB), + .pub_id = to_underlying(TopicIndex::GEOPOSE_PUB), + .sub_id = to_underlying(TopicIndex::GEOPOSE_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GEOPOSE_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GEOPOSE_PUB), .type=UXR_DATAREADER_ID}, .topic_profile_label = "geopose__t", .dw_profile_label = "geopose__dw", .dr_profile_label = "", - .serialize = Generic_serialize_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_serialize_topic), - .deserialize = Generic_deserialize_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_deserialize_topic), - .size_of = Generic_size_of_topic_fn_t(&geographic_msgs_msg_GeoPoseStamped_size_of_topic), }, { - .topic_id = 0x08, - .pub_id = 0x08, - .sub_id = 0x08, - .dw_id = uxrObjectId{.id=0x08, .type=UXR_DATAWRITER_ID}, - .dr_id = uxrObjectId{.id=0x08, .type=UXR_DATAREADER_ID}, + .topic_id = to_underlying(TopicIndex::CLOCK_PUB), + .pub_id = to_underlying(TopicIndex::CLOCK_PUB), + .sub_id = to_underlying(TopicIndex::CLOCK_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::CLOCK_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::CLOCK_PUB), .type=UXR_DATAREADER_ID}, .topic_profile_label = "clock__t", .dw_profile_label = "clock__dw", .dr_profile_label = "", - .serialize = Generic_serialize_topic_fn_t(&rosgraph_msgs_msg_Clock_serialize_topic), - .deserialize = Generic_deserialize_topic_fn_t(&rosgraph_msgs_msg_Clock_deserialize_topic), - .size_of = Generic_size_of_topic_fn_t(&rosgraph_msgs_msg_Clock_size_of_topic), }, { - .topic_id = 0x09, - .pub_id = 0x09, - .sub_id = 0x09, - .dw_id = uxrObjectId{.id=0x09, .type=UXR_DATAWRITER_ID}, - .dr_id = uxrObjectId{.id=0x09, .type=UXR_DATAREADER_ID}, + .topic_id = to_underlying(TopicIndex::JOY_SUB), + .pub_id = to_underlying(TopicIndex::JOY_SUB), + .sub_id = to_underlying(TopicIndex::JOY_SUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::JOY_SUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::JOY_SUB), .type=UXR_DATAREADER_ID}, .topic_profile_label = "joy__t", .dw_profile_label = "", .dr_profile_label = "joy__dr", - .serialize = Generic_serialize_topic_fn_t(&sensor_msgs_msg_Joy_serialize_topic), - .deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_Joy_deserialize_topic), - .size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_Joy_size_of_topic), }, };