mirror of https://github.com/ArduPilot/ardupilot
1779 lines
68 KiB
C++
1779 lines
68 KiB
C++
#include <AP_HAL/AP_HAL_Boards.h>
|
|
|
|
#include "AP_DDS_config.h"
|
|
#if AP_DDS_ENABLED
|
|
#include <uxr/client/util/ping.h>
|
|
|
|
#include <AP_GPS/AP_GPS.h>
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <RC_Channel/RC_Channel.h>
|
|
#include <AP_RTC/AP_RTC.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_InertialSensor/AP_InertialSensor.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
#include <AP_BattMonitor/AP_BattMonitor.h>
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#if AP_DDS_ARM_SERVER_ENABLED
|
|
#include <AP_Arming/AP_Arming.h>
|
|
# endif // AP_DDS_ARM_SERVER_ENABLED
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
|
#include <AP_Common/AP_FWVersion.h>
|
|
#include <AP_ExternalControl/AP_ExternalControl_config.h>
|
|
|
|
#if AP_DDS_ARM_SERVER_ENABLED
|
|
#include "ardupilot_msgs/srv/ArmMotors.h"
|
|
#endif // AP_DDS_ARM_SERVER_ENABLED
|
|
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
|
|
#include "ardupilot_msgs/srv/ModeSwitch.h"
|
|
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
|
|
#if AP_DDS_ARM_CHECK_SERVER_ENABLED
|
|
#include "std_srvs/srv/Trigger.h"
|
|
#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
|
|
#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
|
|
#include "ardupilot_msgs/srv/Takeoff.h"
|
|
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
|
|
|
|
#if AP_EXTERNAL_CONTROL_ENABLED
|
|
#include "AP_DDS_ExternalControl.h"
|
|
#endif // AP_EXTERNAL_CONTROL_ENABLED
|
|
#include "AP_DDS_Frames.h"
|
|
|
|
#include "AP_DDS_Client.h"
|
|
#include "AP_DDS_Topic_Table.h"
|
|
#include "AP_DDS_Service_Table.h"
|
|
#include "AP_DDS_External_Odom.h"
|
|
|
|
#define STRCPY(D,S) strncpy(D, S, ARRAY_SIZE(D))
|
|
|
|
// Enable DDS at runtime by default
|
|
static constexpr uint8_t ENABLED_BY_DEFAULT = 1;
|
|
#if AP_DDS_TIME_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_TIME_TOPIC_MS = AP_DDS_DELAY_TIME_TOPIC_MS;
|
|
#endif // AP_DDS_TIME_PUB_ENABLED
|
|
#if AP_DDS_BATTERY_STATE_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = AP_DDS_DELAY_BATTERY_STATE_TOPIC_MS;
|
|
#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
|
|
#if AP_DDS_IMU_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_IMU_TOPIC_MS = AP_DDS_DELAY_IMU_TOPIC_MS;
|
|
#endif // AP_DDS_IMU_PUB_ENABLED
|
|
#if AP_DDS_LOCAL_POSE_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = AP_DDS_DELAY_LOCAL_POSE_TOPIC_MS;
|
|
#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
|
|
#if AP_DDS_LOCAL_VEL_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = AP_DDS_DELAY_LOCAL_VELOCITY_TOPIC_MS;
|
|
#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
|
|
#if AP_DDS_AIRSPEED_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = AP_DDS_DELAY_AIRSPEED_TOPIC_MS;
|
|
#endif // AP_DDS_AIRSPEED_PUB_ENABLED
|
|
#if AP_DDS_GEOPOSE_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = AP_DDS_DELAY_GEO_POSE_TOPIC_MS;
|
|
#endif // AP_DDS_GEOPOSE_PUB_ENABLED
|
|
#if AP_DDS_GOAL_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_GOAL_TOPIC_MS = AP_DDS_DELAY_GOAL_TOPIC_MS ;
|
|
#endif // AP_DDS_GOAL_PUB_ENABLED
|
|
#if AP_DDS_CLOCK_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_CLOCK_TOPIC_MS =AP_DDS_DELAY_CLOCK_TOPIC_MS;
|
|
#endif // AP_DDS_CLOCK_PUB_ENABLED
|
|
#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = AP_DDS_DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS;
|
|
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_PING_MS = 500;
|
|
#ifdef AP_DDS_STATUS_PUB_ENABLED
|
|
static constexpr uint16_t DELAY_STATUS_TOPIC_MS = AP_DDS_DELAY_STATUS_TOPIC_MS;
|
|
#endif // AP_DDS_STATUS_PUB_ENABLED
|
|
|
|
// Define the subscriber data members, which are static class scope.
|
|
// If these are created on the stack in the subscriber,
|
|
// the AP_DDS_Client::on_topic frame size is exceeded.
|
|
#if AP_DDS_JOY_SUB_ENABLED
|
|
sensor_msgs_msg_Joy AP_DDS_Client::rx_joy_topic {};
|
|
#endif // AP_DDS_JOY_SUB_ENABLED
|
|
tf2_msgs_msg_TFMessage AP_DDS_Client::rx_dynamic_transforms_topic {};
|
|
#if AP_DDS_VEL_CTRL_ENABLED
|
|
geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {};
|
|
#endif // AP_DDS_VEL_CTRL_ENABLED
|
|
#if AP_DDS_GLOBAL_POS_CTRL_ENABLED
|
|
ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {};
|
|
#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
|
|
|
|
// Define the parameter server data members, which are static class scope.
|
|
// If these are created on the stack, then the AP_DDS_Client::on_request
|
|
// frame size is exceeded.
|
|
#if AP_DDS_PARAMETER_SERVER_ENABLED
|
|
rcl_interfaces_srv_SetParameters_Request AP_DDS_Client::set_parameter_request {};
|
|
rcl_interfaces_srv_SetParameters_Response AP_DDS_Client::set_parameter_response {};
|
|
rcl_interfaces_srv_GetParameters_Request AP_DDS_Client::get_parameters_request {};
|
|
rcl_interfaces_srv_GetParameters_Response AP_DDS_Client::get_parameters_response {};
|
|
rcl_interfaces_msg_Parameter AP_DDS_Client::param {};
|
|
#endif
|
|
|
|
const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
|
|
|
|
// @Param: _ENABLE
|
|
// @DisplayName: DDS enable
|
|
// @Description: Enable DDS subsystem
|
|
// @Values: 0:Disabled,1:Enabled
|
|
// @RebootRequired: True
|
|
// @User: Advanced
|
|
AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_DDS_Client, enabled, ENABLED_BY_DEFAULT, AP_PARAM_FLAG_ENABLE),
|
|
|
|
#if AP_DDS_UDP_ENABLED
|
|
// @Param: _UDP_PORT
|
|
// @DisplayName: DDS UDP port
|
|
// @Description: UDP port number for DDS
|
|
// @Range: 1 65535
|
|
// @RebootRequired: True
|
|
// @User: Standard
|
|
AP_GROUPINFO("_UDP_PORT", 2, AP_DDS_Client, udp.port, 2019),
|
|
|
|
// @Group: _IP
|
|
// @Path: ../AP_Networking/AP_Networking_address.cpp
|
|
AP_SUBGROUPINFO(udp.ip, "_IP", 3, AP_DDS_Client, AP_Networking_IPV4),
|
|
|
|
#endif
|
|
|
|
// @Param: _DOMAIN_ID
|
|
// @DisplayName: DDS DOMAIN ID
|
|
// @Description: Set the ROS_DOMAIN_ID
|
|
// @Range: 0 232
|
|
// @RebootRequired: True
|
|
// @User: Standard
|
|
AP_GROUPINFO("_DOMAIN_ID", 4, AP_DDS_Client, domain_id, 0),
|
|
|
|
// @Param: _TIMEOUT_MS
|
|
// @DisplayName: DDS ping timeout
|
|
// @Description: The time in milliseconds the DDS client will wait for a response from the XRCE agent before reattempting.
|
|
// @Units: ms
|
|
// @Range: 1 10000
|
|
// @RebootRequired: True
|
|
// @Increment: 1
|
|
// @User: Standard
|
|
AP_GROUPINFO("_TIMEOUT_MS", 5, AP_DDS_Client, ping_timeout_ms, 1000),
|
|
|
|
// @Param: _MAX_RETRY
|
|
// @DisplayName: DDS ping max attempts
|
|
// @Description: The maximum number of times the DDS client will attempt to ping the XRCE agent before exiting.
|
|
// @Range: 1 100
|
|
// @RebootRequired: True
|
|
// @Increment: 1
|
|
// @User: Standard
|
|
AP_GROUPINFO("_MAX_RETRY", 6, AP_DDS_Client, ping_max_retry, 10),
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
static void initialize(geometry_msgs_msg_Quaternion& q)
|
|
{
|
|
q.x = 0.0;
|
|
q.y = 0.0;
|
|
q.z = 0.0;
|
|
q.w = 1.0;
|
|
}
|
|
|
|
AP_DDS_Client::~AP_DDS_Client()
|
|
{
|
|
// close transport
|
|
if (is_using_serial) {
|
|
uxr_close_custom_transport(&serial.transport);
|
|
} else {
|
|
#if AP_DDS_UDP_ENABLED
|
|
uxr_close_custom_transport(&udp.transport);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
#if AP_DDS_TIME_PUB_ENABLED
|
|
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;
|
|
|
|
}
|
|
#endif // AP_DDS_TIME_PUB_ENABLED
|
|
|
|
#if AP_DDS_NAVSATFIX_PUB_ENABLED
|
|
bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance)
|
|
{
|
|
// 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; };
|
|
|
|
auto &gps = AP::gps();
|
|
WITH_SEMAPHORE(gps.get_semaphore());
|
|
|
|
if (!gps.is_healthy(instance)) {
|
|
msg.status.status = -1; // STATUS_NO_FIX
|
|
msg.status.service = 0; // No services supported
|
|
msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN
|
|
return false;
|
|
}
|
|
|
|
// No update is needed
|
|
const auto last_fix_time_ms = gps.last_fix_time_ms(instance);
|
|
if (last_nav_sat_fix_time_ms == last_fix_time_ms) {
|
|
return false;
|
|
} else {
|
|
last_nav_sat_fix_time_ms = last_fix_time_ms;
|
|
}
|
|
|
|
|
|
update_topic(msg.header.stamp);
|
|
static_assert(GPS_MAX_RECEIVERS <= 9, "GPS_MAX_RECEIVERS is greater than 9");
|
|
hal.util->snprintf(msg.header.frame_id, 2, "%u", instance);
|
|
msg.status.service = 0; // SERVICE_GPS
|
|
msg.status.status = -1; // STATUS_NO_FIX
|
|
|
|
|
|
//! @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 = gps.status(instance);
|
|
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 true;
|
|
case AP_GPS::GPS_OK_FIX_2D:
|
|
case AP_GPS::GPS_OK_FIX_3D:
|
|
msg.status.status = 0; // STATUS_FIX
|
|
break;
|
|
case AP_GPS::GPS_OK_FIX_3D_DGPS:
|
|
msg.status.status = 1; // STATUS_SBAS_FIX
|
|
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
|
|
break;
|
|
default:
|
|
//! @todo Can we not just use an enum class and not worry about this condition?
|
|
break;
|
|
}
|
|
const auto loc = gps.location(instance);
|
|
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 true;
|
|
}
|
|
msg.altitude = alt_cm * 0.01;
|
|
|
|
// ROS allows double precision, ArduPilot exposes float precision today
|
|
Matrix3f cov;
|
|
msg.position_covariance_type = (uint8_t)gps.position_covariance(instance, cov);
|
|
msg.position_covariance[0] = cov[0][0];
|
|
msg.position_covariance[1] = cov[0][1];
|
|
msg.position_covariance[2] = cov[0][2];
|
|
msg.position_covariance[3] = cov[1][0];
|
|
msg.position_covariance[4] = cov[1][1];
|
|
msg.position_covariance[5] = cov[1][2];
|
|
msg.position_covariance[6] = cov[2][0];
|
|
msg.position_covariance[7] = cov[2][1];
|
|
msg.position_covariance[8] = cov[2][2];
|
|
|
|
return true;
|
|
}
|
|
#endif // AP_DDS_NAVSATFIX_PUB_ENABLED
|
|
|
|
#if AP_DDS_STATIC_TF_PUB_ENABLED
|
|
void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
|
|
{
|
|
msg.transforms_size = 0;
|
|
|
|
auto &gps = AP::gps();
|
|
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
|
|
const auto gps_type = gps.get_type(i);
|
|
if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) {
|
|
continue;
|
|
}
|
|
update_topic(msg.transforms[i].header.stamp);
|
|
char gps_frame_id[16];
|
|
//! @todo should GPS frame ID's be 0 or 1 indexed in ROS?
|
|
hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i);
|
|
STRCPY(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID);
|
|
STRCPY(msg.transforms[i].child_frame_id, gps_frame_id);
|
|
// The body-frame offsets
|
|
// X - Forward
|
|
// Y - Right
|
|
// Z - Down
|
|
// https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation
|
|
|
|
const auto offset = gps.get_antenna_offset(i);
|
|
|
|
// In ROS REP 103, it follows this convention
|
|
// X - Forward
|
|
// Y - Left
|
|
// Z - Up
|
|
// https://www.ros.org/reps/rep-0103.html#axis-orientation
|
|
|
|
msg.transforms[i].transform.translation.x = offset[0];
|
|
msg.transforms[i].transform.translation.y = -1 * offset[1];
|
|
msg.transforms[i].transform.translation.z = -1 * offset[2];
|
|
|
|
// Ensure rotation is initialized.
|
|
initialize(msg.transforms[i].transform.rotation);
|
|
|
|
msg.transforms_size++;
|
|
}
|
|
|
|
}
|
|
#endif // AP_DDS_STATIC_TF_PUB_ENABLED
|
|
|
|
#if AP_DDS_BATTERY_STATE_PUB_ENABLED
|
|
void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance)
|
|
{
|
|
if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) {
|
|
return;
|
|
}
|
|
static_assert(AP_BATT_MONITOR_MAX_INSTANCES <= 99, "AP_BATT_MONITOR_MAX_INSTANCES is greater than 99");
|
|
|
|
update_topic(msg.header.stamp);
|
|
hal.util->snprintf(msg.header.frame_id, 2, "%u", instance);
|
|
|
|
auto &battery = AP::battery();
|
|
|
|
if (!battery.healthy(instance)) {
|
|
msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD
|
|
msg.present = false;
|
|
return;
|
|
}
|
|
msg.present = true;
|
|
|
|
msg.voltage = battery.voltage(instance);
|
|
|
|
float temperature;
|
|
msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN;
|
|
|
|
float current;
|
|
msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN;
|
|
|
|
const float design_capacity = (float)battery.pack_capacity_mah(instance) * 0.001;
|
|
msg.design_capacity = design_capacity;
|
|
|
|
uint8_t percentage;
|
|
if (battery.capacity_remaining_pct(percentage, instance)) {
|
|
msg.percentage = percentage * 0.01;
|
|
msg.charge = (percentage * design_capacity) * 0.01;
|
|
} else {
|
|
msg.percentage = NAN;
|
|
msg.charge = NAN;
|
|
}
|
|
|
|
msg.capacity = NAN;
|
|
|
|
if (battery.current_amps(current, instance)) {
|
|
if (percentage == 100) {
|
|
msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL
|
|
} else if (is_negative(current)) {
|
|
msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING
|
|
} else if (is_positive(current)) {
|
|
msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING
|
|
} else {
|
|
msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING
|
|
}
|
|
} else {
|
|
msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN
|
|
}
|
|
|
|
msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD
|
|
|
|
msg.power_supply_technology = 0; //POWER_SUPPLY_TECHNOLOGY_UNKNOWN
|
|
|
|
if (battery.has_cell_voltages(instance)) {
|
|
const auto &cells = battery.get_cell_voltages(instance);
|
|
const uint8_t ncells_max = MIN(ARRAY_SIZE(msg.cell_voltage), ARRAY_SIZE(cells.cells));
|
|
for (uint8_t i=0; i< ncells_max; i++) {
|
|
msg.cell_voltage[i] = cells.cells[i] * 0.001;
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
|
|
|
|
#if AP_DDS_LOCAL_POSE_PUB_ENABLED
|
|
void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
|
|
{
|
|
update_topic(msg.header.stamp);
|
|
STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
|
|
|
|
auto &ahrs = AP::ahrs();
|
|
WITH_SEMAPHORE(ahrs.get_semaphore());
|
|
|
|
// ROS REP 103 uses the ENU convention:
|
|
// X - East
|
|
// Y - North
|
|
// Z - Up
|
|
// https://www.ros.org/reps/rep-0103.html#axis-orientation
|
|
// AP_AHRS uses the NED convention
|
|
// X - North
|
|
// Y - East
|
|
// Z - Down
|
|
// As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
|
|
// as well as invert Z
|
|
|
|
Vector3f position;
|
|
if (ahrs.get_relative_position_NED_home(position)) {
|
|
msg.pose.position.x = position[1];
|
|
msg.pose.position.y = position[0];
|
|
msg.pose.position.z = -position[2];
|
|
}
|
|
|
|
// In ROS REP 103, axis orientation uses the following convention:
|
|
// X - Forward
|
|
// Y - Left
|
|
// Z - Up
|
|
// https://www.ros.org/reps/rep-0103.html#axis-orientation
|
|
// As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
|
|
// as well as invert Z (NED to ENU conversion) as well as a 90 degree rotation in the Z axis
|
|
// for x to point forward
|
|
Quaternion orientation;
|
|
if (ahrs.get_quaternion(orientation)) {
|
|
Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation
|
|
Quaternion transformation (sqrtF(2) * 0.5,0,0,sqrtF(2) * 0.5); // Z axis 90 degree rotation
|
|
orientation = aux * transformation;
|
|
msg.pose.orientation.w = orientation[0];
|
|
msg.pose.orientation.x = orientation[1];
|
|
msg.pose.orientation.y = orientation[2];
|
|
msg.pose.orientation.z = orientation[3];
|
|
} else {
|
|
initialize(msg.pose.orientation);
|
|
}
|
|
}
|
|
#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
|
|
|
|
#if AP_DDS_LOCAL_VEL_PUB_ENABLED
|
|
void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
|
|
{
|
|
update_topic(msg.header.stamp);
|
|
STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
|
|
|
|
auto &ahrs = AP::ahrs();
|
|
WITH_SEMAPHORE(ahrs.get_semaphore());
|
|
|
|
// ROS REP 103 uses the ENU convention:
|
|
// X - East
|
|
// Y - North
|
|
// Z - Up
|
|
// https://www.ros.org/reps/rep-0103.html#axis-orientation
|
|
// AP_AHRS uses the NED convention
|
|
// X - North
|
|
// Y - East
|
|
// Z - Down
|
|
// As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
|
|
// as well as invert Z
|
|
Vector3f velocity;
|
|
if (ahrs.get_velocity_NED(velocity)) {
|
|
msg.twist.linear.x = velocity[1];
|
|
msg.twist.linear.y = velocity[0];
|
|
msg.twist.linear.z = -velocity[2];
|
|
}
|
|
|
|
// In ROS REP 103, axis orientation uses the following convention:
|
|
// X - Forward
|
|
// Y - Left
|
|
// Z - Up
|
|
// https://www.ros.org/reps/rep-0103.html#axis-orientation
|
|
// The gyro data is received from AP_AHRS in body-frame
|
|
// X - Forward
|
|
// Y - Right
|
|
// Z - Down
|
|
// As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z
|
|
Vector3f angular_velocity = ahrs.get_gyro();
|
|
msg.twist.angular.x = angular_velocity[0];
|
|
msg.twist.angular.y = -angular_velocity[1];
|
|
msg.twist.angular.z = -angular_velocity[2];
|
|
}
|
|
#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
|
|
#if AP_DDS_AIRSPEED_PUB_ENABLED
|
|
bool AP_DDS_Client::update_topic(geometry_msgs_msg_Vector3Stamped& msg)
|
|
{
|
|
update_topic(msg.header.stamp);
|
|
STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
|
|
auto &ahrs = AP::ahrs();
|
|
WITH_SEMAPHORE(ahrs.get_semaphore());
|
|
// In ROS REP 103, axis orientation uses the following convention:
|
|
// X - Forward
|
|
// Y - Left
|
|
// Z - Up
|
|
// https://www.ros.org/reps/rep-0103.html#axis-orientation
|
|
// The true airspeed data is received from AP_AHRS in body-frame
|
|
// X - Forward
|
|
// Y - Right
|
|
// Z - Down
|
|
// As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z
|
|
Vector3f true_airspeed_vec_bf;
|
|
bool is_airspeed_available {false};
|
|
if (ahrs.airspeed_vector_true(true_airspeed_vec_bf)) {
|
|
msg.vector.x = true_airspeed_vec_bf[0];
|
|
msg.vector.y = -true_airspeed_vec_bf[1];
|
|
msg.vector.z = -true_airspeed_vec_bf[2];
|
|
is_airspeed_available = true;
|
|
}
|
|
return is_airspeed_available;
|
|
}
|
|
#endif // AP_DDS_AIRSPEED_PUB_ENABLED
|
|
|
|
#if AP_DDS_GEOPOSE_PUB_ENABLED
|
|
void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& 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 loc;
|
|
if (ahrs.get_location(loc)) {
|
|
msg.pose.position.latitude = loc.lat * 1E-7;
|
|
msg.pose.position.longitude = loc.lng * 1E-7;
|
|
// TODO this is assumed to be absolute frame in WGS-84 as per the GeoPose message definition in ROS.
|
|
// Use loc.get_alt_frame() to convert if necessary.
|
|
msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m
|
|
}
|
|
|
|
// In ROS REP 103, axis orientation uses the following convention:
|
|
// X - Forward
|
|
// Y - Left
|
|
// Z - Up
|
|
// https://www.ros.org/reps/rep-0103.html#axis-orientation
|
|
// As a consequence, to follow ROS REP 103, it is necessary to switch X and Y,
|
|
// as well as invert Z (NED to ENU conversion) as well as a 90 degree rotation in the Z axis
|
|
// for x to point forward
|
|
Quaternion orientation;
|
|
if (ahrs.get_quaternion(orientation)) {
|
|
Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation
|
|
Quaternion transformation(sqrtF(2) * 0.5, 0, 0, sqrtF(2) * 0.5); // Z axis 90 degree rotation
|
|
orientation = aux * transformation;
|
|
msg.pose.orientation.w = orientation[0];
|
|
msg.pose.orientation.x = orientation[1];
|
|
msg.pose.orientation.y = orientation[2];
|
|
msg.pose.orientation.z = orientation[3];
|
|
} else {
|
|
initialize(msg.pose.orientation);
|
|
}
|
|
}
|
|
#endif // AP_DDS_GEOPOSE_PUB_ENABLED
|
|
|
|
#if AP_DDS_GOAL_PUB_ENABLED
|
|
bool AP_DDS_Client::update_topic_goal(geographic_msgs_msg_GeoPointStamped& msg)
|
|
{
|
|
const auto &vehicle = AP::vehicle();
|
|
update_topic(msg.header.stamp);
|
|
Location target_loc;
|
|
// Exit if no target is available.
|
|
if (!vehicle->get_target_location(target_loc)) {
|
|
return false;
|
|
}
|
|
target_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
|
|
msg.position.latitude = target_loc.lat * 1e-7;
|
|
msg.position.longitude = target_loc.lng * 1e-7;
|
|
msg.position.altitude = target_loc.alt * 1e-2;
|
|
|
|
// Check whether the goal has changed or if the topic has never been published.
|
|
const double tolerance_lat_lon = 1e-8; // One order of magnitude smaller than the target's resolution.
|
|
const double distance_alt = 1e-3;
|
|
if (abs(msg.position.latitude - prev_goal_msg.position.latitude) > tolerance_lat_lon ||
|
|
abs(msg.position.longitude - prev_goal_msg.position.longitude) > tolerance_lat_lon ||
|
|
abs(msg.position.altitude - prev_goal_msg.position.altitude) > distance_alt ||
|
|
prev_goal_msg.header.stamp.sec == 0 ) {
|
|
update_topic(prev_goal_msg.header.stamp);
|
|
prev_goal_msg.position.latitude = msg.position.latitude;
|
|
prev_goal_msg.position.longitude = msg.position.longitude;
|
|
prev_goal_msg.position.altitude = msg.position.altitude;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
#endif // AP_DDS_GOAL_PUB_ENABLED
|
|
|
|
#if AP_DDS_IMU_PUB_ENABLED
|
|
void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg)
|
|
{
|
|
update_topic(msg.header.stamp);
|
|
STRCPY(msg.header.frame_id, BASE_LINK_NED_FRAME_ID);
|
|
|
|
auto &imu = AP::ins();
|
|
auto &ahrs = AP::ahrs();
|
|
|
|
WITH_SEMAPHORE(ahrs.get_semaphore());
|
|
|
|
Quaternion orientation;
|
|
if (ahrs.get_quaternion(orientation)) {
|
|
msg.orientation.x = orientation[0];
|
|
msg.orientation.y = orientation[1];
|
|
msg.orientation.z = orientation[2];
|
|
msg.orientation.w = orientation[3];
|
|
} else {
|
|
initialize(msg.orientation);
|
|
}
|
|
msg.orientation_covariance[0] = -1;
|
|
|
|
uint8_t accel_index = ahrs.get_primary_accel_index();
|
|
uint8_t gyro_index = ahrs.get_primary_gyro_index();
|
|
const Vector3f accel_data = imu.get_accel(accel_index);
|
|
const Vector3f gyro_data = imu.get_gyro(gyro_index);
|
|
|
|
// Populate the message fields
|
|
msg.linear_acceleration.x = accel_data.x;
|
|
msg.linear_acceleration.y = accel_data.y;
|
|
msg.linear_acceleration.z = accel_data.z;
|
|
|
|
msg.angular_velocity.x = gyro_data.x;
|
|
msg.angular_velocity.y = gyro_data.y;
|
|
msg.angular_velocity.z = gyro_data.z;
|
|
msg.angular_velocity_covariance[0] = -1;
|
|
msg.linear_acceleration_covariance[0] = -1;
|
|
}
|
|
#endif // AP_DDS_IMU_PUB_ENABLED
|
|
|
|
#if AP_DDS_CLOCK_PUB_ENABLED
|
|
void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
|
|
{
|
|
update_topic(msg.clock);
|
|
}
|
|
#endif // AP_DDS_CLOCK_PUB_ENABLED
|
|
|
|
#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
|
|
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;
|
|
}
|
|
}
|
|
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
|
|
|
|
#if AP_DDS_STATUS_PUB_ENABLED
|
|
bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg)
|
|
{
|
|
// Fill the new message.
|
|
const auto &vehicle = AP::vehicle();
|
|
const auto &battery = AP::battery();
|
|
msg.vehicle_type = static_cast<uint8_t>(AP::fwversion().vehicle_type);
|
|
msg.armed = hal.util->get_soft_armed();
|
|
msg.mode = vehicle->get_mode();
|
|
msg.flying = vehicle->get_likely_flying();
|
|
msg.external_control = true; // Always true for now. To be filled after PR#28429.
|
|
uint8_t fs_iter = 0;
|
|
msg.failsafe_size = 0;
|
|
if (rc().in_rc_failsafe()) {
|
|
msg.failsafe[fs_iter++] = FS_RADIO;
|
|
}
|
|
if (battery.has_failsafed()) {
|
|
msg.failsafe[fs_iter++] = FS_BATTERY;
|
|
}
|
|
// TODO: replace flag with function.
|
|
if (AP_Notify::flags.failsafe_gcs) {
|
|
msg.failsafe[fs_iter++] = FS_GCS;
|
|
}
|
|
// TODO: replace flag with function.
|
|
if (AP_Notify::flags.failsafe_ekf) {
|
|
msg.failsafe[fs_iter++] = FS_EKF;
|
|
}
|
|
msg.failsafe_size = fs_iter;
|
|
|
|
// Compare with the previous one.
|
|
bool is_message_changed {false};
|
|
is_message_changed |= (last_status_msg_.flying != msg.flying);
|
|
is_message_changed |= (last_status_msg_.armed != msg.armed);
|
|
is_message_changed |= (last_status_msg_.mode != msg.mode);
|
|
is_message_changed |= (last_status_msg_.vehicle_type != msg.vehicle_type);
|
|
is_message_changed |= (last_status_msg_.failsafe_size != msg.failsafe_size);
|
|
is_message_changed |= (last_status_msg_.external_control != msg.external_control);
|
|
|
|
if ( is_message_changed ) {
|
|
last_status_msg_.flying = msg.flying;
|
|
last_status_msg_.armed = msg.armed;
|
|
last_status_msg_.mode = msg.mode;
|
|
last_status_msg_.vehicle_type = msg.vehicle_type;
|
|
last_status_msg_.failsafe_size = msg.failsafe_size;
|
|
last_status_msg_.external_control = msg.external_control;
|
|
update_topic(msg.header.stamp);
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
#endif // AP_DDS_STATUS_PUB_ENABLED
|
|
/*
|
|
start the DDS thread
|
|
*/
|
|
bool AP_DDS_Client::start(void)
|
|
{
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
AP_Param::load_object_from_eeprom(this, var_info);
|
|
|
|
if (enabled == 0) {
|
|
return true;
|
|
}
|
|
|
|
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, "%s Thread create failed", msg_prefix);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// read function triggered at every subscription callback
|
|
void AP_DDS_Client::on_topic_trampoline(uxrSession* uxr_session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length,
|
|
void* args)
|
|
{
|
|
AP_DDS_Client *dds = (AP_DDS_Client *)args;
|
|
dds->on_topic(uxr_session, object_id, request_id, stream_id, ub, length);
|
|
}
|
|
|
|
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)
|
|
{
|
|
/*
|
|
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
|
|
*/
|
|
(void) uxr_session;
|
|
(void) request_id;
|
|
(void) stream_id;
|
|
(void) length;
|
|
switch (object_id.id) {
|
|
#if AP_DDS_JOY_SUB_ENABLED
|
|
case topics[to_underlying(TopicIndex::JOY_SUB)].dr_id.id: {
|
|
const bool success = sensor_msgs_msg_Joy_deserialize_topic(ub, &rx_joy_topic);
|
|
|
|
if (success == false) {
|
|
break;
|
|
}
|
|
|
|
if (rx_joy_topic.axes_size >= 4) {
|
|
const uint32_t t_now = AP_HAL::millis();
|
|
|
|
for (uint8_t i = 0; i < MIN(8U, rx_joy_topic.axes_size); i++) {
|
|
// Ignore channel override if NaN.
|
|
if (std::isnan(rx_joy_topic.axes[i])) {
|
|
// Setting the RC override to 0U releases the channel back to the RC.
|
|
RC_Channels::set_override(i, 0U, t_now);
|
|
} else {
|
|
const uint16_t mapped_data = static_cast<uint16_t>(
|
|
linear_interpolate(rc().channel(i)->get_radio_min(),
|
|
rc().channel(i)->get_radio_max(),
|
|
rx_joy_topic.axes[i],
|
|
-1.0, 1.0));
|
|
RC_Channels::set_override(i, mapped_data, t_now);
|
|
}
|
|
}
|
|
|
|
}
|
|
break;
|
|
}
|
|
#endif // AP_DDS_JOY_SUB_ENABLED
|
|
#if AP_DDS_DYNAMIC_TF_SUB_ENABLED
|
|
case topics[to_underlying(TopicIndex::DYNAMIC_TRANSFORMS_SUB)].dr_id.id: {
|
|
const bool success = tf2_msgs_msg_TFMessage_deserialize_topic(ub, &rx_dynamic_transforms_topic);
|
|
if (success == false) {
|
|
break;
|
|
}
|
|
|
|
if (rx_dynamic_transforms_topic.transforms_size > 0) {
|
|
#if AP_DDS_VISUALODOM_ENABLED
|
|
AP_DDS_External_Odom::handle_external_odom(rx_dynamic_transforms_topic);
|
|
#endif // AP_DDS_VISUALODOM_ENABLED
|
|
|
|
} else {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Received tf2_msgs/TFMessage: TF is empty", msg_prefix);
|
|
}
|
|
break;
|
|
}
|
|
#endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED
|
|
#if AP_DDS_VEL_CTRL_ENABLED
|
|
case topics[to_underlying(TopicIndex::VELOCITY_CONTROL_SUB)].dr_id.id: {
|
|
const bool success = geometry_msgs_msg_TwistStamped_deserialize_topic(ub, &rx_velocity_control_topic);
|
|
if (success == false) {
|
|
break;
|
|
}
|
|
#if AP_EXTERNAL_CONTROL_ENABLED
|
|
if (!AP_DDS_External_Control::handle_velocity_control(rx_velocity_control_topic)) {
|
|
// TODO #23430 handle velocity control failure through rosout, throttled.
|
|
}
|
|
#endif // AP_EXTERNAL_CONTROL_ENABLED
|
|
break;
|
|
}
|
|
#endif // AP_DDS_VEL_CTRL_ENABLED
|
|
#if AP_DDS_GLOBAL_POS_CTRL_ENABLED
|
|
case topics[to_underlying(TopicIndex::GLOBAL_POSITION_SUB)].dr_id.id: {
|
|
const bool success = ardupilot_msgs_msg_GlobalPosition_deserialize_topic(ub, &rx_global_position_control_topic);
|
|
if (success == false) {
|
|
break;
|
|
}
|
|
|
|
#if AP_EXTERNAL_CONTROL_ENABLED
|
|
if (!AP_DDS_External_Control::handle_global_position_control(rx_global_position_control_topic)) {
|
|
// TODO #23430 handle global position control failure through rosout, throttled.
|
|
}
|
|
#endif // AP_EXTERNAL_CONTROL_ENABLED
|
|
break;
|
|
}
|
|
#endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED
|
|
}
|
|
|
|
}
|
|
|
|
/*
|
|
callback on request completion
|
|
*/
|
|
void AP_DDS_Client::on_request_trampoline(uxrSession* uxr_session, uxrObjectId object_id, uint16_t request_id, SampleIdentity* sample_id, ucdrBuffer* ub, uint16_t length, void* args)
|
|
{
|
|
AP_DDS_Client *dds = (AP_DDS_Client *)args;
|
|
dds->on_request(uxr_session, object_id, request_id, sample_id, ub, length);
|
|
}
|
|
|
|
void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, uint16_t request_id, SampleIdentity* sample_id, ucdrBuffer* ub, uint16_t length)
|
|
{
|
|
(void) request_id;
|
|
(void) length;
|
|
switch (object_id.id) {
|
|
#if AP_DDS_ARM_SERVER_ENABLED
|
|
case services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id: {
|
|
ardupilot_msgs_srv_ArmMotors_Request arm_motors_request;
|
|
ardupilot_msgs_srv_ArmMotors_Response arm_motors_response;
|
|
const bool deserialize_success = ardupilot_msgs_srv_ArmMotors_Request_deserialize_topic(ub, &arm_motors_request);
|
|
if (deserialize_success == false) {
|
|
break;
|
|
}
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for %sing received", msg_prefix, arm_motors_request.arm ? "arm" : "disarm");
|
|
#if AP_EXTERNAL_CONTROL_ENABLED
|
|
const bool do_checks = true;
|
|
arm_motors_response.result = arm_motors_request.arm ? AP_DDS_External_Control::arm(AP_Arming::Method::DDS, do_checks) : AP_DDS_External_Control::disarm(AP_Arming::Method::DDS, do_checks);
|
|
if (!arm_motors_response.result) {
|
|
// TODO #23430 handle arm failure through rosout, throttled.
|
|
}
|
|
#endif // AP_EXTERNAL_CONTROL_ENABLED
|
|
|
|
const uxrObjectId replier_id = {
|
|
.id = services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id,
|
|
.type = UXR_REPLIER_ID
|
|
};
|
|
|
|
uint8_t reply_buffer[8] {};
|
|
ucdrBuffer reply_ub;
|
|
|
|
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
|
|
const bool serialize_success = ardupilot_msgs_srv_ArmMotors_Response_serialize_topic(&reply_ub, &arm_motors_response);
|
|
if (serialize_success == false) {
|
|
break;
|
|
}
|
|
|
|
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Arming/Disarming : %s", msg_prefix, arm_motors_response.result ? "SUCCESS" : "FAIL");
|
|
break;
|
|
}
|
|
#endif // AP_DDS_ARM_SERVER_ENABLED
|
|
#if AP_DDS_MODE_SWITCH_SERVER_ENABLED
|
|
case services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id: {
|
|
ardupilot_msgs_srv_ModeSwitch_Request mode_switch_request;
|
|
ardupilot_msgs_srv_ModeSwitch_Response mode_switch_response;
|
|
const bool deserialize_success = ardupilot_msgs_srv_ModeSwitch_Request_deserialize_topic(ub, &mode_switch_request);
|
|
if (deserialize_success == false) {
|
|
break;
|
|
}
|
|
mode_switch_response.status = AP::vehicle()->set_mode(mode_switch_request.mode, ModeReason::DDS_COMMAND);
|
|
mode_switch_response.curr_mode = AP::vehicle()->get_mode();
|
|
|
|
const uxrObjectId replier_id = {
|
|
.id = services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id,
|
|
.type = UXR_REPLIER_ID
|
|
};
|
|
|
|
uint8_t reply_buffer[8] {};
|
|
ucdrBuffer reply_ub;
|
|
|
|
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
|
|
const bool serialize_success = ardupilot_msgs_srv_ModeSwitch_Response_serialize_topic(&reply_ub, &mode_switch_response);
|
|
if (serialize_success == false) {
|
|
break;
|
|
}
|
|
|
|
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Mode Switch : %s", msg_prefix, mode_switch_response.status ? "SUCCESS" : "FAIL");
|
|
break;
|
|
}
|
|
#endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
|
|
#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
|
|
case services[to_underlying(ServiceIndex::TAKEOFF)].rep_id: {
|
|
ardupilot_msgs_srv_Takeoff_Request takeoff_request;
|
|
ardupilot_msgs_srv_Takeoff_Response takeoff_response;
|
|
const bool deserialize_success = ardupilot_msgs_srv_Takeoff_Request_deserialize_topic(ub, &takeoff_request);
|
|
if (deserialize_success == false) {
|
|
break;
|
|
}
|
|
takeoff_response.status = AP::vehicle()->start_takeoff(takeoff_request.alt);
|
|
|
|
const uxrObjectId replier_id = {
|
|
.id = services[to_underlying(ServiceIndex::TAKEOFF)].rep_id,
|
|
.type = UXR_REPLIER_ID
|
|
};
|
|
|
|
uint8_t reply_buffer[8] {};
|
|
ucdrBuffer reply_ub;
|
|
|
|
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
|
|
const bool serialize_success = ardupilot_msgs_srv_Takeoff_Response_serialize_topic(&reply_ub, &takeoff_response);
|
|
if (serialize_success == false) {
|
|
break;
|
|
}
|
|
|
|
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Takeoff : %s", msg_prefix, takeoff_response.status ? "SUCCESS" : "FAIL");
|
|
break;
|
|
}
|
|
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
|
|
#if AP_DDS_ARM_CHECK_SERVER_ENABLED
|
|
case services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id: {
|
|
std_srvs_srv_Trigger_Request prearm_check_request;
|
|
std_srvs_srv_Trigger_Response prearm_check_response;
|
|
const bool deserialize_success = std_srvs_srv_Trigger_Request_deserialize_topic(ub, &prearm_check_request);
|
|
if (deserialize_success == false) {
|
|
break;
|
|
}
|
|
prearm_check_response.success = AP::arming().pre_arm_checks(false);
|
|
STRCPY(prearm_check_response.message, prearm_check_response.success ? "Vehicle is Armable" : "Vehicle is Not Armable");
|
|
|
|
const uxrObjectId replier_id = {
|
|
.id = services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id,
|
|
.type = UXR_REPLIER_ID
|
|
};
|
|
|
|
uint8_t reply_buffer[sizeof(prearm_check_response.message) + 1] {};
|
|
ucdrBuffer reply_ub;
|
|
|
|
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
|
|
const bool serialize_success = std_srvs_srv_Trigger_Response_serialize_topic(&reply_ub, &prearm_check_response);
|
|
if (serialize_success == false) {
|
|
break;
|
|
}
|
|
|
|
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
|
|
break;
|
|
}
|
|
#endif //AP_DDS_ARM_CHECK_SERVER_ENABLED
|
|
#if AP_DDS_PARAMETER_SERVER_ENABLED
|
|
case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: {
|
|
const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request);
|
|
if (deserialize_success == false) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Set Parameters Request : Failed to deserialize request.", msg_prefix);
|
|
break;
|
|
}
|
|
|
|
if (set_parameter_request.parameters_size > 8U) {
|
|
break;
|
|
}
|
|
|
|
// Set parameters and responses for each one requested
|
|
set_parameter_response.results_size = set_parameter_request.parameters_size;
|
|
for (size_t i = 0; i < set_parameter_request.parameters_size; i++) {
|
|
param = set_parameter_request.parameters[i];
|
|
|
|
enum ap_var_type var_type;
|
|
|
|
// set parameter
|
|
AP_Param *vp;
|
|
char param_key[AP_MAX_NAME_SIZE + 1];
|
|
strncpy(param_key, (char *)param.name, AP_MAX_NAME_SIZE);
|
|
param_key[AP_MAX_NAME_SIZE] = 0;
|
|
|
|
// Currently only integer and double value types can be set.
|
|
// The following parameter value types are not handled:
|
|
// bool, string, byte_array, bool_array, integer_array, double_array and string_array
|
|
bool param_isnan = true;
|
|
bool param_isinf = true;
|
|
float param_value;
|
|
switch (param.value.type) {
|
|
case PARAMETER_INTEGER: {
|
|
param_isnan = isnan(param.value.integer_value);
|
|
param_isinf = isinf(param.value.integer_value);
|
|
param_value = float(param.value.integer_value);
|
|
break;
|
|
}
|
|
case PARAMETER_DOUBLE: {
|
|
param_isnan = isnan(param.value.double_value);
|
|
param_isinf = isinf(param.value.double_value);
|
|
param_value = float(param.value.double_value);
|
|
break;
|
|
}
|
|
default: {
|
|
break;
|
|
}
|
|
}
|
|
|
|
// find existing param to get the old value
|
|
uint16_t parameter_flags = 0;
|
|
vp = AP_Param::find(param_key, &var_type, ¶meter_flags);
|
|
if (vp == nullptr || param_isnan || param_isinf) {
|
|
set_parameter_response.results[i].successful = false;
|
|
strncpy(set_parameter_response.results[i].reason, "Parameter not found", sizeof(set_parameter_response.results[i].reason));
|
|
continue;
|
|
}
|
|
|
|
// Add existing parameter checks used in GCS_Param.cpp
|
|
if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) {
|
|
// The user can set BRD_OPTIONS to enable set of internal
|
|
// parameters, for developer testing or unusual use cases
|
|
if (AP_BoardConfig::allow_set_internal_parameters()) {
|
|
parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY;
|
|
}
|
|
}
|
|
|
|
if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) {
|
|
set_parameter_response.results[i].successful = false;
|
|
strncpy(set_parameter_response.results[i].reason, "Parameter is read only",sizeof(set_parameter_response.results[i].reason));
|
|
continue;
|
|
}
|
|
|
|
// Set and save the value if it changed
|
|
bool force_save = vp->set_and_save_by_name_ifchanged(param_key, param_value);
|
|
|
|
if (force_save && (parameter_flags & AP_PARAM_FLAG_ENABLE)) {
|
|
AP_Param::invalidate_count();
|
|
}
|
|
|
|
set_parameter_response.results[i].successful = true;
|
|
strncpy(set_parameter_response.results[i].reason, "Parameter accepted", sizeof(set_parameter_response.results[i].reason));
|
|
}
|
|
|
|
const uxrObjectId replier_id = {
|
|
.id = services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id,
|
|
.type = UXR_REPLIER_ID
|
|
};
|
|
|
|
const uint32_t reply_size = rcl_interfaces_srv_SetParameters_Response_size_of_topic(&set_parameter_response, 0U);
|
|
uint8_t reply_buffer[reply_size];
|
|
memset(reply_buffer, 0, reply_size * sizeof(uint8_t));
|
|
ucdrBuffer reply_ub;
|
|
|
|
ucdr_init_buffer(&reply_ub, reply_buffer, reply_size);
|
|
const bool serialize_success = rcl_interfaces_srv_SetParameters_Response_serialize_topic(&reply_ub, &set_parameter_response);
|
|
if (serialize_success == false) {
|
|
break;
|
|
}
|
|
|
|
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
|
|
bool successful_params = true;
|
|
for (size_t i = 0; i < set_parameter_response.results_size; i++) {
|
|
// Check that all the parameters were set successfully
|
|
successful_params &= set_parameter_response.results[i].successful;
|
|
}
|
|
GCS_SEND_TEXT(successful_params ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Set Parameters Request : %s", msg_prefix, successful_params ? "SUCCESSFUL" : "ONE OR MORE PARAMS FAILED" );
|
|
break;
|
|
}
|
|
case services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id: {
|
|
const bool deserialize_success = rcl_interfaces_srv_GetParameters_Request_deserialize_topic(ub, &get_parameters_request);
|
|
if (deserialize_success == false) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Get Parameters Request : Failed to deserialize request.", msg_prefix);
|
|
break;
|
|
}
|
|
|
|
if (get_parameters_request.names_size > 8U) {
|
|
break;
|
|
}
|
|
|
|
bool successful_read = true;
|
|
get_parameters_response.values_size = get_parameters_request.names_size;
|
|
for (size_t i = 0; i < get_parameters_request.names_size; i++) {
|
|
enum ap_var_type var_type;
|
|
|
|
AP_Param *vp;
|
|
char param_key[AP_MAX_NAME_SIZE + 1];
|
|
strncpy(param_key, (char *)get_parameters_request.names[i], AP_MAX_NAME_SIZE);
|
|
param_key[AP_MAX_NAME_SIZE] = 0;
|
|
|
|
vp = AP_Param::find(param_key, &var_type);
|
|
if (vp == nullptr) {
|
|
get_parameters_response.values[i].type = PARAMETER_NOT_SET;
|
|
successful_read &= false;
|
|
continue;
|
|
}
|
|
|
|
switch (var_type) {
|
|
case AP_PARAM_INT8: {
|
|
get_parameters_response.values[i].type = PARAMETER_INTEGER;
|
|
get_parameters_response.values[i].integer_value = ((AP_Int8 *)vp)->get();
|
|
successful_read &= true;
|
|
break;
|
|
}
|
|
case AP_PARAM_INT16: {
|
|
get_parameters_response.values[i].type = PARAMETER_INTEGER;
|
|
get_parameters_response.values[i].integer_value = ((AP_Int16 *)vp)->get();
|
|
successful_read &= true;
|
|
break;
|
|
}
|
|
case AP_PARAM_INT32: {
|
|
get_parameters_response.values[i].type = PARAMETER_INTEGER;
|
|
get_parameters_response.values[i].integer_value = ((AP_Int32 *)vp)->get();
|
|
successful_read &= true;
|
|
break;
|
|
}
|
|
case AP_PARAM_FLOAT: {
|
|
get_parameters_response.values[i].type = PARAMETER_DOUBLE;
|
|
get_parameters_response.values[i].double_value = vp->cast_to_float(var_type);
|
|
successful_read &= true;
|
|
break;
|
|
}
|
|
default: {
|
|
get_parameters_response.values[i].type = PARAMETER_NOT_SET;
|
|
successful_read &= false;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
const uxrObjectId replier_id = {
|
|
.id = services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id,
|
|
.type = UXR_REPLIER_ID
|
|
};
|
|
|
|
const uint32_t reply_size = rcl_interfaces_srv_GetParameters_Response_size_of_topic(&get_parameters_response, 0U);
|
|
uint8_t reply_buffer[reply_size];
|
|
memset(reply_buffer, 0, reply_size * sizeof(uint8_t));
|
|
ucdrBuffer reply_ub;
|
|
|
|
ucdr_init_buffer(&reply_ub, reply_buffer, reply_size);
|
|
const bool serialize_success = rcl_interfaces_srv_GetParameters_Response_serialize_topic(&reply_ub, &get_parameters_response);
|
|
if (serialize_success == false) {
|
|
break;
|
|
}
|
|
|
|
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
|
|
|
|
GCS_SEND_TEXT(successful_read ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Get Parameters Request : %s", msg_prefix, successful_read ? "SUCCESSFUL" : "ONE OR MORE PARAM NOT FOUND");
|
|
break;
|
|
}
|
|
#endif // AP_DDS_PARAMETER_SERVER_ENABLED
|
|
}
|
|
}
|
|
|
|
/*
|
|
main loop for DDS thread
|
|
*/
|
|
void AP_DDS_Client::main_loop(void)
|
|
{
|
|
if (!init_transport()) {
|
|
return;
|
|
}
|
|
|
|
//! @todo check for request to stop task
|
|
while (true) {
|
|
if (comm == nullptr) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s transport invalid, exiting", msg_prefix);
|
|
return;
|
|
}
|
|
|
|
// check ping
|
|
if (!uxr_ping_agent_attempts(comm, ping_timeout_ms, ping_max_retry)) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s No ping response, exiting", msg_prefix);
|
|
return;
|
|
}
|
|
|
|
// create session
|
|
if (!init_session() || !create()) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Creation Requests failed", msg_prefix);
|
|
return;
|
|
}
|
|
connected = true;
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Initialization passed", msg_prefix);
|
|
|
|
#if AP_DDS_STATIC_TF_PUB_ENABLED
|
|
populate_static_transforms(tx_static_transforms_topic);
|
|
write_static_transforms();
|
|
#endif // AP_DDS_STATIC_TF_PUB_ENABLED
|
|
|
|
uint64_t last_ping_ms{0};
|
|
uint8_t num_pings_missed{0};
|
|
bool had_ping_reply{false};
|
|
while (connected) {
|
|
hal.scheduler->delay(1);
|
|
|
|
// publish topics
|
|
update();
|
|
|
|
// check ping response
|
|
if (session.on_pong_flag == PONG_IN_SESSION_STATUS) {
|
|
had_ping_reply = true;
|
|
}
|
|
|
|
const auto cur_time_ms = AP_HAL::millis64();
|
|
if (cur_time_ms - last_ping_ms > DELAY_PING_MS) {
|
|
last_ping_ms = cur_time_ms;
|
|
|
|
if (had_ping_reply) {
|
|
num_pings_missed = 0;
|
|
|
|
} else {
|
|
++num_pings_missed;
|
|
}
|
|
|
|
const int ping_agent_timeout_ms{0};
|
|
const uint8_t ping_agent_attempts{1};
|
|
uxr_ping_agent_session(&session, ping_agent_timeout_ms, ping_agent_attempts);
|
|
|
|
had_ping_reply = false;
|
|
}
|
|
|
|
if (num_pings_missed > 2) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR,
|
|
"%s No ping response, disconnecting", msg_prefix);
|
|
connected = false;
|
|
}
|
|
}
|
|
|
|
// delete session if connected
|
|
if (connected) {
|
|
uxr_delete_session(&session);
|
|
}
|
|
}
|
|
}
|
|
|
|
bool AP_DDS_Client::init_transport()
|
|
{
|
|
// serial init will fail if the SERIALn_PROTOCOL is not setup
|
|
bool initTransportStatus = ddsSerialInit();
|
|
is_using_serial = initTransportStatus;
|
|
|
|
#if AP_DDS_UDP_ENABLED
|
|
// fallback to UDP if available
|
|
if (!initTransportStatus) {
|
|
initTransportStatus = ddsUdpInit();
|
|
}
|
|
#endif
|
|
|
|
if (!initTransportStatus) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Transport initialization failed", msg_prefix);
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool AP_DDS_Client::init_session()
|
|
{
|
|
// init session
|
|
uxr_init_session(&session, comm, key);
|
|
|
|
// Register topic callbacks
|
|
uxr_set_topic_callback(&session, AP_DDS_Client::on_topic_trampoline, this);
|
|
|
|
// ROS-2 Service : Register service request callbacks
|
|
uxr_set_request_callback(&session, AP_DDS_Client::on_request_trampoline, this);
|
|
|
|
while (!uxr_create_session(&session)) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Initialization waiting...", msg_prefix);
|
|
hal.scheduler->delay(1000);
|
|
}
|
|
|
|
// setup reliable stream buffers
|
|
input_reliable_stream = NEW_NOTHROW uint8_t[DDS_BUFFER_SIZE];
|
|
output_reliable_stream = NEW_NOTHROW uint8_t[DDS_BUFFER_SIZE];
|
|
if (input_reliable_stream == nullptr || output_reliable_stream == nullptr) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Allocation failed", msg_prefix);
|
|
return false;
|
|
}
|
|
|
|
reliable_in = uxr_create_input_reliable_stream(&session, input_reliable_stream, DDS_BUFFER_SIZE, DDS_STREAM_HISTORY);
|
|
reliable_out = uxr_create_output_reliable_stream(&session, output_reliable_stream, DDS_BUFFER_SIZE, DDS_STREAM_HISTORY);
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Init complete", msg_prefix);
|
|
|
|
return true;
|
|
}
|
|
|
|
bool AP_DDS_Client::create()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
|
|
// Participant
|
|
const uxrObjectId participant_id = {
|
|
.id = 0x01,
|
|
.type = UXR_PARTICIPANT_ID
|
|
};
|
|
const char* participant_name = AP_DDS_PARTICIPANT_NAME;
|
|
const auto participant_req_id = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id,
|
|
static_cast<uint16_t>(domain_id), participant_name, UXR_REPLACE);
|
|
|
|
//Participant requests
|
|
constexpr uint8_t nRequestsParticipant = 1;
|
|
const uint16_t requestsParticipant[nRequestsParticipant] = {participant_req_id};
|
|
|
|
constexpr uint16_t maxTimeMsPerRequestMs = 500;
|
|
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, "%s Participant session request failure", msg_prefix);
|
|
// TODO add a failure log message sharing the status results
|
|
return false;
|
|
}
|
|
|
|
for (uint16_t i = 0 ; i < ARRAY_SIZE(topics); i++) {
|
|
// Topic
|
|
const uxrObjectId topic_id = {
|
|
.id = topics[i].topic_id,
|
|
.type = UXR_TOPIC_ID
|
|
};
|
|
const auto topic_req_id = uxr_buffer_create_topic_bin(&session, reliable_out, topic_id,
|
|
participant_id, topics[i].topic_name, topics[i].type_name, UXR_REPLACE);
|
|
|
|
// Status requests
|
|
constexpr uint8_t nRequests = 3;
|
|
uint16_t requests[nRequests];
|
|
constexpr uint16_t requestTimeoutMs = nRequests * maxTimeMsPerRequestMs;
|
|
uint8_t status[nRequests];
|
|
|
|
if (topics[i].topic_rw == Topic_rw::DataWriter) {
|
|
// Publisher
|
|
const uxrObjectId pub_id = {
|
|
.id = topics[i].pub_id,
|
|
.type = UXR_PUBLISHER_ID
|
|
};
|
|
const auto pub_req_id = uxr_buffer_create_publisher_bin(&session, reliable_out, pub_id,
|
|
participant_id, UXR_REPLACE);
|
|
|
|
// Data Writer
|
|
const auto dwriter_req_id = uxr_buffer_create_datawriter_bin(&session, reliable_out, topics[i].dw_id,
|
|
pub_id, topic_id, topics[i].qos, 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, "%s Topic/Pub/Writer session request failure for index '%u'", msg_prefix, i);
|
|
for (uint8_t s = 0 ; s < nRequests; s++) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Status '%d' result '%u'", msg_prefix, s, status[s]);
|
|
}
|
|
// TODO add a failure log message sharing the status results
|
|
return false;
|
|
} else {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Topic/Pub/Writer session pass for index '%u'", msg_prefix, i);
|
|
}
|
|
} else if (topics[i].topic_rw == Topic_rw::DataReader) {
|
|
// Subscriber
|
|
const uxrObjectId sub_id = {
|
|
.id = topics[i].sub_id,
|
|
.type = UXR_SUBSCRIBER_ID
|
|
};
|
|
const auto sub_req_id = uxr_buffer_create_subscriber_bin(&session, reliable_out, sub_id,
|
|
participant_id, UXR_REPLACE);
|
|
|
|
// Data Reader
|
|
const auto dreader_req_id = uxr_buffer_create_datareader_bin(&session, reliable_out, topics[i].dr_id,
|
|
sub_id, topic_id, topics[i].qos, 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, "%s Topic/Sub/Reader session request failure for index '%u'", msg_prefix, i);
|
|
for (uint8_t s = 0 ; s < nRequests; s++) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Status '%d' result '%u'", msg_prefix, s, status[s]);
|
|
}
|
|
// TODO add a failure log message sharing the status results
|
|
return false;
|
|
} else {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Topic/Sub/Reader session pass for index '%u'", msg_prefix, i);
|
|
uxr_buffer_request_data(&session, reliable_out, topics[i].dr_id, reliable_in, &delivery_control);
|
|
}
|
|
}
|
|
}
|
|
|
|
// ROS-2 Service : else case for service requests
|
|
|
|
for (uint16_t i = 0; i < ARRAY_SIZE(services); i++) {
|
|
|
|
constexpr uint16_t requestTimeoutMs = maxTimeMsPerRequestMs;
|
|
|
|
if (services[i].service_rr == Service_rr::Replier) {
|
|
const uxrObjectId rep_id = {
|
|
.id = services[i].rep_id,
|
|
.type = UXR_REPLIER_ID
|
|
};
|
|
const auto replier_req_id = uxr_buffer_create_replier_bin(&session, reliable_out, rep_id,
|
|
participant_id, services[i].service_name, services[i].request_type, services[i].reply_type,
|
|
services[i].request_topic_name, services[i].reply_topic_name, services[i].qos, UXR_REPLACE);
|
|
|
|
uint16_t request = replier_req_id;
|
|
uint8_t status;
|
|
|
|
if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, &request, &status, 1)) {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Service/Replier session request failure for index '%u'", msg_prefix, i);
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Status result '%u'", msg_prefix, status);
|
|
// TODO add a failure log message sharing the status results
|
|
return false;
|
|
} else {
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Service/Replier session pass for index '%u'", msg_prefix, i);
|
|
uxr_buffer_request_data(&session, reliable_out, rep_id, reliable_in, &delivery_control);
|
|
}
|
|
|
|
} else if (services[i].service_rr == Service_rr::Requester) {
|
|
// TODO : Add Similar Code for Requester Profile
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void AP_DDS_Client::write_time_topic()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = builtin_interfaces_msg_Time_size_of_topic(&time_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::TIME_PUB)].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");
|
|
}
|
|
}
|
|
}
|
|
|
|
#if AP_DDS_NAVSATFIX_PUB_ENABLED
|
|
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[to_underlying(TopicIndex::NAV_SAT_FIX_PUB)].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");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_NAVSATFIX_PUB_ENABLED
|
|
|
|
#if AP_DDS_STATIC_TF_PUB_ENABLED
|
|
void AP_DDS_Client::write_static_transforms()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = tf2_msgs_msg_TFMessage_size_of_topic(&tx_static_transforms_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::STATIC_TRANSFORMS_PUB)].dw_id, &ub, topic_size);
|
|
const bool success = tf2_msgs_msg_TFMessage_serialize_topic(&ub, &tx_static_transforms_topic);
|
|
if (!success) {
|
|
// TODO sometimes serialization fails on bootup. Determine why.
|
|
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_STATIC_TF_PUB_ENABLED
|
|
|
|
#if AP_DDS_BATTERY_STATE_PUB_ENABLED
|
|
void AP_DDS_Client::write_battery_state_topic()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = sensor_msgs_msg_BatteryState_size_of_topic(&battery_state_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::BATTERY_STATE_PUB)].dw_id, &ub, topic_size);
|
|
const bool success = sensor_msgs_msg_BatteryState_serialize_topic(&ub, &battery_state_topic);
|
|
if (!success) {
|
|
// TODO sometimes serialization fails on bootup. Determine why.
|
|
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
|
|
|
|
#if AP_DDS_LOCAL_POSE_PUB_ENABLED
|
|
void AP_DDS_Client::write_local_pose_topic()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = geometry_msgs_msg_PoseStamped_size_of_topic(&local_pose_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::LOCAL_POSE_PUB)].dw_id, &ub, topic_size);
|
|
const bool success = geometry_msgs_msg_PoseStamped_serialize_topic(&ub, &local_pose_topic);
|
|
if (!success) {
|
|
// TODO sometimes serialization fails on bootup. Determine why.
|
|
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
|
|
|
|
#if AP_DDS_LOCAL_VEL_PUB_ENABLED
|
|
void AP_DDS_Client::write_tx_local_velocity_topic()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = geometry_msgs_msg_TwistStamped_size_of_topic(&tx_local_velocity_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::LOCAL_VELOCITY_PUB)].dw_id, &ub, topic_size);
|
|
const bool success = geometry_msgs_msg_TwistStamped_serialize_topic(&ub, &tx_local_velocity_topic);
|
|
if (!success) {
|
|
// TODO sometimes serialization fails on bootup. Determine why.
|
|
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
|
|
#if AP_DDS_AIRSPEED_PUB_ENABLED
|
|
void AP_DDS_Client::write_tx_local_airspeed_topic()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = geometry_msgs_msg_Vector3Stamped_size_of_topic(&tx_local_airspeed_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::LOCAL_AIRSPEED_PUB)].dw_id, &ub, topic_size);
|
|
const bool success = geometry_msgs_msg_Vector3Stamped_serialize_topic(&ub, &tx_local_airspeed_topic);
|
|
if (!success) {
|
|
// TODO sometimes serialization fails on bootup. Determine why.
|
|
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_AIRSPEED_PUB_ENABLED
|
|
#if AP_DDS_IMU_PUB_ENABLED
|
|
void AP_DDS_Client::write_imu_topic()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = sensor_msgs_msg_Imu_size_of_topic(&imu_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::IMU_PUB)].dw_id, &ub, topic_size);
|
|
const bool success = sensor_msgs_msg_Imu_serialize_topic(&ub, &imu_topic);
|
|
if (!success) {
|
|
// TODO sometimes serialization fails on bootup. Determine why.
|
|
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_IMU_PUB_ENABLED
|
|
|
|
#if AP_DDS_GEOPOSE_PUB_ENABLED
|
|
void AP_DDS_Client::write_geo_pose_topic()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = geographic_msgs_msg_GeoPoseStamped_size_of_topic(&geo_pose_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GEOPOSE_PUB)].dw_id, &ub, topic_size);
|
|
const bool success = geographic_msgs_msg_GeoPoseStamped_serialize_topic(&ub, &geo_pose_topic);
|
|
if (!success) {
|
|
// TODO sometimes serialization fails on bootup. Determine why.
|
|
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_GEOPOSE_PUB_ENABLED
|
|
|
|
#if AP_DDS_CLOCK_PUB_ENABLED
|
|
void AP_DDS_Client::write_clock_topic()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = rosgraph_msgs_msg_Clock_size_of_topic(&clock_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::CLOCK_PUB)].dw_id, &ub, topic_size);
|
|
const bool success = rosgraph_msgs_msg_Clock_serialize_topic(&ub, &clock_topic);
|
|
if (!success) {
|
|
// TODO sometimes serialization fails on bootup. Determine why.
|
|
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_CLOCK_PUB_ENABLED
|
|
|
|
#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
|
|
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");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
|
|
|
|
#if AP_DDS_GOAL_PUB_ENABLED
|
|
void AP_DDS_Client::write_goal_topic()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = geographic_msgs_msg_GeoPointStamped_size_of_topic(&goal_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GOAL_PUB)].dw_id, &ub, topic_size);
|
|
const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &goal_topic);
|
|
if (!success) {
|
|
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_GOAL_PUB_ENABLED
|
|
|
|
#if AP_DDS_STATUS_PUB_ENABLED
|
|
void AP_DDS_Client::write_status_topic()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
if (connected) {
|
|
ucdrBuffer ub {};
|
|
const uint32_t topic_size = ardupilot_msgs_msg_Status_size_of_topic(&status_topic, 0);
|
|
uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::STATUS_PUB)].dw_id, &ub, topic_size);
|
|
const bool success = ardupilot_msgs_msg_Status_serialize_topic(&ub, &status_topic);
|
|
if (!success) {
|
|
// TODO sometimes serialization fails on bootup. Determine why.
|
|
// AP_HAL::panic("FATAL: DDS_Client failed to serialize");
|
|
}
|
|
}
|
|
}
|
|
#endif // AP_DDS_STATUS_PUB_ENABLED
|
|
|
|
void AP_DDS_Client::update()
|
|
{
|
|
WITH_SEMAPHORE(csem);
|
|
const auto cur_time_ms = AP_HAL::millis64();
|
|
|
|
#if AP_DDS_TIME_PUB_ENABLED
|
|
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();
|
|
}
|
|
#endif // AP_DDS_TIME_PUB_ENABLED
|
|
#if AP_DDS_NAVSATFIX_PUB_ENABLED
|
|
constexpr uint8_t gps_instance = 0;
|
|
if (update_topic(nav_sat_fix_topic, gps_instance)) {
|
|
write_nav_sat_fix_topic();
|
|
}
|
|
#endif // AP_DDS_NAVSATFIX_PUB_ENABLED
|
|
#if AP_DDS_BATTERY_STATE_PUB_ENABLED
|
|
if (cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) {
|
|
for (uint8_t battery_instance = 0; battery_instance < AP_BATT_MONITOR_MAX_INSTANCES; battery_instance++) {
|
|
update_topic(battery_state_topic, battery_instance);
|
|
if (battery_state_topic.present) {
|
|
write_battery_state_topic();
|
|
}
|
|
}
|
|
last_battery_state_time_ms = cur_time_ms;
|
|
}
|
|
#endif // AP_DDS_BATTERY_STATE_PUB_ENABLED
|
|
#if AP_DDS_LOCAL_POSE_PUB_ENABLED
|
|
if (cur_time_ms - last_local_pose_time_ms > DELAY_LOCAL_POSE_TOPIC_MS) {
|
|
update_topic(local_pose_topic);
|
|
last_local_pose_time_ms = cur_time_ms;
|
|
write_local_pose_topic();
|
|
}
|
|
#endif // AP_DDS_LOCAL_POSE_PUB_ENABLED
|
|
#if AP_DDS_LOCAL_VEL_PUB_ENABLED
|
|
if (cur_time_ms - last_local_velocity_time_ms > DELAY_LOCAL_VELOCITY_TOPIC_MS) {
|
|
update_topic(tx_local_velocity_topic);
|
|
last_local_velocity_time_ms = cur_time_ms;
|
|
write_tx_local_velocity_topic();
|
|
}
|
|
#endif // AP_DDS_LOCAL_VEL_PUB_ENABLED
|
|
#if AP_DDS_AIRSPEED_PUB_ENABLED
|
|
if (cur_time_ms - last_airspeed_time_ms > DELAY_AIRSPEED_TOPIC_MS) {
|
|
last_airspeed_time_ms = cur_time_ms;
|
|
if (update_topic(tx_local_airspeed_topic)) {
|
|
write_tx_local_airspeed_topic();
|
|
}
|
|
}
|
|
#endif // AP_DDS_AIRSPEED_PUB_ENABLED
|
|
#if AP_DDS_IMU_PUB_ENABLED
|
|
if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) {
|
|
update_topic(imu_topic);
|
|
last_imu_time_ms = cur_time_ms;
|
|
write_imu_topic();
|
|
}
|
|
#endif // AP_DDS_IMU_PUB_ENABLED
|
|
#if AP_DDS_GEOPOSE_PUB_ENABLED
|
|
if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) {
|
|
update_topic(geo_pose_topic);
|
|
last_geo_pose_time_ms = cur_time_ms;
|
|
write_geo_pose_topic();
|
|
}
|
|
#endif // AP_DDS_GEOPOSE_PUB_ENABLED
|
|
#if AP_DDS_CLOCK_PUB_ENABLED
|
|
if (cur_time_ms - last_clock_time_ms > DELAY_CLOCK_TOPIC_MS) {
|
|
update_topic(clock_topic);
|
|
last_clock_time_ms = cur_time_ms;
|
|
write_clock_topic();
|
|
}
|
|
#endif // AP_DDS_CLOCK_PUB_ENABLED
|
|
#if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
|
|
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();
|
|
}
|
|
#endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
|
|
#if AP_DDS_GOAL_PUB_ENABLED
|
|
if (cur_time_ms - last_goal_time_ms > DELAY_GOAL_TOPIC_MS) {
|
|
if (update_topic_goal(goal_topic)) {
|
|
write_goal_topic();
|
|
}
|
|
last_goal_time_ms = cur_time_ms;
|
|
}
|
|
#endif // AP_DDS_GOAL_PUB_ENABLED
|
|
#if AP_DDS_STATUS_PUB_ENABLED
|
|
if (cur_time_ms - last_status_check_time_ms > DELAY_STATUS_TOPIC_MS) {
|
|
if (update_topic(status_topic)) {
|
|
write_status_topic();
|
|
}
|
|
last_status_check_time_ms = cur_time_ms;
|
|
}
|
|
#endif // AP_DDS_STATUS_PUB_ENABLED
|
|
|
|
status_ok = uxr_run_session_time(&session, 1);
|
|
}
|
|
|
|
#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 != HAL_BOARD_SITL
|
|
|
|
#endif // AP_DDS_ENABLED
|