eigen_mav_msgs.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00007  *
00008  * Licensed under the Apache License, Version 2.0 (the "License");
00009  * you may not use this file except in compliance with the License.
00010  * You may obtain a copy of the License at
00011  *
00012  *     http://www.apache.org/licenses/LICENSE-2.0
00013 
00014  * Unless required by applicable law or agreed to in writing, software
00015  * distributed under the License is distributed on an "AS IS" BASIS,
00016  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00017  * See the License for the specific language governing permissions and
00018  * limitations under the License.
00019  */
00020 
00021 #ifndef MAV_MSGS_EIGEN_MAV_MSGS_H
00022 #define MAV_MSGS_EIGEN_MAV_MSGS_H
00023 
00024 #include <Eigen/Eigen>
00025 #include <deque>
00026 #include <iostream>
00027 
00028 #include "mav_msgs/common.h"
00029 
00030 namespace mav_msgs {
00031 
00032 struct EigenAttitudeThrust {
00033   EigenAttitudeThrust()
00034       : attitude(Eigen::Quaterniond::Identity()),
00035         thrust(Eigen::Vector3d::Zero()) {}
00036   EigenAttitudeThrust(const Eigen::Quaterniond& _attitude,
00037                       const Eigen::Vector3d& _thrust) {
00038     attitude = _attitude;
00039     thrust = _thrust;
00040   }
00041 
00042   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00043   Eigen::Quaterniond attitude;
00044   Eigen::Vector3d thrust;
00045 };
00046 
00047 struct EigenActuators {
00048   // TODO(ffurrer): Find a proper way of initializing :)
00049 
00050   EigenActuators(const Eigen::VectorXd& _angular_velocities) {
00051     angular_velocities = _angular_velocities;
00052   }
00053 
00054   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00055   Eigen::VectorXd angles;              // In rad.
00056   Eigen::VectorXd angular_velocities;  // In rad/s.
00057   Eigen::VectorXd normalized;          // Everything else, normalized [-1 to 1].
00058 };
00059 
00060 struct EigenRateThrust {
00061   EigenRateThrust()
00062       : angular_rates(Eigen::Vector3d::Zero()),
00063         thrust(Eigen::Vector3d::Zero()) {}
00064 
00065   EigenRateThrust(const Eigen::Vector3d& _angular_rates,
00066                   const Eigen::Vector3d _thrust)
00067       : angular_rates(_angular_rates), thrust(_thrust) {}
00068 
00069   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00070   Eigen::Vector3d angular_rates;
00071   Eigen::Vector3d thrust;
00072 };
00073 
00074 struct EigenTorqueThrust {
00075   EigenTorqueThrust()
00076       : torque(Eigen::Vector3d::Zero()), thrust(Eigen::Vector3d::Zero()) {}
00077 
00078   EigenTorqueThrust(const Eigen::Vector3d& _torque,
00079                     const Eigen::Vector3d _thrust)
00080       : torque(_torque), thrust(_thrust) {}
00081 
00082   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00083   Eigen::Vector3d torque;
00084   Eigen::Vector3d thrust;
00085 };
00086 
00087 struct EigenRollPitchYawrateThrust {
00088   EigenRollPitchYawrateThrust()
00089       : roll(0.0), pitch(0.0), yaw_rate(0.0), thrust(Eigen::Vector3d::Zero()) {}
00090 
00091   EigenRollPitchYawrateThrust(double _roll, double _pitch, double _yaw_rate,
00092                               const Eigen::Vector3d& _thrust)
00093       : roll(_roll), pitch(_pitch), yaw_rate(_yaw_rate), thrust(_thrust) {}
00094 
00095   double roll;
00096   double pitch;
00097   double yaw_rate;
00098   Eigen::Vector3d thrust;
00099 };
00100 
00108 class EigenMavState {
00109  public:
00110   typedef std::vector<EigenMavState,
00111           Eigen::aligned_allocator<EigenMavState>> Vector;
00112   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00113 
00115   EigenMavState()
00116       : position_W(Eigen::Vector3d::Zero()),
00117         velocity_W(Eigen::Vector3d::Zero()),
00118         acceleration_B(Eigen::Vector3d::Zero()),
00119         orientation_W_B(Eigen::Quaterniond::Identity()),
00120         angular_velocity_B(Eigen::Vector3d::Zero()),
00121         angular_acceleration_B(Eigen::Vector3d::Zero()) {}
00122 
00123   EigenMavState(const Eigen::Vector3d& position_W,
00124                 const Eigen::Vector3d& velocity_W,
00125                 const Eigen::Vector3d& acceleration_B,
00126                 const Eigen::Quaterniond& orientation_W_B,
00127                 const Eigen::Vector3d& angular_velocity_B,
00128                 const Eigen::Vector3d& angular_acceleration_B)
00129       : position_W(position_W),
00130         velocity_W(velocity_W),
00131         acceleration_B(acceleration_B),
00132         orientation_W_B(orientation_W_B),
00133         angular_velocity_B(angular_velocity_B),
00134         angular_acceleration_B(angular_acceleration_B) {}
00135 
00136   std::string toString() const {
00137     std::stringstream ss;
00138     ss << "position:              " << position_W.transpose() << std::endl
00139        << "velocity:              " << velocity_W.transpose() << std::endl
00140        << "acceleration_body:     " << acceleration_B.transpose() << std::endl
00141        << "orientation (w-x-y-z): " << orientation_W_B.w() << " "
00142        << orientation_W_B.x() << " " << orientation_W_B.y() << " "
00143        << orientation_W_B.z() << " " << std::endl
00144        << "angular_velocity_body: " << angular_velocity_B.transpose()
00145        << std::endl
00146        << "angular_acceleration_body: " << angular_acceleration_B.transpose()
00147        << std::endl;
00148 
00149     return ss.str();
00150   }
00151 
00152   Eigen::Vector3d position_W;
00153   Eigen::Vector3d velocity_W;
00154   Eigen::Vector3d acceleration_B;
00155   Eigen::Quaterniond orientation_W_B;
00156   Eigen::Vector3d angular_velocity_B;
00157   Eigen::Vector3d angular_acceleration_B;
00158 };
00159 
00160 struct EigenTrajectoryPoint {
00161   typedef std::vector<EigenTrajectoryPoint,
00162   Eigen::aligned_allocator<EigenTrajectoryPoint>> Vector;
00163   EigenTrajectoryPoint()
00164       : timestamp_ns(-1),
00165         time_from_start_ns(0),
00166         position_W(Eigen::Vector3d::Zero()),
00167         velocity_W(Eigen::Vector3d::Zero()),
00168         acceleration_W(Eigen::Vector3d::Zero()),
00169         jerk_W(Eigen::Vector3d::Zero()),
00170         snap_W(Eigen::Vector3d::Zero()),
00171         orientation_W_B(Eigen::Quaterniond::Identity()),
00172         angular_velocity_W(Eigen::Vector3d::Zero()),
00173         angular_acceleration_W(Eigen::Vector3d::Zero()) {}
00174 
00175   EigenTrajectoryPoint(int64_t _time_from_start_ns,
00176                        const Eigen::Vector3d& _position,
00177                        const Eigen::Vector3d& _velocity,
00178                        const Eigen::Vector3d& _acceleration,
00179                        const Eigen::Vector3d& _jerk,
00180                        const Eigen::Vector3d& _snap,
00181                        const Eigen::Quaterniond& _orientation,
00182                        const Eigen::Vector3d& _angular_velocity,
00183                        const Eigen::Vector3d& _angular_acceleration)
00184       : time_from_start_ns(_time_from_start_ns),
00185         position_W(_position),
00186         velocity_W(_velocity),
00187         acceleration_W(_acceleration),
00188         jerk_W(_jerk),
00189         snap_W(_snap),
00190         orientation_W_B(_orientation),
00191         angular_velocity_W(_angular_velocity),
00192         angular_acceleration_W(_angular_acceleration) {}
00193 
00194   EigenTrajectoryPoint(int64_t _time_from_start_ns,
00195                        const Eigen::Vector3d& _position,
00196                        const Eigen::Vector3d& _velocity,
00197                        const Eigen::Vector3d& _acceleration,
00198                        const Eigen::Vector3d& _jerk,
00199                        const Eigen::Vector3d& _snap,
00200                        const Eigen::Quaterniond& _orientation,
00201                        const Eigen::Vector3d& _angular_velocity)
00202       : EigenTrajectoryPoint(_time_from_start_ns, _position, _velocity,
00203                              _acceleration, _jerk, _snap, _orientation,
00204                              _angular_velocity, Eigen::Vector3d::Zero()) {}
00205 
00206   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00207   int64_t timestamp_ns;  // Time since epoch, negative value = invalid timestamp.
00208   int64_t time_from_start_ns;
00209   Eigen::Vector3d position_W;
00210   Eigen::Vector3d velocity_W;
00211   Eigen::Vector3d acceleration_W;
00212   Eigen::Vector3d jerk_W;
00213   Eigen::Vector3d snap_W;
00214 
00215   Eigen::Quaterniond orientation_W_B;
00216   Eigen::Vector3d angular_velocity_W;
00217   Eigen::Vector3d angular_acceleration_W;
00218 
00219   // Accessors for making dealing with orientation/angular velocity easier.
00220   inline double getYaw() const { return yawFromQuaternion(orientation_W_B); }
00221   inline double getYawRate() const { return angular_velocity_W.z(); }
00222   inline double getYawAcc() const { return angular_acceleration_W.z(); }
00223   // WARNING: sets roll and pitch to 0.
00224   inline void setFromYaw(double yaw) {
00225     orientation_W_B = quaternionFromYaw(yaw);
00226   }
00227   inline void setFromYawRate(double yaw_rate) {
00228     angular_velocity_W.x() = 0.0;
00229     angular_velocity_W.y() = 0.0;
00230     angular_velocity_W.z() = yaw_rate;
00231   }
00232   inline void setFromYawAcc(double yaw_acc) {
00233     angular_acceleration_W.x() = 0.0;
00234     angular_acceleration_W.y() = 0.0;
00235     angular_acceleration_W.z() = yaw_acc;
00236   }
00237 
00238   std::string toString() const {
00239     std::stringstream ss;
00240     ss << "position:          " << position_W.transpose() << std::endl
00241        << "velocity:          " << velocity_W.transpose() << std::endl
00242        << "acceleration:      " << acceleration_W.transpose() << std::endl
00243        << "jerk:              " << jerk_W.transpose() << std::endl
00244        << "snap:              " << snap_W.transpose() << std::endl
00245        << "yaw:               " << getYaw() << std::endl
00246        << "yaw_rate:          " << getYawRate() << std::endl
00247        << "yaw_acc:           " << getYawAcc() << std::endl;
00248 
00249     return ss.str();
00250   }
00251 };
00252 
00253 // Operator overload to transform Trajectory Points according to the Eigen
00254 // interfaces (uses operator* for this).
00255 // Has to be outside of class.
00256 // Example:
00257 // Eigen::Affine3d transform; EigenTrajectoryPoint point;
00258 // EigenTrajectoryPoint transformed = transform * point;
00259 inline EigenTrajectoryPoint operator*(const Eigen::Affine3d& lhs,
00260                                       const EigenTrajectoryPoint& rhs) {
00261   EigenTrajectoryPoint transformed(rhs);
00262   transformed.position_W = lhs * rhs.position_W;
00263   transformed.velocity_W = lhs.rotation() * rhs.velocity_W;
00264   transformed.acceleration_W = lhs.rotation() * rhs.acceleration_W;
00265   transformed.jerk_W = lhs.rotation() * rhs.jerk_W;
00266   transformed.snap_W = lhs.rotation() * rhs.snap_W;
00267   transformed.orientation_W_B = lhs.rotation() * rhs.orientation_W_B;
00268   transformed.angular_velocity_W = lhs.rotation() * rhs.angular_velocity_W;
00269   transformed.angular_acceleration_W =
00270       lhs.rotation() * rhs.angular_acceleration_W;
00271   return transformed;
00272 }
00273 
00274 struct EigenOdometry {
00275   EigenOdometry()
00276       : timestamp_ns(-1),
00277         position_W(Eigen::Vector3d::Zero()),
00278         orientation_W_B(Eigen::Quaterniond::Identity()),
00279         velocity_B(Eigen::Vector3d::Zero()),
00280         angular_velocity_B(Eigen::Vector3d::Zero()) {}
00281 
00282   EigenOdometry(const Eigen::Vector3d& _position,
00283                 const Eigen::Quaterniond& _orientation,
00284                 const Eigen::Vector3d& _velocity_body,
00285                 const Eigen::Vector3d& _angular_velocity)
00286       : position_W(_position),
00287         orientation_W_B(_orientation),
00288         velocity_B(_velocity_body),
00289         angular_velocity_B(_angular_velocity) {}
00290 
00291   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00292   int64_t timestamp_ns;  // Time since epoch, negative value = invalid timestamp.
00293   Eigen::Vector3d position_W;
00294   Eigen::Quaterniond orientation_W_B;
00295   Eigen::Vector3d velocity_B;  // Velocity in expressed in the Body frame!
00296   Eigen::Vector3d angular_velocity_B;
00297   Eigen::Matrix<double, 6, 6> pose_covariance_;
00298   Eigen::Matrix<double, 6, 6> twist_covariance_;
00299 
00300   // Accessors for making dealing with orientation/angular velocity easier.
00301   inline double getYaw() const { return yawFromQuaternion(orientation_W_B); }
00302   inline void getEulerAngles(Eigen::Vector3d* euler_angles) const {
00303     getEulerAnglesFromQuaternion(orientation_W_B, euler_angles);
00304   }
00305   inline double getYawRate() const { return angular_velocity_B.z(); }
00306   // WARNING: sets roll and pitch to 0.
00307   inline void setFromYaw(double yaw) {
00308     orientation_W_B = quaternionFromYaw(yaw);
00309   }
00310   inline void setFromYawRate(double yaw_rate) {
00311     angular_velocity_B.x() = 0.0;
00312     angular_velocity_B.y() = 0.0;
00313     angular_velocity_B.z() = yaw_rate;
00314   }
00315 
00316   inline Eigen::Vector3d getVelocityWorld() const {
00317     return orientation_W_B * velocity_B;
00318   }
00319   inline void setVelocityWorld(const Eigen::Vector3d& velocity_world) {
00320     velocity_B = orientation_W_B.inverse() * velocity_world;
00321   }
00322 };
00323 
00324 // TODO(helenol): replaced with aligned allocator headers from Simon.
00325 #define MAV_MSGS_CONCATENATE(x, y) x##y
00326 #define MAV_MSGS_CONCATENATE2(x, y) MAV_MSGS_CONCATENATE(x, y)
00327 #define MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EIGEN_TYPE)                    \
00328   typedef std::vector<EIGEN_TYPE, Eigen::aligned_allocator<EIGEN_TYPE>> \
00329       MAV_MSGS_CONCATENATE2(EIGEN_TYPE, Vector);                        \
00330   typedef std::deque<EIGEN_TYPE, Eigen::aligned_allocator<EIGEN_TYPE>>  \
00331       MAV_MSGS_CONCATENATE2(EIGEN_TYPE, Deque);
00332 
00333 MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenAttitudeThrust)
00334 MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenActuators)
00335 MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenRateThrust)
00336 MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenTrajectoryPoint)
00337 MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenRollPitchYawrateThrust)
00338 MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenOdometry)
00339 }
00340 
00341 #endif  // MAV_MSGS_EIGEN_MAV_MSGS_H


mav_msgs
Author(s): Simon Lynen, Markus Achtelik, Pascal Gohl, Sammy Omari, Michael Burri, Fadri Furrer, Helen Oleynikova, Mina Kamel, Karen Bodie, Rik Bähnemann
autogenerated on Fri Jun 14 2019 19:31:56