Skip to content

Commit

Permalink
AP_DDS: use common code from AP_ROS
Browse files Browse the repository at this point in the history 
Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: use common code from AP_ROS for external control

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: use common code from AP_ROS for time type conversion

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: add mutable versions of accessor templates

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: use common code from AP_ROS for published topics

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: consolidate code for publishing nav sat fix

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: delete code moved to AP_ROS

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: rename mutable transforms

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: consolidate code for publishing static transforms

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: move template specialisation definitions to cpp to prevent duplicate symbols

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: fix formatting

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: add accessor templates for battery state

Signed-off-by: Rhys Mainwaring <[email protected]>

AP_DDS: consolidate code for publishing battery state

Signed-off-by: Rhys Mainwaring <[email protected]>
  • Loading branch information
@srmainwaring
srmainwaring committed Oct 11, 2023
1 parent ff5e6d0 commit 6560d16
Show file tree
Hide file tree
Showing 5 changed files with 122 additions and 396 deletions.
316 changes: 14 additions & 302 deletions libraries/AP_DDS/AP_DDS_Client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,19 @@
#include <AP_Arming/AP_Arming.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_ExternalControl/AP_ExternalControl_config.h>

#include <AP_ROS/AP_ROS_Client.h>

#if AP_EXTERNAL_CONTROL_ENABLED
#include "AP_DDS_ExternalControl.h"
#endif
#include "AP_DDS_Frames.h"
// #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"
#include "AP_DDS_Type_Conversions.h"

// Enable DDS at runtime by default
static constexpr uint8_t ENABLED_BY_DEFAULT = 1;
Expand Down Expand Up @@ -65,335 +69,42 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] {

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;

AP_ROS_Client::update_time(msg);
}

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; };

// assert(instance >= GPS_MAX_RECEIVERS);
if (instance >= GPS_MAX_RECEIVERS) {
return false;
}

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);
strcpy(msg.header.frame_id, WGS_84_FRAME_ID);
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;
return AP_ROS_Client::update_nav_sat_fix(msg, instance, last_nav_sat_fix_time_ms);
}

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];

msg.transforms_size++;
}

AP_ROS_Client::update_static_transforms(msg);
}

void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance)
{
if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) {
return;
}

update_topic(msg.header.stamp);
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 (current < 0.0) {
msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING
} else if (current > 0.0) {
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 uint16_t* cellVoltages = battery.get_cell_voltages(instance).cells;
std::copy(cellVoltages, cellVoltages + AP_BATT_MONITOR_CELLS_MAX, msg.cell_voltage);
}
AP_ROS_Client::update_battery_state(msg, instance);
}

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 convertion) 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];
}
AP_ROS_Client::update_pose_stamped(msg);
}

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];
AP_ROS_Client::update_twist_stamped(msg);
}

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;
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 convertion) 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];
}
AP_ROS_Client::update_geopose_stamped(msg);
}

void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
{
update_topic(msg.clock);
AP_ROS_Client::update_clock(msg);
}

/*
Expand Down Expand Up @@ -839,6 +550,7 @@ void AP_DDS_Client::write_battery_state_topic()
}
}
}

void AP_DDS_Client::write_local_pose_topic()
{
WITH_SEMAPHORE(csem);
Expand Down
Loading

0 comments on commit 6560d16

Please sign in to comment.