aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_DDS: Add Subscriber support with Joy

Browse Source 
* Register subscriber and deserialize sensor_msgs/Joy

Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com>
Co-authored-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
arshPratap 2023-05-22 05:20:51 +05:30 committed by Peter Barker
parent 02802c88ad
commit 5168f39463
5 changed files with 187 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -401,6 +401,32 @@ bool AP_DDS_Client::start(void)
return true;
}
// read function triggered at every subscription callback
void AP_DDS_Client::on_topic (uxrSession* uxr_session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length, void* args)
{
(void) uxr_session; (void) object_id; (void) request_id; (void) stream_id; (void) length;
/*
TEMPLATE for reading to the subscribed topics
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;
}
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 ");
}
}
/*
main loop for DDS thread
*/
@ -438,6 +464,10 @@ bool AP_DDS_Client::init()
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Transport Initialization failed");
return false;
}
// Register topic callbacks
uxr_set_topic_callback(&session, AP_DDS_Client::on_topic, &count);
while (!uxr_create_session(&session)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Initialization waiting...");
hal.scheduler->delay(1000);
@ -479,8 +509,8 @@ bool AP_DDS_Client::create()
constexpr uint8_t nRequestsParticipant = 1;
const uint16_t requestsParticipant[nRequestsParticipant] = {participant_req_id};
constexpr int maxTimeMsPerRequestMs = 250;
constexpr int requestTimeoutParticipantMs = nRequestsParticipant * maxTimeMsPerRequestMs;
constexpr uint8_t maxTimeMsPerRequestMs = 250;
constexpr uint16_t requestTimeoutParticipantMs = (uint16_t) nRequestsParticipant * maxTimeMsPerRequestMs;
uint8_t statusParticipant[nRequestsParticipant];
if (!uxr_run_session_until_all_status(&session, requestTimeoutParticipantMs, requestsParticipant, statusParticipant, nRequestsParticipant)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Participant session request failure");
@ -497,35 +527,71 @@ bool AP_DDS_Client::create()
const char* topic_ref = topics[i].topic_profile_label;
const auto topic_req_id = uxr_buffer_create_topic_ref(&session,reliable_out,topic_id,participant_id,topic_ref,UXR_REPLACE);
// Publisher
const uxrObjectId pub_id = {
.id = topics[i].pub_id,
.type = UXR_PUBLISHER_ID
};
const char* pub_xml = "";
const auto pub_req_id = uxr_buffer_create_publisher_xml(&session,reliable_out,pub_id,participant_id,pub_xml,UXR_REPLACE);
// Data Writer
const char* data_writer_ref = topics[i].dw_profile_label;
const auto dwriter_req_id = uxr_buffer_create_datawriter_ref(&session,reliable_out,topics[i].dw_id,pub_id,data_writer_ref,UXR_REPLACE);
// Status requests
constexpr uint8_t nRequests = 3;
const uint16_t requests[nRequests] = {topic_req_id, pub_req_id, dwriter_req_id};
constexpr int requestTimeoutMs = nRequests * maxTimeMsPerRequestMs;
uint16_t requests[nRequests];
constexpr uint16_t requestTimeoutMs = (uint8_t) nRequests * maxTimeMsPerRequestMs;
uint8_t status[nRequests];
if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Topic/Pub/Writer session request failure for index 'TODO'");
for (int s = 0 ; s < nRequests; s++) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Status '%d' result '%u'", s, status[s]);
if (strlen(topics[i].dw_profile_label) > 0) {
// Publisher
const uxrObjectId pub_id = {
.id = topics[i].pub_id,
.type = UXR_PUBLISHER_ID
};
const char* pub_xml = "";
const auto pub_req_id = uxr_buffer_create_publisher_xml(&session,reliable_out,pub_id,participant_id,pub_xml,UXR_REPLACE);
// Data Writer
const char* data_writer_ref = topics[i].dw_profile_label;
const auto dwriter_req_id = uxr_buffer_create_datawriter_ref(&session,reliable_out,topics[i].dw_id,pub_id,data_writer_ref,UXR_REPLACE);
// save the request statuses
requests[0] = topic_req_id;
requests[1] = pub_req_id;
requests[2] = dwriter_req_id;
if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Topic/Pub/Writer session request failure for index 'TODO'");
for (uint8_t s = 0 ; s < nRequests; s++) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Status '%d' result '%u'", s, status[s]);
}
// TODO add a failure log message sharing the status results
return false;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Topic/Pub/Writer session pass for index 'TOOO'");
}
} else if (strlen(topics[i].dr_profile_label) > 0) {
// Subscriber
const uxrObjectId sub_id = {
.id = topics[i].sub_id,
.type = UXR_SUBSCRIBER_ID
};
const char* sub_xml = "";
const auto sub_req_id = uxr_buffer_create_subscriber_xml(&session,reliable_out,sub_id,participant_id,sub_xml,UXR_REPLACE);
// Data Reader
const char* data_reader_ref = topics[i].dr_profile_label;
const auto dreader_req_id = uxr_buffer_create_datareader_ref(&session,reliable_out,topics[i].dr_id,sub_id,data_reader_ref,UXR_REPLACE);
// save the request statuses
requests[0] = topic_req_id;
requests[1] = sub_req_id;
requests[2] = dreader_req_id;
if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Topic/Sub/Reader session request failure for index '%d'",(int)i);
for (uint8_t s = 0 ; s < nRequests; s++) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Status '%d' result '%u'", s, status[s]);
}
// TODO add a failure log message sharing the status results
return false;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Topic/Sub/Reader session pass for index '%d'",(int)i);
uxr_buffer_request_data(&session, reliable_out, topics[i].dr_id, reliable_in, &delivery_control);
}
// TODO add a failure log message sharing the status results
return false;
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Topic/Pub/Writer session pass for index 'TOOO'");
}
}
return true;
}

19
libraries/AP_DDS/AP_DDS_Client.h
View File

@ -10,6 +10,7 @@
#include "sensor_msgs/msg/NavSatFix.h"
#include "tf2_msgs/msg/TFMessage.h"
#include "sensor_msgs/msg/BatteryState.h"
#include "sensor_msgs/msg/Joy.h"
#include "geometry_msgs/msg/PoseStamped.h"
#include "geometry_msgs/msg/TwistStamped.h"
#include "geographic_msgs/msg/GeoPoseStamped.h"
@ -58,6 +59,7 @@ private:
sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
tf2_msgs_msg_TFMessage static_transforms_topic;
sensor_msgs_msg_BatteryState battery_state_topic;
sensor_msgs_msg_Joy joy_topic;
geometry_msgs_msg_PoseStamped local_pose_topic;
geometry_msgs_msg_TwistStamped local_velocity_topic;
geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
@ -77,6 +79,20 @@ private:
static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg);
static void update_topic(rosgraph_msgs_msg_Clock& msg);
// subscription callback function
static void on_topic(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length, void* args);
// count of subscribed samples
uint32_t count;
// delivery control parameters
uxrDeliveryControl delivery_control {
.max_samples = UXR_MAX_SAMPLES_UNLIMITED,
.max_elapsed_time = 0,
.max_bytes_per_second = 0,
.min_pace_period = 0
};
// The last ms timestamp AP_DDS wrote a Time message
uint64_t last_time_time_ms;
// The last ms timestamp AP_DDS wrote a NavSatFix message
@ -163,9 +179,12 @@ public:
struct Topic_table {
const uint8_t topic_id;
const uint8_t pub_id;
const uint8_t sub_id; // added sub_id fields to avoid confusion
const uxrObjectId dw_id;
const uxrObjectId dr_id; // added dr_id fields to avoid confusion
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;

37
libraries/AP_DDS/AP_DDS_Topic_Table.h
View File

@ -16,9 +16,12 @@ const 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_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),
@ -26,9 +29,12 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
{
.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_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),
@ -36,9 +42,12 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
{
.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_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),
@ -46,9 +55,12 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
{
.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_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),
@ -56,9 +68,12 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
{
.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_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),
@ -66,9 +81,12 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
{
.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_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),
@ -76,9 +94,12 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
{
.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_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),
@ -86,11 +107,27 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
{
.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_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_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),
},
};

25
libraries/AP_DDS/Idl/sensor_msgs/msg/Joy.idl Normal file
View File

@ -0,0 +1,25 @@
// generated from rosidl_adapter/resource/msg.idl.em
// with input from sensor_msgs/msg/Joy.msg
// generated code does not contain a copyright notice
#include "std_msgs/msg/Header.idl"
module sensor_msgs {
module msg {
@verbatim (language="comment", text=
"Reports the state of a joystick's axes and buttons.")
struct Joy {
@verbatim (language="comment", text=
"The timestamp is the time at which data is received from the joystick.")
std_msgs::msg::Header header;
@verbatim (language="comment", text=
"The axes measurements from a joystick.")
sequence<float> axes;
@verbatim (language="comment", text=
"The buttons measurements from a joystick.")
sequence<int32> buttons;
};
};
};

15
libraries/AP_DDS/dds_xrce_profile.xml
View File

@ -223,4 +223,19 @@
</historyQos>
</topic>
</data_writer>
<topic profile_name="joy__t">
<name>rt/ap/joy</name>
<dataType>sensor_msgs::msg::dds_::Joy_</dataType>
<historyQos>
<kind>KEEP_LAST</kind>
<depth>5</depth>
</historyQos>
</topic>
<data_reader profile_name="joy__dr">
<topic>
<kind>NO_KEY</kind>
<name>rt/ap/joy</name>
<dataType>sensor_msgs::msg::dds_::Joy_</dataType>
</topic>
</data_reader>
</profiles>