Skip to content

Commit

Permalink
changed frame conversion func name; add 3x3 cov matrix frame conversi…
Browse files Browse the repository at this point in the history 
…on; general doxygen comment cleanup
  • Loading branch information
@TSC21
TSC21 committed Jun 27, 2015
1 parent 6cce473 commit 37d325e
Show file tree
Hide file tree
Showing 14 changed files with 235 additions and 139 deletions.
47 changes: 28 additions & 19 deletions mavros/include/mavros/mavros_uas.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -297,35 +297,20 @@ class UAS {
*/
static tf::Vector3 sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation);

/**
* @brief Function to convert position values from ENU to NED frames and vice-versa
*/
static tf::Vector3 convert_position(float _x, float _y, float _z);

/**
* @brief Function to convert velocity values from ENU to NED frames and vice-versa
*/
static tf::Vector3 convert_velocity(float _vx, float _vy, float _vz);

/**
* @brief Function to convert acceleration values from ENU to NED frames and vice-versa
*/
static tf::Vector3 convert_accel(float _ax, float _ay, float _az);

/**
* @brief Function to convert general XYZ values from ENU to NED frames and vice-versa
*/
static tf::Vector3 convert_general_xyz(float _x, float _y, float _z);
static tf::Vector3 transform_frame_general_xyz(float _x, float _y, float _z);

/**
* @brief Function to convert attitude quaternion values from ENU to NED frames and vice-versa
*/
static tf::Quaternion convert_attitude_q(tf::Quaternion qo);
static tf::Quaternion transform_frame_attitude_q(tf::Quaternion qo);

/**
* @brief Function to convert attitude euler angles values from ENU to NED frames and vice-versa
*/
static tf::Vector3 convert_attitude_rpy(float _roll, float _pitch, float _yaw);
static tf::Vector3 transform_frame_attitude_rpy(float _roll, float _pitch, float _yaw);

/**
* @brief Function to convert full 6D pose covariance matrix values from ENU to NED frames and vice-versa
Expand All @@ -349,7 +334,31 @@ class UAS {
*
* C' = R * C * R^T
*/
static std::array<float, 36> convert_covariance_pose6x6(std::array<float, 36> _covariance);
static std::array<float, 36> transform_frame_covariance_pose6x6(std::array<float, 36> _covariance);

/**
* @brief Function to convert position or attitude RPY covariance matrix values from ENU to NED frames and vice-versa
* @details Matrix formats
*
* Pos_Cov_matrix = |var_x cov_xy cov_xz |
* |cov_yx var_y cov_yz |
* |cov_zx cov_zy var_z |
*
* Att_Cov_matrix = |var_Z cov_ZY cov_ZX |
* |cov_YZ var_Y cov_YX |
* |cov_XZ cov_XY var_X |
*
* Note that for ROS<->ENU frame transformations, the rotation matrix is the same for position and attitude.
*
* Rot_matrix = | 1 0 0 |
* | 0 -1 0 |
* | 0 0 -1 |
*
* Compute Covariance matrix in another frame:
*
* C' = R * C * R^T
*/
static std::array<float, 9> transform_frame_covariance_general3x3(std::array<float, 9> _covariance);

private:
std::recursive_mutex mutex;
Expand Down
46 changes: 21 additions & 25 deletions mavros/src/lib/uas_frame_conversions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,49 +21,28 @@ const double PI = boost::math::constants::pi<double>();

using namespace mavros;

static tf::Vector3 convert_position(float _x, float _y, float _z){
static tf::Vector3 transform_frame_general_xyz(float _x, float _y, float _z){
float x = _x;
float y = -_y;
float z = -_z;
return tf::Vector3(x, y, z);
};

static tf::Vector3 convert_velocity(float _vx, float _vy, float _vz){
float vx = _vx;
float vy = -_vy;
float vz = -_vz;
return tf::Vector3(vx, vy, vz);
};

static tf::Vector3 convert_accel(float _ax, float _ay, float _az){
float ax = _ax;
float ay = -_ay;
float az = -_az;
return tf::Vector3(ax, ay, az);
};

static tf::Vector3 convert_general_xyz(float _x, float _y, float _z){
float x = _x;
float y = -_y;
float z = -_z;
return tf::Vector3(x, y, z);
};

static tf::Quaternion convert_attitude_q(tf::Quaternion qo){
static tf::Quaternion transform_frame_attitude_q(tf::Quaternion qo){
double roll = PI, pitch = 0.0 , yaw = 0.0f;
tf::Quaternion qr = tf::createQuaternionFromRPY(roll, pitch, yaw);
tf::Quaternion qt = qo * qr;
return qt;
};

static tf::Vector3 convert_attitude_rpy(float _roll, float _pitch, float _yaw){
static tf::Vector3 transform_frame_attitude_rpy(float _roll, float _pitch, float _yaw){
float roll = _roll + PI;
float pitch = _pitch;
float yaw = _yaw;
return tf::Vector3(roll, pitch, yaw);
};

static std::array<float, 36> convert_covariance_pose6x6(std::array<float, 36> _covariance){
static std::array<float, 36> transform_frame_covariance_pose6x6(std::array<float, 36> _covariance){

std::array<float, 36> rotation = {1 ,0,0,0,0,0,
0,-1,0,0,0,0,
Expand All @@ -82,3 +61,20 @@ static std::array<float, 36> convert_covariance_pose6x6(std::array<float, 36> _c

return covariance;
};

static std::array<float, 9> transform_frame_covariance_general3x3(std::array<float, 9> _covariance){

std::array<float, 9> rotation = {1 ,0,0,
0,-1,0,
0,0,-1};

std::array<float, 9> covariance;
std::array<float, 9> temp; // temporary matrix = R * C

// The rotation matrix in this case is a diagonal matrix so R = R^T

std::transform(rotation.begin()+1, rotation.end(), _covariance.begin()+1, temp.begin(), std::multiplies<float>());
std::transform(temp.begin()+1, temp.end(), rotation.begin()+1, covariance.begin(), std::multiplies<float>());

return covariance;
};
8 changes: 6 additions & 2 deletions mavros/src/plugins/global_position.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ class GlobalPositionPlugin : public MavRosPlugin {
double northing, easting;
std::string zone;

/** Adapted from gps_umd ROS package @a http://wiki.ros.org/gps_umd
/** @note Adapted from gps_umd ROS package @a http://wiki.ros.org/gps_umd
* Author: Ken Tossell <ken AT tossell DOT net>
*/
UTM::LLtoUTM(gp_fix->latitude, gp_fix->longitude, northing, easting, zone);
Expand Down Expand Up @@ -189,7 +189,11 @@ class GlobalPositionPlugin : public MavRosPlugin {

if (send_tf) {
tf::Transform transform;
auto position = UAS::convert_position(pose_cov->pose.pose.position.x,

/**
* @note NED->ENU frame conversion
*/
auto position = UAS::transform_frame_general_xyz(pose_cov->pose.pose.position.x,
pose_cov->pose.pose.position.y,
pose_cov->pose.pose.position.z);

Expand Down
62 changes: 42 additions & 20 deletions mavros/src/plugins/imu_pub.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,12 +178,14 @@ class IMUPubPlugin : public MavRosPlugin {

auto imu_msg = boost::make_shared<sensor_msgs::Imu>();

// NED -> ENU (body-fixed)
tf::Vector3 attitude = UAS::convert_attitude_rpy(att.roll, att.pitch, att.yaw);
/**
* @note NED->ENU frame conversion
*/
tf::Vector3 attitude = UAS::transform_frame_attitude_rpy(att.roll, att.pitch, att.yaw);
tf::Quaternion orientation = tf::createQuaternionFromRPY(
attitude.x(), attitude.y(), attitude.z());

tf::Vector3 attitude_s = UAS::convert_attitude_rpy(att.rollspeed, att.pitchspeed, att.yawspeed);
tf::Vector3 attitude_s = UAS::transform_frame_attitude_rpy(att.rollspeed, att.pitchspeed, att.yawspeed);
fill_imu_msg_attitude(imu_msg, orientation,
attitude_s.x(), attitude_s.y(), attitude_s.z());

Expand All @@ -207,11 +209,13 @@ class IMUPubPlugin : public MavRosPlugin {

auto imu_msg = boost::make_shared<sensor_msgs::Imu>();

// PX4 NED -> ROS ENU (body-fixed)
/**
* @note NED->ENU frame conversion
*/
tf::Quaternion orientation(att_q.q1, att_q.q2, att_q.q3, att_q.q4);
tf::Quaternion qo = UAS::convert_attitude_q(orientation);
tf::Quaternion qo = UAS::transform_frame_attitude_q(orientation);

tf::Vector3 attitude_s = UAS::convert_attitude_rpy(att_q.rollspeed, att_q.pitchspeed, att_q.yawspeed);
tf::Vector3 attitude_s = UAS::transform_frame_attitude_rpy(att_q.rollspeed, att_q.pitchspeed, att_q.yawspeed);
fill_imu_msg_attitude(imu_msg, orientation,
attitude_s.x(), attitude_s.y(), attitude_s.z());

Expand Down Expand Up @@ -240,8 +244,11 @@ class IMUPubPlugin : public MavRosPlugin {
if (imu_hr.fields_updated & ((7 << 3) | (7 << 0))) {
auto imu_msg = boost::make_shared<sensor_msgs::Imu>();

auto hr_imu_gyro = UAS::convert_general_xyz(imu_hr.xgyro, imu_hr.ygyro, imu_hr.zgyro);
auto hr_imu_acc = UAS::convert_general_xyz(imu_hr.xacc, imu_hr.yacc, imu_hr.zacc);
/**
* @note NED->ENU frame conversion
*/
auto hr_imu_gyro = UAS::transform_frame_general_xyz(imu_hr.xgyro, imu_hr.ygyro, imu_hr.zgyro);
auto hr_imu_acc = UAS::transform_frame_general_xyz(imu_hr.xacc, imu_hr.yacc, imu_hr.zacc);

fill_imu_msg_raw(imu_msg,
hr_imu_gyro.x(), hr_imu_gyro.y(), hr_imu_gyro.z(),
Expand All @@ -254,8 +261,10 @@ class IMUPubPlugin : public MavRosPlugin {
if (imu_hr.fields_updated & (7 << 6)) {
auto magn_msg = boost::make_shared<sensor_msgs::MagneticField>();

// Convert from local NED plane to ENU
auto mag_field = UAS::convert_general_xyz(imu_hr.xmag, imu_hr.ymag, imu_hr.zmag);
/**
* @note NED->ENU frame conversion
*/
auto mag_field = UAS::transform_frame_general_xyz(imu_hr.xmag, imu_hr.ymag, imu_hr.zmag);

magn_msg->magnetic_field.x = mag_field.x() * MILLIT_TO_TESLA;
magn_msg->magnetic_field.y = mag_field.y() * MILLIT_TO_TESLA;
Expand Down Expand Up @@ -296,9 +305,15 @@ class IMUPubPlugin : public MavRosPlugin {
header.stamp = uas->synchronise_stamp(imu_raw.time_usec);
header.frame_id = frame_id;

/* NOTE: APM send SCALED_IMU data as RAW_IMU */
auto raw_imu_gyro = UAS::convert_general_xyz(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro);
auto raw_imu_acc = UAS::convert_general_xyz(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
/**
* @note APM send SCALED_IMU data as RAW_IMU
*/

/**
* @note NED->ENU frame conversion
*/
auto raw_imu_gyro = UAS::transform_frame_general_xyz(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro);
auto raw_imu_acc = UAS::transform_frame_general_xyz(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);

fill_imu_msg_raw(imu_msg,
raw_imu_gyro.x() * MILLIRS_TO_RADSEC,
Expand All @@ -321,9 +336,11 @@ class IMUPubPlugin : public MavRosPlugin {
/* -*- magnetic vector -*- */
auto magn_msg = boost::make_shared<sensor_msgs::MagneticField>();

// Convert from local NED plane to ENU
auto mag_field = UAS::convert_general_xyz(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag);

/**
* @note NED->ENU frame conversion
*/
auto mag_field = UAS::transform_frame_general_xyz(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag);

magn_msg->magnetic_field.x = mag_field.x() * MILLIT_TO_TESLA;
magn_msg->magnetic_field.y = mag_field.y() * MILLIT_TO_TESLA;
magn_msg->magnetic_field.z = mag_field.z() * MILLIT_TO_TESLA;
Expand All @@ -348,8 +365,11 @@ class IMUPubPlugin : public MavRosPlugin {
std_msgs::Header header;
header.stamp = uas->synchronise_stamp(imu_raw.time_boot_ms);

auto raw_imu_gyro = UAS::convert_general_xyz(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro);
auto raw_imu_acc = UAS::convert_general_xyz(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
/**
* @note NED->ENU frame conversion
*/
auto raw_imu_gyro = UAS::transform_frame_general_xyz(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro);
auto raw_imu_acc = UAS::transform_frame_general_xyz(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);

fill_imu_msg_raw(imu_msg,
raw_imu_gyro.x() * MILLIRS_TO_RADSEC,
Expand All @@ -365,8 +385,10 @@ class IMUPubPlugin : public MavRosPlugin {
/* -*- magnetic vector -*- */
auto magn_msg = boost::make_shared<sensor_msgs::MagneticField>();

// Convert from local NED plane to ENU
auto mag_field = UAS::convert_general_xyz(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag);
/**
* @note NED->ENU frame conversion
*/
auto mag_field = UAS::transform_frame_general_xyz(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag);

magn_msg->magnetic_field.x = mag_field.x() * MILLIT_TO_TESLA;
magn_msg->magnetic_field.y = mag_field.y() * MILLIT_TO_TESLA;
Expand Down
9 changes: 6 additions & 3 deletions mavros/src/plugins/local_position.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@
namespace mavplugin {
/**
* @brief Local position plugin.
* Publish local position to TF and PositionStamped,
* Publish local position to TF and PositionStamped
*/
class LocalPositionPlugin : public MavRosPlugin {
public:
Expand Down Expand Up @@ -73,8 +73,11 @@ class LocalPositionPlugin : public MavRosPlugin {
pos_ned.vx, pos_ned.vy, pos_ned.vz);

tf::Transform transform;
// ENU->NED
auto position = UAS::convert_position(pos_ned.x, pos_ned.y, pos_ned.z);

/**
* @note NED->ENU frame conversion
*/
auto position = UAS::transform_frame_general_xyz(pos_ned.x, pos_ned.y, pos_ned.z);

transform.setOrigin(position);
transform.setRotation(uas->get_attitude_orientation());
Expand Down
10 changes: 6 additions & 4 deletions mavros/src/plugins/safety_area.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ class SafetyAreaPlugin : public MavRosPlugin {
/* -*- mid-level helpers -*- */

/**
* Send a safety zone (volume), which is defined by two corners of a cube,
* @brief Send a safety zone (volume), which is defined by two corners of a cube,
* to the FCU.
*
* @note ENU frame.
Expand All @@ -110,9 +110,11 @@ class SafetyAreaPlugin : public MavRosPlugin {
p1x, p1y, p1z,
p2x, p2y, p2z);

// ENU->NED
auto p1 = UAS::convert_position(p1x, p1y, p1z);
auto p2 = UAS::convert_position(p2x, p2y, p2z);
/**
* @note ENU->NED frame conversion
*/
auto p1 = UAS::transform_frame_general_xyz(p1x, p1y, p1z);
auto p2 = UAS::transform_frame_general_xyz(p2x, p2y, p2z);

safety_set_allowed_area(
MAV_FRAME_LOCAL_NED, // TODO: use enum from lib
Expand Down
10 changes: 6 additions & 4 deletions mavros/src/plugins/setpoint_accel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,9 +62,9 @@ class SetpointAccelerationPlugin : public MavRosPlugin,
/* -*- mid-level helpers -*- */

/**
* Send acceleration/force to FCU acceleration controller
* @brief Send acceleration/force to FCU acceleration controller.
*
* Note: send only AFX AFY AFZ. ENU frame.
* @warning Send only AFX AFY AFZ. ENU frame.
*/
void send_setpoint_acceleration(const ros::Time &stamp, float afx, float afy, float afz) {
/* Documentation start from bit 1 instead 0.
Expand All @@ -75,8 +75,10 @@ class SetpointAccelerationPlugin : public MavRosPlugin,
if (send_force)
ignore_all_except_a_xyz |= (1 << 9);

// ENU->NED
auto accel = UAS::convert_accel(afx, afy, afz);
/**
* @note ENU->NED frame conversion
*/
auto accel = UAS::transform_frame_general_xyz(afx, afy, afz);

set_position_target_local_ned(stamp.toNSec() / 1000000,
MAV_FRAME_LOCAL_NED, // TODO: use enum on lib
Expand Down
Loading

0 comments on commit 37d325e

Please sign in to comment.