Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DDS: Add multi-topic support with NavSatFix 

* Implement NavSatFix message
* Support covariance
* Set frame ID to WGS-84
* Closes #23284

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2023-03-11 11:42:42 -07:00 committed by Andrew Tridgell
parent 18dc37eef8
commit 863656b037
4 changed files with 215 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

193
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -2,6 +2,7 @@
#if AP_DDS_ENABLED
#include <AP_GPS/AP_GPS.h>
#include <AP_RTC/AP_RTC.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <GCS_MAVLink/GCS.h>
@ -9,6 +10,11 @@
#include "AP_DDS_Client.h"
#include "generated/Time.h"
static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10;
static constexpr uint16_t DELAY_NAV_SAT_FIX_TOPIC_MS = 1000;
static constexpr uint8_t GPS_INSTANCE_NO = 0; // Use primary GPS
static char WGS_84_FRAME_ID[] = "WGS-84";
AP_HAL::UARTDriver *dds_port;
@ -31,6 +37,83 @@ void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg)
}
void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg)
{
// Add a lambda that takes in navsatfix msg and populates the cov
// Make it constexpr if possible
// https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/
// constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; };
update_topic(msg.header.stamp);
strcpy(msg.header.frame_id, WGS_84_FRAME_ID);
msg.status.service = 0; // SERVICE_GPS
msg.status.status = -1; // STATUS_NO_FIX
if (!AP::gps().is_healthy()) {
msg.status.status = -1; // STATUS_NO_FIX
msg.status.service = 0; // No services supported
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
return;
}
//! @todo What about glonass, compass, galileo?
//! This will be properly designed and implemented to spec in #23277
msg.status.service = 1; // SERVICE_GPS
const auto status = AP::gps().status(GPS_INSTANCE_NO);
switch (status) {
case AP_GPS::NO_GPS:
case AP_GPS::NO_FIX:
msg.status.status = -1; // STATUS_NO_FIX
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
return;
case AP_GPS::GPS_OK_FIX_2D:
case AP_GPS::GPS_OK_FIX_3D:
msg.status.status = 0; // STATUS_FIX
msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED
break;
case AP_GPS::GPS_OK_FIX_3D_DGPS:
msg.status.status = 1; // STATUS_SBAS_FIX
msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED
break;
case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT:
case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED:
msg.status.status = 2; // STATUS_SBAS_FIX
msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED
// RTK provides "rtk_accuracy" member, should it be used for the covariance?
break;
default:
//! @todo Can we not just use an enum class and not worry about this condition?
break;
}
const auto loc = AP::gps().location(GPS_INSTANCE_NO);
msg.latitude = loc.lat * 1E-7;
msg.longitude = loc.lng * 1E-7;
int32_t alt_cm;
if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) {
// With absolute frame, this condition is unlikely
msg.status.status = -1; // STATUS_NO_FIX
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
return;
}
msg.altitude = alt_cm / 100.0;
// Calculate covariance: https://answers.ros.org/question/10310/calculate-navsatfix-covariance/
// https://github.com/ros-drivers/nmea_navsat_driver/blob/indigo-devel/src/libnmea_navsat_driver/driver.py#L110-L114
//! @todo This calculation will be moved to AP::gps and fixed in #23259
//! It is a placeholder for now matching the ROS1 nmea_navsat_driver behavior
const auto hdop = AP::gps().get_hdop(GPS_INSTANCE_NO);
const auto hdopSq = hdop * hdop;
const auto vdop = AP::gps().get_vdop(GPS_INSTANCE_NO);
const auto vdopSq = vdop * vdop;
msg.position_covariance[0] = hdopSq;
msg.position_covariance[4] = hdopSq;
msg.position_covariance[8] = vdopSq;
}
/*
class constructor
@ -107,64 +190,108 @@ bool AP_DDS_Client::create()
const char* participant_ref = "participant_profile";
const auto participant_req_id = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id,0,participant_ref,UXR_REPLACE);
// Topic
const uxrObjectId topic_id = {
.id = topics[0].topic_id,
.type = UXR_TOPIC_ID
};
const char* topic_ref = topics[0].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[0].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[0].dw_profile_label;
const auto dwriter_req_id = uxr_buffer_create_datawriter_ref(&session,reliable_out,dwriter_id,pub_id,data_writer_ref,UXR_REPLACE);
//Status requests
constexpr uint8_t nRequests = 4;
const uint16_t requests[nRequests] = {participant_req_id, topic_req_id, pub_req_id, dwriter_req_id};
//Participant requests
constexpr uint8_t nRequestsParticipant = 1;
const uint16_t requestsParticipant[nRequestsParticipant] = {participant_req_id};
constexpr int maxTimeMsPerRequestMs = 250;
constexpr int requestTimeoutMs = nRequests * maxTimeMsPerRequestMs;
uint8_t status[nRequests];
if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) {
constexpr int requestTimeoutParticipantMs = nRequestsParticipant * maxTimeMsPerRequestMs;
uint8_t statusPartipant[nRequestsParticipant];
if (!uxr_run_session_until_all_status(&session, requestTimeoutParticipantMs, requestsParticipant, statusPartipant, nRequestsParticipant)) {
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Partipant session request failure");
// TODO add a failure log message sharing the status results
return false;
}
for (size_t i = 0 ; i < ARRAY_SIZE(topics); i++) {
// Topic
const uxrObjectId topic_id = {
.id = topics[i].topic_id,
.type = UXR_TOPIC_ID
};
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;
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]);
}
// 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;
}
void AP_DDS_Client::write()
void AP_DDS_Client::write_time_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub;
uint32_t topic_size = builtin_interfaces_msg_Time_size_of_topic(&time_topic, 0);
uxr_prepare_output_stream(&session,reliable_out,dwriter_id,&ub,topic_size);
const uint32_t topic_size = builtin_interfaces_msg_Time_size_of_topic(&time_topic, 0);
uxr_prepare_output_stream(&session,reliable_out,topics[0].dw_id,&ub,topic_size);
const bool success = builtin_interfaces_msg_Time_serialize_topic(&ub, &time_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: XRCE_Client failed to serialize\n");
}
}
}
void AP_DDS_Client::write_nav_sat_fix_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub;
const uint32_t topic_size = sensor_msgs_msg_NavSatFix_size_of_topic(&nav_sat_fix_topic, 0);
uxr_prepare_output_stream(&session,reliable_out,topics[1].dw_id,&ub,topic_size);
const bool success = sensor_msgs_msg_NavSatFix_serialize_topic(&ub, &nav_sat_fix_topic);
if (!success) {
// TODO sometimes serialization fails on bootup. Determine why.
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
}
}
}
void AP_DDS_Client::update()
{
WITH_SEMAPHORE(csem);
update_topic(time_topic);
write();
const auto cur_time_ms = AP_HAL::millis64();
if (cur_time_ms - last_time_time_ms > DELAY_TIME_TOPIC_MS) {
update_topic(time_topic);
last_time_time_ms = cur_time_ms;
write_time_topic();
}
if (cur_time_ms - last_nav_sat_fix_time_ms > DELAY_NAV_SAT_FIX_TOPIC_MS) {
update_topic(nav_sat_fix_topic);
last_nav_sat_fix_time_ms = cur_time_ms;
write_nav_sat_fix_topic();
}
connected = uxr_run_session_time(&session, 1);
}

24
libraries/AP_DDS/AP_DDS_Client.h
View File

@ -6,6 +6,7 @@
#include "ucdr/microcdr.h"
#include "generated/Time.h"
#include "AP_DDS_Generic_Fn_T.h"
#include "generated/NavSatFix.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/Scheduler.h>
@ -39,12 +40,7 @@ private:
// Topic
builtin_interfaces_msg_Time time_topic;
// Data Writer
const uxrObjectId dwriter_id = {
.id = 0x01,
.type = UXR_DATAWRITER_ID
};
sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
HAL_Semaphore csem;
@ -52,6 +48,13 @@ private:
bool connected = true;
static void update_topic(builtin_interfaces_msg_Time& msg);
static void update_topic(sensor_msgs_msg_NavSatFix& msg);
// 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
uint64_t last_nav_sat_fix_time_ms;
public:
// Constructor
@ -61,15 +64,17 @@ public:
//! @brief Initialize the client's transport, uxr session, and IO stream(s)
//! @return True on successful initialization, false on failure
bool init() WARN_IF_UNUSED;
bool init() WARN_IF_UNUSED;
//! @brief Set up the client's participants, data read/writes,
// publishers, subscribers
//! @return True on successful creation, false on failure
bool create() WARN_IF_UNUSED;
//! @brief Serialize the current data state and publish to to the IO stream(s)
void write();
//! @brief Serialize the current time state and publish to to the IO stream(s)
void write_time_topic();
//! @brief Serialize the current nav_sat_fix state and publish to to the IO stream(s)
void write_nav_sat_fix_topic();
//! @brief Update the internally stored DDS messages with latest data
void update();
@ -80,6 +85,7 @@ public:
struct Topic_table {
const uint8_t topic_id;
const uint8_t pub_id;
const uxrObjectId dw_id;
const char* topic_profile_label;
const char* dw_profile_label;
Generic_serialize_topic_fn_t serialize;

16
libraries/AP_DDS/AP_DDS_Topic_Table.h
View File

@ -1,6 +1,9 @@
// #include "Generated/Time.h" // might change to brackets for include path
#include "generated/Time.h"
#include "generated/NavSatFix.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.
@ -11,10 +14,21 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
{
.topic_id = 0x01,
.pub_id = 0x01,
.dw_id = uxrObjectId{.id=0x01, .type=UXR_DATAWRITER_ID},
.topic_profile_label = "time__t",
.dw_profile_label = "time__dw",
.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,
.dw_id = uxrObjectId{.id=0x02, .type=UXR_DATAWRITER_ID},
.topic_profile_label = "navsatfix0__t",
.dw_profile_label = "navsatfix0__dw",
.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),
},
};

25
libraries/AP_DDS/dds_xrce_profile.xml
View File

@ -30,4 +30,29 @@
</historyQos>
</topic>
</data_writer>
<topic profile_name="navsatfix0__t">
<name>rt/ROS2_NavSatFix0</name>
<dataType>sensor_msgs::msg::dds_::NavSatFix_</dataType>
<historyQos>
<kind>KEEP_LAST</kind>
<depth>20</depth>
</historyQos>
</topic>
<data_writer profile_name="navsatfix0__dw">
<historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
<qos>
<reliability>
<kind>RELIABLE</kind>
</reliability>
</qos>
<topic>
<kind>NO_KEY</kind>
<name>rt/ROS2_NavSatFix0</name>
<dataType>sensor_msgs::msg::dds_::NavSatFix_</dataType>
<historyQos>
<kind>KEEP_LAST</kind>
<depth>20</depth>
</historyQos>
</topic>
</data_writer>
</profiles>