uas_frame_conversions.cpp
Go to the documentation of this file.
00001 
00010 /*
00011  * Copyright 2015 Nuno Marques.
00012  *
00013  * This file is part of the mavros package and subject to the license terms
00014  * in the top-level LICENSE file of the mavros repository.
00015  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00016  */
00017 #include <array>
00018 #include <mavros/mavros_uas.h>
00019 
00020 using namespace mavros;
00021 
00022 // Static quaternion needed for rotating between ENU and NED frames
00023 // +PI rotation around X (North) axis follwed by +PI/2 rotation about Z (Down)
00024 // gives the ENU frame.  Similarly, a +PI rotation about X (East) followed by
00025 // a +PI/2 roation about Z (Up) gives the NED frame.
00026 static const Eigen::Quaterniond NED_ENU_Q = UAS::quaternion_from_rpy(M_PI, 0.0, M_PI_2);
00027 
00028 // Static quaternion needed for rotating between aircraft and base_link frames
00029 // +PI rotation around X (Forward) axis transforms from Forward, Right, Down (aircraft)
00030 // Fto Forward, Left, Up (base_link) frames.
00031 static const Eigen::Quaterniond AIRCRAFT_BASELINK_Q = UAS::quaternion_from_rpy(M_PI, 0.0, 0.0);
00032 
00033 static const Eigen::Affine3d NED_ENU_AFFINE(NED_ENU_Q);
00034 static const Eigen::Affine3d AIRCRAFT_BASELINK_AFFINE(AIRCRAFT_BASELINK_Q);
00035 
00036 static inline const Eigen::Affine3d &get_static_transform_affine(const UAS::STATIC_TRANSFORM transform)
00037 {
00038         switch (transform) {
00039         case UAS::STATIC_TRANSFORM::NED_TO_ENU:
00040         case UAS::STATIC_TRANSFORM::ENU_TO_NED: {
00041                 return NED_ENU_AFFINE;
00042                 break;
00043         }
00044         case UAS::STATIC_TRANSFORM::AIRCRAFT_TO_BASELINK:
00045         case UAS::STATIC_TRANSFORM::BASELINK_TO_AIRCRAFT: {
00046                 return AIRCRAFT_BASELINK_AFFINE;
00047                 break;
00048         }
00049         default: {
00050                 ROS_BREAK();
00051         }
00052         }
00053 }
00054 
00055 
00057 //static const Eigen::Transform<double, 3, Eigen::Affine> FRAME_TRANSFORM_VECTOR3(FRAME_ROTATE_Q);
00058 
00059 
00060 Eigen::Quaterniond UAS::transform_orientation(const Eigen::Quaterniond &q, const UAS::STATIC_TRANSFORM transform)
00061 {
00062         //Transform the attitude representation from frame to frame.  The proof for this transform can
00063         //be seen http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
00064         switch (transform) {
00065         case STATIC_TRANSFORM::NED_TO_ENU:
00066         case STATIC_TRANSFORM::ENU_TO_NED: {
00067                 return NED_ENU_Q * q;
00068                 break;
00069         }
00070         case STATIC_TRANSFORM::AIRCRAFT_TO_BASELINK:
00071         case STATIC_TRANSFORM::BASELINK_TO_AIRCRAFT: {
00072                 return q * AIRCRAFT_BASELINK_Q;
00073                 break;
00074         }
00075         default: {
00076                 //We don't know how to express the attitude WRT an undefined
00077                 //frame.
00078                 ROS_BREAK();
00079                 return q;
00080                 break;
00081         }
00082         }
00083 }
00084 
00085 Eigen::Vector3d UAS::transform_static_frame(const Eigen::Vector3d &vec, const UAS::STATIC_TRANSFORM transform)
00086 {
00087         auto affine_transform = get_static_transform_affine(transform);
00088         return affine_transform * vec;
00089 }
00090 
00091 UAS::Covariance3d UAS::transform_static_frame(const Covariance3d &cov, const UAS::STATIC_TRANSFORM transform)
00092 {
00093         Covariance3d cov_out_;
00094         EigenMapConstCovariance3d cov_in(cov.data());
00095         EigenMapCovariance3d cov_out(cov_out_.data());
00096         switch (transform) {
00097         case STATIC_TRANSFORM::NED_TO_ENU:
00098         case STATIC_TRANSFORM::ENU_TO_NED: {
00099                 // code from imu_transformer tf2_sensor_msgs.h
00100                 //cov_out = FRAME_ROTATE_Q * cov_in * FRAME_ROTATE_Q.inverse();
00101                 // from comments on github about tf2_sensor_msgs.h
00102                 cov_out = cov_in * NED_ENU_Q;
00103                 return cov_out_;
00104                 break;
00105         }
00106         case STATIC_TRANSFORM::AIRCRAFT_TO_BASELINK:
00107         case STATIC_TRANSFORM::BASELINK_TO_AIRCRAFT: {
00108                 // code from imu_transformer tf2_sensor_msgs.h
00109                 //cov_out = FRAME_ROTATE_Q * cov_in * FRAME_ROTATE_Q.inverse();
00110                 // from comments on github about tf2_sensor_msgs.h
00111                 cov_out = cov_in * AIRCRAFT_BASELINK_Q;
00112                 return cov_out_;
00113                 break;
00114         }
00115         default: {
00116                 //We don't know the transform between the static frames.
00117                 //throw error and return given covariance
00118                 ROS_BREAK();
00119                 return cov;
00120                 break;
00121         }
00122         }
00123 }
00124 
00125 UAS::Covariance6d UAS::transform_static_frame(const Covariance6d &cov, const UAS::STATIC_TRANSFORM transform)
00126 {
00128         switch (transform) {
00129         default: {
00130                 Covariance6d cov_out_;
00131                 EigenMapConstCovariance6d cov_in(cov.data());
00132                 EigenMapCovariance6d cov_out(cov_out_.data());
00133                 ROS_BREAK();
00134                 return cov_out_;
00135         }
00136         }
00137 }
00138 
00139 Eigen::Vector3d UAS::transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q)
00140 {
00141         Eigen::Affine3d FRAME_TRANSFORM_VECTOR3(q);
00142         return FRAME_TRANSFORM_VECTOR3 * vec;
00143 }
00144 
00145 UAS::Covariance3d UAS::transform_frame(const Covariance3d &cov, const Eigen::Quaterniond &q)
00146 {
00147         Covariance3d cov_out_;
00148         EigenMapConstCovariance3d cov_in(cov.data());
00149         EigenMapCovariance3d cov_out(cov_out_.data());
00150 
00151         // code from imu_transformer tf2_sensor_msgs.h
00152         //cov_out = FRAME_ROTATE_Q * cov_in * FRAME_ROTATE_Q.inverse();
00153         // from comments on github about tf2_sensor_msgs.h
00154         cov_out = cov_in * q;
00155         return cov_out_;
00156 }
00157 
00158 UAS::Covariance6d UAS::transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &q)
00159 {
00160         Covariance6d cov_out_;
00161         EigenMapConstCovariance6d cov_in(cov.data());
00162         EigenMapCovariance6d cov_out(cov_out_.data());
00163 
00165         ROS_BREAK();
00166         return cov_out_;
00167 }


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19