00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
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;
00056 Eigen::VectorXd angular_velocities;
00057 Eigen::VectorXd normalized;
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;
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
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
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
00254
00255
00256
00257
00258
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;
00293 Eigen::Vector3d position_W;
00294 Eigen::Quaterniond orientation_W_B;
00295 Eigen::Vector3d velocity_B;
00296 Eigen::Vector3d angular_velocity_B;
00297 Eigen::Matrix<double, 6, 6> pose_covariance_;
00298 Eigen::Matrix<double, 6, 6> twist_covariance_;
00299
00300
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
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
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