mirror of https://github.com/ArduPilot/ardupilot
AP_DDS: Add initial DDS Client support
* Use clang to verify no unused files * Add a topic table to prepare for code generating interfaces * Generated IDL's to to a generated directory in build * Use black to format python files * Populate a ROS time maessage with Linux epoch time for ROS time * Add workarounds for PoseStamped and TwistStamped with manual mods to IDL Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Arsh Pratap <arshpratapofficial@gmail.com> Co-authored-by: Andrew Tridgell <andrew@tridgell.net> Co-authored-by: Russ Webber <russ@rw.id.au> Co-authored-by: Peter Barker <pb-gh@barker.dropbear.id.au>
This commit is contained in:
Ryan Friedman
Andrew Tridgell
parent
d8e221b872
commit
0905ffa438
|
|
@ -0,0 +1,247 @@
|
|||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
|
||||
#if AP_DDS_ENABLED
|
||||
|
||||
#include <AP_RTC/AP_RTC.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#include "AP_DDS_Client.h"
|
||||
#include "generated/Time.h"
|
||||
|
||||
AP_HAL::UARTDriver *dds_port;
|
||||
|
||||
|
||||
const AP_Param::GroupInfo AP_DDS_Client::var_info[]= {
|
||||
//! @todo Params go here
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
#include "AP_DDS_Topic_Table.h"
|
||||
|
||||
void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg)
|
||||
{
|
||||
uint64_t utc_usec;
|
||||
if (!AP::rtc().get_utc_usec(utc_usec)) {
|
||||
utc_usec = AP_HAL::micros64();
|
||||
}
|
||||
msg.sec = utc_usec / 1000000ULL;
|
||||
msg.nanosec = (utc_usec % 1000000ULL) * 1000UL;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
class constructor
|
||||
*/
|
||||
AP_DDS_Client::AP_DDS_Client(void)
|
||||
{
|
||||
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_DDS_Client::main_loop, void),
|
||||
"DDS",
|
||||
8192, AP_HAL::Scheduler::PRIORITY_IO, 1)) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"DDS Client: thread create failed");
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
main loop for DDS thread
|
||||
*/
|
||||
void AP_DDS_Client::main_loop(void)
|
||||
{
|
||||
if (!init() || !create()) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"DDS Client: Creation Requests failed");
|
||||
return;
|
||||
}
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Initialization passed");
|
||||
while (true) {
|
||||
hal.scheduler->delay(1);
|
||||
update();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool AP_DDS_Client::init()
|
||||
{
|
||||
AP_SerialManager *serial_manager = AP_SerialManager::get_singleton();
|
||||
dds_port = serial_manager->find_serial(AP_SerialManager::SerialProtocol_DDS_XRCE, 0);
|
||||
if (dds_port == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// ensure we own the UART
|
||||
dds_port->begin(0);
|
||||
|
||||
constexpr uint8_t fd = 0;
|
||||
constexpr uint8_t relativeSerialAgentAddr = 0;
|
||||
constexpr uint8_t relativeSerialClientAddr = 1;
|
||||
if (!uxr_init_serial_transport(&serial_transport,fd,relativeSerialAgentAddr,relativeSerialClientAddr)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
constexpr uint32_t uniqueClientKey = 0xAAAABBBB;
|
||||
//TODO does this need to be inside the loop to handle reconnect?
|
||||
uxr_init_session(&session, &serial_transport.comm, uniqueClientKey);
|
||||
while (!uxr_create_session(&session)) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Initialization waiting...");
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
|
||||
reliable_in = uxr_create_input_reliable_stream(&session,input_reliable_stream,BUFFER_SIZE_SERIAL,STREAM_HISTORY);
|
||||
reliable_out = uxr_create_output_reliable_stream(&session,output_reliable_stream,BUFFER_SIZE_SERIAL,STREAM_HISTORY);
|
||||
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Init Complete");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_DDS_Client::create()
|
||||
{
|
||||
WITH_SEMAPHORE(csem);
|
||||
|
||||
// Participant
|
||||
const uxrObjectId participant_id = {
|
||||
.id = 0x01,
|
||||
.type = UXR_PARTICIPANT_ID
|
||||
};
|
||||
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};
|
||||
|
||||
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)) {
|
||||
// TODO add a failure log message sharing the status results
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void AP_DDS_Client::write()
|
||||
{
|
||||
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 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: DDS_Client failed to serialize\n");
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void AP_DDS_Client::update()
|
||||
{
|
||||
WITH_SEMAPHORE(csem);
|
||||
|
||||
update_topic(time_topic);
|
||||
write();
|
||||
connected = uxr_run_session_time(&session, 1);
|
||||
}
|
||||
|
||||
/*
|
||||
implement C functions for serial transport
|
||||
*/
|
||||
extern "C" {
|
||||
#include <uxr/client/profile/transport/serial/serial_transport_platform.h>
|
||||
}
|
||||
|
||||
bool uxr_init_serial_platform(void* args, int fd, uint8_t remote_addr, uint8_t local_addr)
|
||||
{
|
||||
//! @todo Add error reporting
|
||||
return true;
|
||||
}
|
||||
|
||||
bool uxr_close_serial_platform(void* args)
|
||||
{
|
||||
//! @todo Add error reporting
|
||||
return true;
|
||||
}
|
||||
|
||||
size_t uxr_write_serial_data_platform(void* args, const uint8_t* buf, size_t len, uint8_t* errcode)
|
||||
{
|
||||
if (dds_port == nullptr) {
|
||||
*errcode = 1;
|
||||
return 0;
|
||||
}
|
||||
ssize_t bytes_written = dds_port->write(buf, len);
|
||||
if (bytes_written <= 0) {
|
||||
*errcode = 1;
|
||||
return 0;
|
||||
}
|
||||
//! @todo Add populate the error code correctly
|
||||
*errcode = 0;
|
||||
return bytes_written;
|
||||
}
|
||||
|
||||
size_t uxr_read_serial_data_platform(void* args, uint8_t* buf, size_t len, int timeout, uint8_t* errcode)
|
||||
{
|
||||
if (dds_port == nullptr) {
|
||||
*errcode = 1;
|
||||
return 0;
|
||||
}
|
||||
while (timeout > 0 && dds_port->available() < len) {
|
||||
hal.scheduler->delay(1); // TODO select or poll this is limiting speed (1mS)
|
||||
timeout--;
|
||||
}
|
||||
ssize_t bytes_read = dds_port->read(buf, len);
|
||||
if (bytes_read <= 0) {
|
||||
*errcode = 1;
|
||||
return 0;
|
||||
}
|
||||
//! @todo Add error reporting
|
||||
*errcode = 0;
|
||||
return bytes_read;
|
||||
}
|
||||
|
||||
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL
|
||||
extern "C" {
|
||||
int clock_gettime(clockid_t clockid, struct timespec *ts);
|
||||
}
|
||||
|
||||
int clock_gettime(clockid_t clockid, struct timespec *ts)
|
||||
{
|
||||
//! @todo the value of clockid is ignored here.
|
||||
//! A fallback mechanism is employed against the caller's choice of clock.
|
||||
uint64_t utc_usec;
|
||||
if (!AP::rtc().get_utc_usec(utc_usec)) {
|
||||
utc_usec = AP_HAL::micros64();
|
||||
}
|
||||
ts->tv_sec = utc_usec / 1000000ULL;
|
||||
ts->tv_nsec = (utc_usec % 1000000ULL) * 1000UL;
|
||||
return 0;
|
||||
}
|
||||
#endif // CONFIG_HAL_BOARD
|
||||
|
||||
#endif // AP_DDS_ENABLED
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,96 @@
|
|||
#pragma once
|
||||
|
||||
#if AP_DDS_ENABLED
|
||||
|
||||
#include "uxr/client/client.h"
|
||||
#include "ucdr/microcdr.h"
|
||||
#include "generated/Time.h"
|
||||
#include "AP_DDS_Generic_Fn_T.h"
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/Scheduler.h>
|
||||
#include <AP_HAL/Semaphores.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
|
||||
#include "fcntl.h"
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#define STREAM_HISTORY 8
|
||||
#define BUFFER_SIZE_SERIAL UXR_CONFIG_SERIAL_TRANSPORT_MTU * STREAM_HISTORY
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
class AP_DDS_Client
|
||||
{
|
||||
|
||||
private:
|
||||
|
||||
// Serial Allocation
|
||||
uxrSerialTransport serial_transport; // client uxr serial transport
|
||||
uxrSession session; //Session
|
||||
|
||||
// Input Stream
|
||||
uint8_t input_reliable_stream[BUFFER_SIZE_SERIAL];
|
||||
uxrStreamId reliable_in;
|
||||
|
||||
// Output Stream
|
||||
uint8_t output_reliable_stream[BUFFER_SIZE_SERIAL];
|
||||
uxrStreamId reliable_out;
|
||||
|
||||
// Topic
|
||||
builtin_interfaces_msg_Time time_topic;
|
||||
|
||||
// Data Writer
|
||||
const uxrObjectId dwriter_id = {
|
||||
.id = 0x01,
|
||||
.type = UXR_DATAWRITER_ID
|
||||
};
|
||||
|
||||
HAL_Semaphore csem;
|
||||
|
||||
// connection parametrics
|
||||
bool connected = true;
|
||||
|
||||
static void update_topic(builtin_interfaces_msg_Time& msg);
|
||||
|
||||
public:
|
||||
// Constructor
|
||||
AP_DDS_Client();
|
||||
|
||||
void main_loop(void);
|
||||
|
||||
//! @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;
|
||||
|
||||
//! @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 Update the internally stored DDS messages with latest data
|
||||
void update();
|
||||
|
||||
//! @brief Parameter storage
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
//! @brief Convenience grouping for a single "channel" of data
|
||||
struct Topic_table {
|
||||
const uint8_t topic_id;
|
||||
const uint8_t pub_id;
|
||||
const char* topic_profile_label;
|
||||
const char* dw_profile_label;
|
||||
Generic_serialize_topic_fn_t serialize;
|
||||
Generic_deserialize_topic_fn_t deserialize;
|
||||
Generic_size_of_topic_fn_t size_of;
|
||||
};
|
||||
static const struct Topic_table topics[];
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // AP_DDS_ENABLED
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
// table maps between string names and pointer to element
|
||||
// Function pointer that matches signature of generated topics
|
||||
typedef bool (*Generic_serialize_topic_fn_t)(struct ucdrBuffer*, const void*);
|
||||
typedef bool (*Generic_deserialize_topic_fn_t)(struct ucdrBuffer*, void*);
|
||||
typedef uint32_t (*Generic_size_of_topic_fn_t)(struct ucdrBuffer*, uint32_t);
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
// #include "Generated/Time.h" // might change to brackets for include path
|
||||
|
||||
#include "AP_DDS_Generic_Fn_T.h"
|
||||
|
||||
// Code generated table based on the enabled topics.
|
||||
// Mavgen is using python, loops are not readable.
|
||||
// Can use jinja to template (like Flask)
|
||||
|
||||
|
||||
const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
|
||||
{
|
||||
.topic_id = 0x01,
|
||||
.pub_id = 0x01,
|
||||
.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),
|
||||
},
|
||||
};
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from sensor_msgs/msg/BatteryState.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
#include "Header.idl"
|
||||
|
||||
module sensor_msgs {
|
||||
module msg {
|
||||
module BatteryState_Constants {
|
||||
@verbatim (language="comment", text=
|
||||
"Constants are chosen to match the enums in the linux kernel" "\n" "defined in include/linux/power_supply.h as of version 3.7" "\n" "The one difference is for style reasons the constants are" "\n" "all uppercase not mixed case." "\n" "Power supply status constants")
|
||||
const octet POWER_SUPPLY_STATUS_UNKNOWN = 0;
|
||||
const octet POWER_SUPPLY_STATUS_CHARGING = 1;
|
||||
const octet POWER_SUPPLY_STATUS_DISCHARGING = 2;
|
||||
const octet POWER_SUPPLY_STATUS_NOT_CHARGING = 3;
|
||||
const octet POWER_SUPPLY_STATUS_FULL = 4;
|
||||
@verbatim (language="comment", text=
|
||||
"Power supply health constants")
|
||||
const octet POWER_SUPPLY_HEALTH_UNKNOWN = 0;
|
||||
const octet POWER_SUPPLY_HEALTH_GOOD = 1;
|
||||
const octet POWER_SUPPLY_HEALTH_OVERHEAT = 2;
|
||||
const octet POWER_SUPPLY_HEALTH_DEAD = 3;
|
||||
const octet POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4;
|
||||
const octet POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5;
|
||||
const octet POWER_SUPPLY_HEALTH_COLD = 6;
|
||||
const octet POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7;
|
||||
const octet POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8;
|
||||
@verbatim (language="comment", text=
|
||||
"Power supply technology (chemistry) constants")
|
||||
const octet POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0;
|
||||
const octet POWER_SUPPLY_TECHNOLOGY_NIMH = 1;
|
||||
const octet POWER_SUPPLY_TECHNOLOGY_LION = 2;
|
||||
const octet POWER_SUPPLY_TECHNOLOGY_LIPO = 3;
|
||||
const octet POWER_SUPPLY_TECHNOLOGY_LIFE = 4;
|
||||
const octet POWER_SUPPLY_TECHNOLOGY_NICD = 5;
|
||||
const octet POWER_SUPPLY_TECHNOLOGY_LIMN = 6;
|
||||
};
|
||||
struct BatteryState {
|
||||
std_msgs::msg::Header header;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Voltage in Volts (Mandatory)")
|
||||
float voltage;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Temperature in Degrees Celsius (If unmeasured NaN)")
|
||||
float temperature;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Negative when discharging (A) (If unmeasured NaN)")
|
||||
float current;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Current charge in Ah (If unmeasured NaN)")
|
||||
float charge;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Capacity in Ah (last full capacity) (If unmeasured NaN)")
|
||||
float capacity;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Capacity in Ah (design capacity) (If unmeasured NaN)")
|
||||
float design_capacity;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Charge percentage on 0 to 1 range (If unmeasured NaN)")
|
||||
float percentage;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"The charging status as reported. Values defined above")
|
||||
octet power_supply_status;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"The battery health metric. Values defined above")
|
||||
octet power_supply_health;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"The battery chemistry. Values defined above")
|
||||
octet power_supply_technology;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"True if the battery is present")
|
||||
boolean present;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"An array of individual cell voltages for each cell in the pack" "\n"
|
||||
"If individual voltages unknown but number of cells known set each to NaN")
|
||||
sequence<float> cell_voltage;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"An array of individual cell temperatures for each cell in the pack" "\n"
|
||||
"If individual temperatures unknown but number of cells known set each to NaN")
|
||||
sequence<float> cell_temperature;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"The location into which the battery is inserted. (slot number or plug)")
|
||||
string location;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"The best approximation of the battery serial number")
|
||||
string serial_number;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from builtin_interfaces/msg/Duration.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
|
||||
module builtin_interfaces {
|
||||
module msg {
|
||||
@verbatim (language="comment", text=
|
||||
"Duration defines a period between two time points." "\n"
|
||||
"Messages of this datatype are of ROS Time following this design:" "\n"
|
||||
"https://design.ros2.org/articles/clock_and_time.html")
|
||||
struct Duration {
|
||||
@verbatim (language="comment", text=
|
||||
"Seconds component, range is valid over any possible int32 value.")
|
||||
int32 sec;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Nanoseconds component in the range of [0, 10e9).")
|
||||
uint32 nanosec;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from std_msgs/msg/Header.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
#include "Time.idl"
|
||||
|
||||
module std_msgs {
|
||||
module msg {
|
||||
@verbatim (language="comment", text=
|
||||
"Standard metadata for higher-level stamped data types." "\n"
|
||||
"This is generally used to communicate timestamped data" "\n"
|
||||
"in a particular coordinate frame.")
|
||||
struct Header {
|
||||
@verbatim (language="comment", text=
|
||||
"Two-integer timestamp that is expressed as seconds and nanoseconds.")
|
||||
builtin_interfaces::msg::Time stamp;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Transform frame with which this data is associated.")
|
||||
string frame_id;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from sensor_msgs/msg/NavSatFix.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
#include "NavSatStatus.idl"
|
||||
#include "Header.idl"
|
||||
|
||||
module sensor_msgs {
|
||||
module msg {
|
||||
typedef double double__9[9];
|
||||
module NavSatFix_Constants {
|
||||
@verbatim (language="comment", text=
|
||||
"If the covariance of the fix is known, fill it in completely. If the" "\n" "GPS receiver provides the variance of each measurement, put them" "\n" "along the diagonal. If only Dilution of Precision is available," "\n" "estimate an approximate covariance from that.")
|
||||
const octet COVARIANCE_TYPE_UNKNOWN = 0;
|
||||
const octet COVARIANCE_TYPE_APPROXIMATED = 1;
|
||||
const octet COVARIANCE_TYPE_DIAGONAL_KNOWN = 2;
|
||||
const octet COVARIANCE_TYPE_KNOWN = 3;
|
||||
};
|
||||
@verbatim (language="comment", text=
|
||||
"Navigation Satellite fix for any Global Navigation Satellite System" "\n"
|
||||
"" "\n"
|
||||
"Specified using the WGS 84 reference ellipsoid")
|
||||
struct NavSatFix {
|
||||
@verbatim (language="comment", text=
|
||||
"header.stamp specifies the ROS time for this measurement (the" "\n"
|
||||
" corresponding satellite time may be reported using the" "\n"
|
||||
" sensor_msgs/TimeReference message)." "\n"
|
||||
"" "\n"
|
||||
"header.frame_id is the frame of reference reported by the satellite" "\n"
|
||||
" receiver, usually the location of the antenna. This is a" "\n"
|
||||
" Euclidean frame relative to the vehicle, not a reference" "\n"
|
||||
" ellipsoid.")
|
||||
std_msgs::msg::Header header;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Satellite fix status information.")
|
||||
sensor_msgs::msg::NavSatStatus status;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Latitude. Positive is north of equator; negative is south.")
|
||||
@unit (value="degrees")
|
||||
double latitude;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Longitude. Positive is east of prime meridian; negative is west.")
|
||||
@unit (value="degrees")
|
||||
double longitude;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Altitude. Positive is above the WGS 84 ellipsoid" "\n"
|
||||
"(quiet NaN if no altitude is available).")
|
||||
@unit (value="m")
|
||||
double altitude;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Position covariance defined relative to a tangential plane" "\n"
|
||||
"through the reported position. The components are East, North, and" "\n"
|
||||
"Up (ENU), in row-major order." "\n"
|
||||
"" "\n"
|
||||
"Beware: this coordinate system exhibits singularities at the poles.")
|
||||
@unit (value="m^2")
|
||||
double position_covariance[9];
|
||||
|
||||
octet position_covariance_type;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from sensor_msgs/msg/NavSatStatus.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
|
||||
module sensor_msgs {
|
||||
module msg {
|
||||
module NavSatStatus_Constants {
|
||||
@verbatim (language="comment", text=
|
||||
"unable to fix position")
|
||||
const char STATUS_NO_FIX = -1;
|
||||
@verbatim (language="comment", text=
|
||||
"unaugmented fix")
|
||||
const char STATUS_FIX = 0;
|
||||
@verbatim (language="comment", text=
|
||||
"with satellite-based augmentation")
|
||||
const char STATUS_SBAS_FIX = 1;
|
||||
@verbatim (language="comment", text=
|
||||
"with ground-based augmentation")
|
||||
const char STATUS_GBAS_FIX = 2;
|
||||
@verbatim (language="comment", text=
|
||||
"Bits defining which Global Navigation Satellite System signals were" "\n" "used by the receiver.")
|
||||
const uint16 SERVICE_GPS = 1;
|
||||
const uint16 SERVICE_GLONASS = 2;
|
||||
@verbatim (language="comment", text=
|
||||
"includes BeiDou.")
|
||||
const uint16 SERVICE_COMPASS = 4;
|
||||
const uint16 SERVICE_GALILEO = 8;
|
||||
};
|
||||
@verbatim (language="comment", text=
|
||||
"Navigation Satellite fix status for any Global Navigation Satellite System." "\n"
|
||||
"" "\n"
|
||||
"Whether to output an augmented fix is determined by both the fix" "\n"
|
||||
"type and the last time differential corrections were received. A" "\n"
|
||||
"fix is valid when status >= STATUS_FIX.")
|
||||
struct NavSatStatus {
|
||||
char status;
|
||||
|
||||
uint16 service;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from geometry_msgs/msg/Point.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
|
||||
module geometry_msgs {
|
||||
module msg {
|
||||
@verbatim (language="comment", text=
|
||||
"This contains the position of a point in free space")
|
||||
struct Point {
|
||||
double x;
|
||||
|
||||
double y;
|
||||
|
||||
double z;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from geometry_msgs/msg/Pose.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
#include "Point.idl"
|
||||
#include "Quaternion.idl"
|
||||
|
||||
module geometry_msgs {
|
||||
module msg {
|
||||
@verbatim (language="comment", text=
|
||||
"A representation of pose in free space, composed of position and orientation.")
|
||||
struct Pose {
|
||||
geometry_msgs::msg::Point position;
|
||||
|
||||
geometry_msgs::msg::Quaternion orientation;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from geometry_msgs/msg/PoseStamped.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
#include "Point.idl"
|
||||
#include "Quaternion.idl"
|
||||
#include "Header.idl"
|
||||
struct Pose {
|
||||
geometry_msgs::msg::Point position;
|
||||
|
||||
geometry_msgs::msg::Quaternion orientation;
|
||||
};
|
||||
module geometry_msgs {
|
||||
module msg {
|
||||
@verbatim (language="comment", text=
|
||||
"A Pose with reference coordinate frame and timestamp")
|
||||
struct PoseStamped {
|
||||
std_msgs::msg::Header header;
|
||||
|
||||
Pose pose;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from geometry_msgs/msg/Quaternion.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
|
||||
module geometry_msgs {
|
||||
module msg {
|
||||
@verbatim (language="comment", text=
|
||||
"This represents an orientation in free space in quaternion form.")
|
||||
struct Quaternion {
|
||||
@default (value=0.0)
|
||||
double x;
|
||||
|
||||
@default (value=0.0)
|
||||
double y;
|
||||
|
||||
@default (value=0.0)
|
||||
double z;
|
||||
|
||||
@default (value=1.0)
|
||||
double w;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from builtin_interfaces/msg/Time.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
|
||||
module builtin_interfaces {
|
||||
module msg {
|
||||
@verbatim (language="comment", text=
|
||||
"This message communicates ROS Time defined here:" "\n"
|
||||
"https://design.ros2.org/articles/clock_and_time.html")
|
||||
struct Time {
|
||||
@verbatim (language="comment", text=
|
||||
"The seconds component, valid over all int32 values.")
|
||||
int32 sec;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"The nanoseconds component, valid in the range [0, 10e9).")
|
||||
uint32 nanosec;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from geometry_msgs/msg/Twist.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
#include "Vector3.idl"
|
||||
|
||||
module geometry_msgs {
|
||||
module msg {
|
||||
@verbatim (language="comment", text=
|
||||
"This expresses velocity in free space broken into its linear and angular parts.")
|
||||
struct Twist {
|
||||
geometry_msgs::msg::Vector3 linear;
|
||||
|
||||
geometry_msgs::msg::Vector3 angular;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from geometry_msgs/msg/TwistStamped.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
#include "Vector3.idl"
|
||||
#include "Header.idl"
|
||||
|
||||
struct Twist {
|
||||
geometry_msgs::msg::Vector3 linear;
|
||||
geometry_msgs::msg::Vector3 angular;
|
||||
};
|
||||
|
||||
module geometry_msgs {
|
||||
module msg {
|
||||
@verbatim (language="comment", text=
|
||||
"A twist with reference coordinate frame and timestamp")
|
||||
struct TwistStamped {
|
||||
std_msgs::msg::Header header;
|
||||
|
||||
Twist twist;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from geometry_msgs/msg/Vector3.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
|
||||
module geometry_msgs {
|
||||
module msg {
|
||||
@verbatim (language="comment", text=
|
||||
"This represents a vector in free space.")
|
||||
struct Vector3 {
|
||||
@verbatim (language="comment", text=
|
||||
"This is semantically different than a point." "\n"
|
||||
"A vector is always anchored at the origin." "\n"
|
||||
"When a transform is applied to a vector, only the rotational component is applied.")
|
||||
double x;
|
||||
|
||||
double y;
|
||||
|
||||
double z;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,200 @@
|
|||
# Testing with DDS/micro-Ros
|
||||
|
||||
## Architecture
|
||||
|
||||
Ardupilot contains the DDS Client library, which can run as SITL. Then, the DDS application runs a ROS2 node, an EProsima Integration Service, and the MicroXRCE Agent. The two systems communicate over serial, which is the only supported protocol in Ardupilot MicroXCE DDS at this time.
|
||||
|
||||
```mermaid
|
||||
---
|
||||
title: Hardware Serial Port Loopback
|
||||
---
|
||||
graph LR
|
||||
|
||||
subgraph Linux Computer
|
||||
|
||||
subgraph Ardupilot SITL
|
||||
veh[sim_vehicle.py] <--> xrceClient[EProsima Micro XRCE DDS Client]
|
||||
xrceClient <--> port1[devUSB1]
|
||||
end
|
||||
|
||||
subgraph DDS Application
|
||||
ros[ROS2 Node] <--> agent[Micro ROS Agent]
|
||||
agent <--> port2[devUSB2]
|
||||
end
|
||||
|
||||
port1 <--> port2
|
||||
|
||||
end
|
||||
```
|
||||
|
||||
Currently, serial is the only supported transport, but there are plans to add IP-based transport over ethernet.
|
||||
|
||||
## Installing Build Dependencies
|
||||
|
||||
While DDS support in Ardupilot is mostly through git submodules, another tool needs to be available on your system: Micro XRCE DDS Gen.
|
||||
|
||||
1. Go to a directory on your system to clone the repo (perhaps next to `ardupilot`)
|
||||
1. Install java
|
||||
```console
|
||||
sudo apt install java
|
||||
````
|
||||
1. Follow instructions [here](https://micro-xrce-dds.docs.eprosima.com/en/latest/installation.html#installing-the-micro-xrce-dds-gen-tool) to install the generator, but use `develop` branch instead of `master` (for now).
|
||||
```console
|
||||
git clone -b develop --recurse-submodules https://github.com/eProsima/Micro-XRCE-DDS-Gen.git
|
||||
cd Micro-XRCE-DDS-Gen
|
||||
./gradlew assemble
|
||||
```
|
||||
|
||||
1. Add the generator directory to $PATH, like [so](https://github.com/eProsima/Micro-XRCE-DDS-docs/issues/83).
|
||||
1. Test it
|
||||
```console
|
||||
cd /path/to/ardupilot
|
||||
microxrceddsgen -version
|
||||
# openjdk version "11.0.18" 2023-01-17
|
||||
# OpenJDK Runtime Environment (build 11.0.18+10-post-Ubuntu-0ubuntu122.04)
|
||||
# OpenJDK 64-Bit Server VM (build 11.0.18+10-post-Ubuntu-0ubuntu122.04, mixed mode, sharing)
|
||||
# microxrceddsgen version: 1.0.0beta2
|
||||
```
|
||||
|
||||
> :warning: **If you have installed FastDDS or FastDDSGen globally on your system**:
|
||||
eProsima's libraries and the packaging system in Ardupilot are not determistic in this scenario.
|
||||
You may experience the wrong version of a library brought in, or runtime segfaults.
|
||||
For now, avoid having simultaneous local and global installs.
|
||||
If you followed the [global install](https://fast-dds.docs.eprosima.com/en/latest/installation/sources/sources_linux.html#global-installation)
|
||||
section, you should remove it and switch to local install.
|
||||
|
||||
## Parameters for DDS
|
||||
|
||||
| Name | Description |
|
||||
| - | - |
|
||||
| SERIAL1_BAUD | The serial baud rate for DDS |
|
||||
| SERIAL1_PROTOCOL | Set this to 45 to use DDS on the serial port |
|
||||
|
||||
|
||||
## Testing with a UART
|
||||
|
||||
On Linux, first create a virtual serial port for use with SITL like [this](https://stackoverflow.com/questions/52187/virtual-serial-port-for-linux)
|
||||
|
||||
```
|
||||
sudo apt-get update
|
||||
sudo apt-get install socat
|
||||
```
|
||||
|
||||
Then, start a virtual serial port with socat. Take note of the two `/dev/pts/*` ports. If yours are different, substitute as needed.
|
||||
```
|
||||
socat -d -d pty,raw,echo=0 pty,raw,echo=0
|
||||
>>> 2023/02/21 05:26:06 socat[334] N PTY is /dev/pts/1
|
||||
>>> 2023/02/21 05:26:06 socat[334] N PTY is /dev/pts/2
|
||||
>>> 2023/02/21 05:26:06 socat[334] N starting data transfer loop with FDs [5,5] and [7,7]
|
||||
```
|
||||
|
||||
Set up your [SITL](https://ardupilot.org/dev/docs/setting-up-sitl-on-linux.html).
|
||||
Run the simulator with the following command (assuming we are using /dev/pts/1 for Ardupilot SITL). Take note how two parameters need adjusting from default to use DDS.
|
||||
```
|
||||
# Wipe params till you see "AP: ArduPilot Ready"
|
||||
# Select your favorite vehicle type
|
||||
sim_vehicle.py -w -v ArduPlane
|
||||
|
||||
# Set params
|
||||
param set SERIAL1_BAUD 115
|
||||
# See libraries/AP_SerialManager/AP_SerialManager.h AP_SerialManager SerialProtocol_DDS_XRCE
|
||||
param set SERIAL1_PROTOCOL 45
|
||||
```
|
||||
|
||||
# Start the sim now with the new params
|
||||
```
|
||||
sim_vehicle.py -v ArduPlane -D --console --enable-dds -A "--uartC=uart:/dev/pts/1"
|
||||
```
|
||||
|
||||
## Starting with microROS Agent
|
||||
|
||||
Follow the steps to use the microROS Agent
|
||||
|
||||
- Install ROS Humble (as described here)
|
||||
|
||||
- https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html
|
||||
|
||||
- Install and run the microROS agent (as descibed here). Make sure to use the `humble` branch.
|
||||
|
||||
- https://micro.ros.org/docs/tutorials/core/first_application_linux/
|
||||
|
||||
Until this [PR](https://github.com/micro-ROS/micro-ROS.github.io) is merged, ignore the notes about `foxy`. It works on `humble`.
|
||||
|
||||
Follow the instructions for the following:
|
||||
|
||||
* Do "Installing ROS 2 and the micro-ROS build system"
|
||||
* Skip the docker run command, build it locally instead
|
||||
* Skip "Creating a new firmware workspace"
|
||||
* Skip "Building the firmware"
|
||||
* Do "Creating the micro-ROS agent"
|
||||
* Source your ROS workspace
|
||||
|
||||
- Run microROS agent with the following command
|
||||
|
||||
```bash
|
||||
cd ardupilot/libraries/AP_DDS
|
||||
ros2 run micro_ros_agent micro_ros_agent serial -b 115200 -D /dev/pts/2 -r dds_xrce_profile.xml # (assuming we are using tty/pts/2 for Ardupilot)
|
||||
```
|
||||
|
||||
## Tutorial
|
||||
|
||||
### Using the ROS2 CLI to Read Ardupilot Data
|
||||
|
||||
If you have installed the microROS agent and ROS-2 Humble
|
||||
|
||||
- Source the ros2 installation
|
||||
- ```source /opt/ros/humble/setup.bash```
|
||||
|
||||
- If SITL is running alongise MicroROS Agent, you should be able to see the agent here and view the data output.
|
||||
|
||||
|
||||
```
|
||||
$ ros2 node list
|
||||
/Ardupilot_DDS_XRCE_Client
|
||||
|
||||
$ ros2 topic list -v
|
||||
Published topics:
|
||||
* /ROS2_Time [builtin_interfaces/msg/Time] 1 publisher
|
||||
* /parameter_events [rcl_interfaces/msg/ParameterEvent] 1 publisher
|
||||
* /rosout [rcl_interfaces/msg/Log] 1 publisher
|
||||
|
||||
Subscribed topics:
|
||||
|
||||
$ ros2 topic hz /ROS2_Time
|
||||
average rate: 50.115
|
||||
min: 0.012s max: 0.024s std dev: 0.00328s window: 52
|
||||
|
||||
$ ros2 topic echo /ROS2_Time
|
||||
sec: 1678668735
|
||||
nanosec: 729410000
|
||||
---
|
||||
```
|
||||
|
||||
## Adding DDS messages to Ardupilot
|
||||
|
||||
Unlike the use of ROS 2 `.msg` files, since Ardupilot supports native DDS, the message files follow [OMG IDL DDS v4.2](https://www.omg.org/spec/IDL/4.2/PDF).
|
||||
This package is intended to work with any `.idl` file complying with those extensions, with some limitations.
|
||||
|
||||
1. IDL files need to be in the same folder, and modified includes.
|
||||
1. Topic types can't use alias types.
|
||||
1. Arrays need manually edited type names.
|
||||
|
||||
Over time, these restrictions will ideally go away.
|
||||
|
||||
To get a new IDL file from ROS2, follow this process:
|
||||
```console
|
||||
cd ardupilot
|
||||
source /opt/ros/humble/setup.bash
|
||||
# Find the IDL file
|
||||
find /opt/ros/$ROS_DISTRO -type f -wholename \*builtin_interfaces/msg/Time.idl
|
||||
# Create the directory in the source tree if it doesn't exist
|
||||
mkdir -p libraries/AP_DDS_Client/Idl/builtin_interfaces/msg/
|
||||
# Copy the IDL
|
||||
cp -r /opt/ros/humble/share/builtin_interfaces/msg/Time.idl libraries/AP_DDS_Client/Idl/builtin_interfaces/msg/
|
||||
# Now, apply the mods manually to be compliant with MicroXRCEDDSGen limitations
|
||||
# Create an output directory to test it
|
||||
mkdir -p /tmp/xrce_out
|
||||
# Run the generator
|
||||
microxrceddsgen -replace -d /tmp/xrce_out libraries/AP_DDS_Client/Idl/builtin_interfaces/msg/Time.idl
|
||||
# cat /tmp/xrce_out/
|
||||
```
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
<?xml version="1.0"?>
|
||||
<profiles>
|
||||
<participant profile_name="participant_profile">
|
||||
<rtps>
|
||||
<name>Ardupilot_DDS_XRCE_Client</name>
|
||||
</rtps>
|
||||
</participant>
|
||||
<topic profile_name="time__t">
|
||||
<name>rt/ROS2_Time</name>
|
||||
<dataType>builtin_interfaces::msg::dds_::Time_</dataType>
|
||||
<historyQos>
|
||||
<kind>KEEP_LAST</kind>
|
||||
<depth>20</depth>
|
||||
</historyQos>
|
||||
</topic>
|
||||
<data_writer profile_name="time__dw">
|
||||
<historyMemoryPolicy>PREALLOCATED_WITH_REALLOC</historyMemoryPolicy>
|
||||
<qos>
|
||||
<reliability>
|
||||
<kind>RELIABLE</kind>
|
||||
</reliability>
|
||||
</qos>
|
||||
<topic>
|
||||
<kind>NO_KEY</kind>
|
||||
<name>rt/ROS2_Time</name>
|
||||
<dataType>builtin_interfaces::msg::dds_::Time_</dataType>
|
||||
<historyQos>
|
||||
<kind>KEEP_LAST</kind>
|
||||
<depth>20</depth>
|
||||
</historyQos>
|
||||
</topic>
|
||||
</data_writer>
|
||||
</profiles>
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
#!/usr/bin/env python
|
||||
'''
|
||||
process a *.h.in file to produce a *.h file
|
||||
'''
|
||||
|
||||
# TODO move to AP_DDS_Client/tools
|
||||
|
||||
import re
|
||||
import sys
|
||||
|
||||
config = {
|
||||
'PROJECT_VERSION_MAJOR': 1,
|
||||
'PROJECT_VERSION_MINOR': 0,
|
||||
'PROJECT_VERSION_PATCH': 0,
|
||||
'PROJECT_VERSION': 2,
|
||||
'UCLIENT_MAX_OUTPUT_BEST_EFFORT_STREAMS': 4,
|
||||
'UCLIENT_MAX_OUTPUT_RELIABLE_STREAMS': 4,
|
||||
'UCLIENT_MAX_INPUT_BEST_EFFORT_STREAMS': 4,
|
||||
'UCLIENT_MAX_INPUT_RELIABLE_STREAMS': 2,
|
||||
'UCLIENT_MAX_SESSION_CONNECTION_ATTEMPTS': 2,
|
||||
'UCLIENT_MIN_SESSION_CONNECTION_INTERVAL': 1000,
|
||||
'UCLIENT_MIN_HEARTBEAT_TIME_INTERVAL': 200,
|
||||
'UCLIENT_UDP_TRANSPORT_MTU': 300,
|
||||
'UCLIENT_TCP_TRANSPORT_MTU': 350,
|
||||
'UCLIENT_SERIAL_TRANSPORT_MTU': 250,
|
||||
'UCLIENT_CUSTOM_TRANSPORT_MTU': 400,
|
||||
'CONFIG_MACHINE_ENDIANNESS': 1, # little endian
|
||||
'UCLIENT_SHARED_MEMORY_MAX_ENTITIES': 0,
|
||||
'UCLIENT_SHARED_MEMORY_STATIC_MEM_SIZE': 0,
|
||||
'UCLIENT_HARD_LIVELINESS_CHECK_TIMEOUT': 1000,
|
||||
}
|
||||
|
||||
defines = {
|
||||
"UCLIENT_PROFILE_UDP": 0,
|
||||
"UCLIENT_PROFILE_TCP": 0,
|
||||
"UCLIENT_PROFILE_CAN": 0,
|
||||
"UCLIENT_PROFILE_SERIAL": 1,
|
||||
"UCLIENT_PROFILE_CUSTOM_TRANSPORT": 0,
|
||||
"UCLIENT_PROFILE_DISCOVERY": 0,
|
||||
"UCLIENT_PLATFORM_POSIX": 1,
|
||||
"UCLIENT_PLATFORM_POSIX_NOPOLL": 0,
|
||||
"UCLIENT_PLATFORM_WINDOWS": 0,
|
||||
"UCLIENT_PLATFORM_FREERTOS_PLUS_TCP": 0,
|
||||
"UCLIENT_PLATFORM_ZEPHYR": 0,
|
||||
"UCLIENT_EXTERNAL_TCP": 0,
|
||||
"UCLIENT_EXTERNAL_UDP": 0,
|
||||
"UCLIENT_EXTERNAL_SERIAL": 0,
|
||||
"UCLIENT_PROFILE_MULTITHREAD": 0,
|
||||
"UCLIENT_PROFILE_SHARED_MEMORY": 0,
|
||||
"UCLIENT_PROFILE_STREAM_FRAMING": 0,
|
||||
"UCLIENT_PLATFORM_RTEMS_BSD_NET": 0,
|
||||
"UCLIENT_TWEAK_XRCE_WRITE_LIMIT": 0,
|
||||
"UCLIENT_HARD_LIVELINESS_CHECK": 0,
|
||||
}
|
||||
|
||||
h_in = sys.argv[1]
|
||||
h = sys.argv[2]
|
||||
print("Processing %s to %s" % (h_in, h))
|
||||
|
||||
txt = open(h_in, 'r').read()
|
||||
|
||||
for c in sorted(config.keys(), key=len, reverse=True):
|
||||
txt = txt.replace("@%s@" % c, str(config[c]))
|
||||
|
||||
for c in sorted(defines.keys(), key=len, reverse=True):
|
||||
if defines[c] != 0:
|
||||
txt = txt.replace("#cmakedefine %s" % c, "#define %s %u" % (c, defines[c]))
|
||||
else:
|
||||
txt = txt.replace("#cmakedefine %s" % c, "// #define %s %u" % (c, defines[c]))
|
||||
|
||||
matches = re.findall(r'@(\w+)@', txt)
|
||||
if len(matches) > 0:
|
||||
print("Missing config elements: ", matches)
|
||||
sys.exit(1)
|
||||
|
||||
matches = re.findall(r'#cmakedefine\s+\w+', txt)
|
||||
if len(matches) > 0:
|
||||
print("Missing #cmakedefine elements: ", matches)
|
||||
sys.exit(1)
|
||||
|
||||
open(h, 'w').write(txt)
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
#!/usr/bin/env python
|
||||
# encoding: utf-8
|
||||
|
||||
import os
|
||||
|
||||
|
||||
def configure(cfg):
|
||||
extra_src = [
|
||||
'modules/Micro-XRCE-DDS-Client/src/c/core/session/stream/*.c',
|
||||
'modules/Micro-XRCE-DDS-Client/src/c/core/session/*.c',
|
||||
'modules/Micro-XRCE-DDS-Client/src/c/core/serialization/*.c',
|
||||
'modules/Micro-XRCE-DDS-Client/src/c/util/*.c',
|
||||
'modules/Micro-XRCE-DDS-Client/src/c/profile/transport/serial/serial_transport.c',
|
||||
'modules/Micro-XRCE-DDS-Client/src/c/profile/transport/stream_framing/stream_framing_protocol.c',
|
||||
'modules/Micro-CDR/src/c/types/*.c',
|
||||
'modules/Micro-CDR/src/c/common.c',
|
||||
]
|
||||
|
||||
cfg.env.AP_LIB_EXTRA_SOURCES['AP_DDS'] = []
|
||||
for pattern in extra_src:
|
||||
s = cfg.srcnode.ant_glob(pattern, dir=False, src=True)
|
||||
for x in s:
|
||||
cfg.env.AP_LIB_EXTRA_SOURCES['AP_DDS'].append(str(x))
|
||||
|
||||
extra_src_inc = [
|
||||
'modules/Micro-XRCE-DDS-Client/include',
|
||||
'modules/Micro-XRCE-DDS-Client/include/uxr/client',
|
||||
'modules/Micro-CDR/src/c',
|
||||
'modules/Micro-CDR/include',
|
||||
'modules/Micro-CDR/include/ucdr',
|
||||
]
|
||||
for inc in extra_src_inc:
|
||||
cfg.env.INCLUDES += [str(cfg.srcnode.make_node(inc))]
|
||||
|
||||
# auto update submodules
|
||||
cfg.env.append_value('GIT_SUBMODULES', 'Micro-XRCE-DDS-Client')
|
||||
cfg.env.append_value('GIT_SUBMODULES', 'Micro-CDR')
|
||||
|
||||
|
||||
def build(bld):
|
||||
config_h_in = [
|
||||
'modules/Micro-XRCE-DDS-Client/include/uxr/client/config.h.in',
|
||||
'modules/Micro-CDR/include/ucdr/config.h.in',
|
||||
]
|
||||
config_h_in_nodes = [bld.srcnode.make_node(h) for h in config_h_in]
|
||||
config_h_nodes = [bld.bldnode.find_or_declare(h[:-3]) for h in config_h_in]
|
||||
|
||||
gen_path = "libraries/AP_DDS"
|
||||
extra_bld_inc = [
|
||||
'modules/Micro-CDR/include',
|
||||
'modules/Micro-XRCE-DDS-Client/include',
|
||||
gen_path,
|
||||
]
|
||||
for inc in extra_bld_inc:
|
||||
bld.env.INCLUDES += [bld.bldnode.find_or_declare(inc).abspath()]
|
||||
|
||||
for i in range(len(config_h_nodes)):
|
||||
print("building %s" % config_h_nodes[i].abspath())
|
||||
bld(
|
||||
# build config.h file
|
||||
source=config_h_in_nodes[i],
|
||||
target=config_h_nodes[i],
|
||||
rule="%s %s/%s %s %s"
|
||||
% (
|
||||
bld.env.get_flat('PYTHON'),
|
||||
bld.env.SRCROOT,
|
||||
"libraries/AP_DDS/gen_config_h.py",
|
||||
config_h_in_nodes[i].abspath(),
|
||||
config_h_nodes[i].abspath(),
|
||||
),
|
||||
group='dynamic_sources',
|
||||
)
|
||||
|
||||
# temporary whitelist while include issue is sorted out
|
||||
whitelist = [
|
||||
'BatteryState.idl',
|
||||
'Duration.idl',
|
||||
'Header.idl',
|
||||
'NavSatFix.idl',
|
||||
'NavSatStatus.idl',
|
||||
'Point.idl',
|
||||
'Pose.idl',
|
||||
'PoseStamped.idl',
|
||||
'Quaternion.idl',
|
||||
'Time.idl',
|
||||
'Twist.idl',
|
||||
'TwistStamped.idl',
|
||||
'Vector3.idl',
|
||||
]
|
||||
|
||||
idl_files = bld.srcnode.ant_glob('libraries/AP_DDS/Idl/**/*.idl')
|
||||
for idl in idl_files:
|
||||
b = os.path.basename(idl.abspath())
|
||||
if not b in whitelist:
|
||||
continue
|
||||
gen_h = b.replace('.idl', '.h')
|
||||
gen_c = b.replace('.idl', '.c')
|
||||
gen = [
|
||||
bld.bldnode.find_or_declare(os.path.join(gen_path, "generated", gen_h)),
|
||||
bld.bldnode.find_or_declare(os.path.join(gen_path, "generated", gen_c)),
|
||||
]
|
||||
bld(
|
||||
# build IDL file
|
||||
source=idl,
|
||||
target=gen,
|
||||
rule=f"{bld.env.MICROXRCEDDSGEN[0]} -replace -d {os.path.join(gen_path, 'generated')} {idl}",
|
||||
group='dynamic_sources',
|
||||
)
|
||||
bld.env.AP_LIB_EXTRA_SOURCES['AP_DDS'] += [os.path.join('generated', gen_c)]
|
||||
Loading…
Reference in New Issue