5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00

AP_DDS: publish gps global origin

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
This commit is contained in:
Rhys Mainwaring 2024-03-05 19:26:39 +00:00 committed by Andrew Tridgell
parent 5dc9366cea
commit 88a4d6848a
4 changed files with 85 additions and 0 deletions

View File

@ -37,6 +37,7 @@ static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33;
static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33;
static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33;
static constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10;
static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = 1000;
static constexpr uint16_t DELAY_PING_MS = 500;
// Define the subscriber data members, which are static class scope.
@ -459,6 +460,24 @@ void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
update_topic(msg.clock);
}
void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg)
{
update_topic(msg.header.stamp);
strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
auto &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
Location ekf_origin;
// LLA is WGS-84 geodetic coordinate.
// Altitude converted from cm to m.
if (ahrs.get_origin(ekf_origin)) {
msg.position.latitude = ekf_origin.lat * 1E-7;
msg.position.longitude = ekf_origin.lng * 1E-7;
msg.position.altitude = ekf_origin.alt * 0.01;
}
}
/*
start the DDS thread
*/
@ -1040,6 +1059,20 @@ void AP_DDS_Client::write_clock_topic()
}
}
void AP_DDS_Client::write_gps_global_origin_topic()
{
WITH_SEMAPHORE(csem);
if (connected) {
ucdrBuffer ub {};
const uint32_t topic_size = geographic_msgs_msg_GeoPointStamped_size_of_topic(&gps_global_origin_topic, 0);
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB)].dw_id, &ub, topic_size);
const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &gps_global_origin_topic);
if (!success) {
// AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
}
}
}
void AP_DDS_Client::update()
{
WITH_SEMAPHORE(csem);
@ -1093,6 +1126,12 @@ void AP_DDS_Client::update()
write_clock_topic();
}
if (cur_time_ms - last_gps_global_origin_time_ms > DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS) {
update_topic(gps_global_origin_topic);
last_gps_global_origin_time_ms = cur_time_ms;
write_gps_global_origin_topic();
}
status_ok = uxr_run_session_time(&session, 1);
}

View File

@ -15,6 +15,7 @@
#include "sensor_msgs/msg/Joy.h"
#include "geometry_msgs/msg/PoseStamped.h"
#include "geometry_msgs/msg/TwistStamped.h"
#include "geographic_msgs/msg/GeoPointStamped.h"
#include "geographic_msgs/msg/GeoPoseStamped.h"
#include "rosgraph_msgs/msg/Clock.h"
@ -58,6 +59,7 @@ private:
// Outgoing Sensor and AHRS data
builtin_interfaces_msg_Time time_topic;
geographic_msgs_msg_GeoPointStamped gps_global_origin_topic;
geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
geometry_msgs_msg_PoseStamped local_pose_topic;
geometry_msgs_msg_TwistStamped tx_local_velocity_topic;
@ -91,6 +93,7 @@ private:
static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg);
static void update_topic(sensor_msgs_msg_Imu& msg);
static void update_topic(rosgraph_msgs_msg_Clock& msg);
static void update_topic(geographic_msgs_msg_GeoPointStamped& msg);
// subscription callback function
static void on_topic_trampoline(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length, void* args);
@ -124,6 +127,8 @@ private:
uint64_t last_geo_pose_time_ms;
// The last ms timestamp AP_DDS wrote a Clock message
uint64_t last_clock_time_ms;
// The last ms timestamp AP_DDS wrote a gps global origin message
uint64_t last_gps_global_origin_time_ms;
// functions for serial transport
bool ddsSerialInit();
@ -196,6 +201,8 @@ public:
void write_imu_topic();
//! @brief Serialize the current clock and publish to the IO stream(s)
void write_clock_topic();
//! @brief Serialize the current gps global origin and publish to the IO stream(s)
void write_gps_global_origin_topic();
//! @brief Update the internally stored DDS messages with latest data
void update();

View File

@ -21,6 +21,7 @@ enum class TopicIndex: uint8_t {
LOCAL_VELOCITY_PUB,
GEOPOSE_PUB,
CLOCK_PUB,
GPS_GLOBAL_ORIGIN_PUB,
JOY_SUB,
DYNAMIC_TRANSFORMS_SUB,
VELOCITY_CONTROL_SUB,
@ -125,6 +126,16 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
.dw_profile_label = "clock__dw",
.dr_profile_label = "",
},
{
.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},
.topic_profile_label = "gps_global_origin__t",
.dw_profile_label = "gps_global_origin__dw",
.dr_profile_label = "",
},
{
.topic_id = to_underlying(TopicIndex::JOY_SUB),
.pub_id = to_underlying(TopicIndex::JOY_SUB),

View File

@ -246,6 +246,34 @@
</historyQos>
</topic>
</data_writer>
<topic profile_name="gps_global_origin__t">
<name>rt/ap/gps_global_origin/filtered</name>
<dataType>geographic_msgs::msg::dds_::GeoPointStamped_</dataType>
<historyQos>
<kind>KEEP_LAST</kind>
<depth>5</depth>
</historyQos>
</topic>
<data_writer profile_name="gps_global_origin__dw">
<historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
<qos>
<reliability>
<kind>BEST_EFFORT</kind>
</reliability>
<durability>
<kind>VOLATILE</kind>
</durability>
</qos>
<topic>
<kind>NO_KEY</kind>
<name>rt/ap/gps_global_origin/filtered</name>
<dataType>geographic_msgs::msg::dds_::GeoPointStamped_</dataType>
<historyQos>
<kind>KEEP_LAST</kind>
<depth>5</depth>
</historyQos>
</topic>
</data_writer>
<topic profile_name="joy__t">
<name>rt/ap/joy</name>
<dataType>sensor_msgs::msg::dds_::Joy_</dataType>