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:
parent
5dc9366cea
commit
88a4d6848a
@ -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);
|
||||
}
|
||||
|
||||
|
@ -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();
|
||||
|
||||
|
@ -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),
|
||||
|
@ -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>
|
||||
|
Loading…
Reference in New Issue
Block a user