2023-04-05 10:23:19 -03:00
|
|
|
#include "builtin_interfaces/msg/Time.h"
|
|
|
|
#include "sensor_msgs/msg/NavSatFix.h"
|
|
|
|
#include "tf2_msgs/msg/TFMessage.h"
|
2023-03-30 13:05:17 -03:00
|
|
|
#include "sensor_msgs/msg/BatteryState.h"
|
2023-04-18 14:23:03 -03:00
|
|
|
#include "geographic_msgs/msg/GeoPoseStamped.h"
|
2024-02-20 13:58:48 -04:00
|
|
|
#include "sensor_msgs/msg/Imu.h"
|
2023-03-12 23:18:29 -03:00
|
|
|
|
2023-03-11 14:42:42 -04:00
|
|
|
#include "uxr/client/client.h"
|
2023-03-12 23:18:29 -03:00
|
|
|
|
|
|
|
// Code generated table based on the enabled topics.
|
|
|
|
// Mavgen is using python, loops are not readable.
|
|
|
|
// Can use jinja to template (like Flask)
|
|
|
|
|
2023-06-04 01:23:53 -03:00
|
|
|
enum class TopicIndex: uint8_t {
|
|
|
|
TIME_PUB,
|
|
|
|
NAV_SAT_FIX_PUB,
|
|
|
|
STATIC_TRANSFORMS_PUB,
|
|
|
|
BATTERY_STATE_PUB,
|
2024-02-20 13:58:48 -04:00
|
|
|
IMU_PUB,
|
2023-06-04 01:23:53 -03:00
|
|
|
LOCAL_POSE_PUB,
|
|
|
|
LOCAL_VELOCITY_PUB,
|
|
|
|
GEOPOSE_PUB,
|
|
|
|
CLOCK_PUB,
|
2024-03-05 15:26:39 -04:00
|
|
|
GPS_GLOBAL_ORIGIN_PUB,
|
2023-06-04 01:23:53 -03:00
|
|
|
JOY_SUB,
|
2023-06-28 00:43:59 -03:00
|
|
|
DYNAMIC_TRANSFORMS_SUB,
|
2023-08-02 00:22:27 -03:00
|
|
|
VELOCITY_CONTROL_SUB,
|
2023-12-07 02:49:01 -04:00
|
|
|
GLOBAL_POSITION_SUB,
|
2023-06-04 01:23:53 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
static inline constexpr uint8_t to_underlying(const TopicIndex index)
|
|
|
|
{
|
|
|
|
static_assert(sizeof(index) == sizeof(uint8_t));
|
|
|
|
return static_cast<uint8_t>(index);
|
|
|
|
}
|
|
|
|
|
2023-03-12 23:18:29 -03:00
|
|
|
|
2023-06-04 01:23:53 -03:00
|
|
|
constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
|
2023-03-12 23:18:29 -03:00
|
|
|
{
|
2023-06-04 01:23:53 -03:00
|
|
|
.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},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataWriter,
|
|
|
|
.topic_name = "rt/ap/time",
|
|
|
|
.type_name = "builtin_interfaces::msg::dds_::Time_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
|
|
|
.reliability = UXR_RELIABILITY_RELIABLE,
|
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 20,
|
|
|
|
},
|
2023-03-12 23:18:29 -03:00
|
|
|
},
|
2023-03-11 14:42:42 -04:00
|
|
|
{
|
2023-06-04 01:23:53 -03:00
|
|
|
.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},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataWriter,
|
|
|
|
.topic_name = "rt/ap/navsat/navsat0",
|
2024-05-25 13:18:22 -03:00
|
|
|
.type_name = "sensor_msgs::msg::dds_::NavSatFix_",
|
2024-05-23 10:34:27 -03:00
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2023-03-11 14:42:42 -04:00
|
|
|
},
|
2023-04-05 03:16:36 -03:00
|
|
|
{
|
2023-06-04 01:23:53 -03:00
|
|
|
.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},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataWriter,
|
|
|
|
.topic_name = "rt/ap/tf_static",
|
|
|
|
.type_name = "tf2_msgs::msg::dds_::TFMessage_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_TRANSIENT_LOCAL,
|
|
|
|
.reliability = UXR_RELIABILITY_RELIABLE,
|
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 1,
|
|
|
|
},
|
2023-04-05 03:16:36 -03:00
|
|
|
},
|
2023-03-30 13:05:17 -03:00
|
|
|
{
|
2023-06-04 01:23:53 -03:00
|
|
|
.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},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataWriter,
|
|
|
|
.topic_name = "rt/ap/battery/battery0",
|
|
|
|
.type_name = "sensor_msgs::msg::dds_::BatteryState_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2023-03-30 13:05:17 -03:00
|
|
|
},
|
2024-02-20 13:58:48 -04:00
|
|
|
{
|
|
|
|
.topic_id = to_underlying(TopicIndex::IMU_PUB),
|
|
|
|
.pub_id = to_underlying(TopicIndex::IMU_PUB),
|
|
|
|
.sub_id = to_underlying(TopicIndex::IMU_PUB),
|
|
|
|
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAWRITER_ID},
|
|
|
|
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::IMU_PUB), .type=UXR_DATAREADER_ID},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataWriter,
|
|
|
|
.topic_name = "rt/ap/imu/experimental/data",
|
|
|
|
.type_name = "sensor_msgs::msg::dds_::Imu_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2024-02-20 13:58:48 -04:00
|
|
|
},
|
2023-04-13 22:51:35 -03:00
|
|
|
{
|
2023-06-04 01:23:53 -03:00
|
|
|
.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},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataWriter,
|
|
|
|
.topic_name = "rt/ap/pose/filtered",
|
|
|
|
.type_name = "geometry_msgs::msg::dds_::PoseStamped_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2023-04-13 22:51:35 -03:00
|
|
|
},
|
2023-04-18 17:52:39 -03:00
|
|
|
{
|
2023-06-04 01:23:53 -03:00
|
|
|
.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},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataWriter,
|
|
|
|
.topic_name = "rt/ap/twist/filtered",
|
|
|
|
.type_name = "geometry_msgs::msg::dds_::TwistStamped_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2023-04-18 14:23:03 -03:00
|
|
|
},
|
|
|
|
{
|
2023-06-04 01:23:53 -03:00
|
|
|
.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},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataWriter,
|
|
|
|
.topic_name = "rt/ap/geopose/filtered",
|
|
|
|
.type_name = "geographic_msgs::msg::dds_::GeoPoseStamped_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2023-04-18 14:23:03 -03:00
|
|
|
},
|
2023-05-10 06:35:29 -03:00
|
|
|
{
|
2023-06-04 01:23:53 -03:00
|
|
|
.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},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataWriter,
|
|
|
|
.topic_name = "rt/ap/clock",
|
|
|
|
.type_name = "rosgraph_msgs::msg::dds_::Clock_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
|
|
|
.reliability = UXR_RELIABILITY_RELIABLE,
|
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 20,
|
|
|
|
},
|
2023-05-10 06:35:29 -03:00
|
|
|
},
|
2024-03-05 15:26:39 -04:00
|
|
|
{
|
|
|
|
.topic_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB),
|
|
|
|
.pub_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB),
|
|
|
|
.sub_id = to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB),
|
|
|
|
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB), .type=UXR_DATAWRITER_ID},
|
|
|
|
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB), .type=UXR_DATAREADER_ID},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataWriter,
|
|
|
|
.topic_name = "rt/ap/gps_global_origin/filtered",
|
|
|
|
.type_name = "geographic_msgs::msg::dds_::GeoPointStamped_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2024-03-05 15:26:39 -04:00
|
|
|
},
|
2023-05-21 20:50:51 -03:00
|
|
|
{
|
2023-06-04 01:23:53 -03:00
|
|
|
.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},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataReader,
|
|
|
|
.topic_name = "rt/ap/joy",
|
|
|
|
.type_name = "sensor_msgs::msg::dds_::Joy_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
2024-05-25 13:24:06 -03:00
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
2024-05-23 10:34:27 -03:00
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2023-05-21 20:50:51 -03:00
|
|
|
},
|
2023-06-28 00:43:59 -03:00
|
|
|
{
|
|
|
|
.topic_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
|
|
|
|
.pub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
|
|
|
|
.sub_id = to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB),
|
|
|
|
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), .type=UXR_DATAWRITER_ID},
|
|
|
|
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB), .type=UXR_DATAREADER_ID},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataReader,
|
|
|
|
.topic_name = "rt/ap/tf",
|
|
|
|
.type_name = "tf2_msgs::msg::dds_::TFMessage_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
2024-05-25 13:24:06 -03:00
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
2024-05-23 10:34:27 -03:00
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2023-06-28 00:43:59 -03:00
|
|
|
},
|
2023-08-02 00:22:27 -03:00
|
|
|
{
|
|
|
|
.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},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataReader,
|
|
|
|
.topic_name = "rt/ap/cmd_vel",
|
|
|
|
.type_name = "geometry_msgs::msg::dds_::TwistStamped_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
2024-05-25 13:24:06 -03:00
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
2024-05-23 10:34:27 -03:00
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2023-08-02 00:22:27 -03:00
|
|
|
},
|
2023-12-07 02:49:01 -04:00
|
|
|
{
|
|
|
|
.topic_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
|
|
|
|
.pub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
|
|
|
|
.sub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB),
|
|
|
|
.dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAWRITER_ID},
|
|
|
|
.dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAREADER_ID},
|
2024-05-23 10:34:27 -03:00
|
|
|
.topic_rw = Topic_rw::DataReader,
|
|
|
|
.topic_name = "rt/ap/cmd_gps_pose",
|
|
|
|
.type_name = "ardupilot_msgs::msg::dds_::GlobalPosition_",
|
|
|
|
.qos = {
|
|
|
|
.durability = UXR_DURABILITY_VOLATILE,
|
2024-05-25 13:24:06 -03:00
|
|
|
.reliability = UXR_RELIABILITY_BEST_EFFORT,
|
2024-05-23 10:34:27 -03:00
|
|
|
.history = UXR_HISTORY_KEEP_LAST,
|
|
|
|
.depth = 5,
|
|
|
|
},
|
2023-12-07 02:49:01 -04:00
|
|
|
},
|
2023-03-12 23:18:29 -03:00
|
|
|
};
|