Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #ifndef MAV_MSGS_COMMON_H
00024 #define MAV_MSGS_COMMON_H
00025
00026 #include <geometry_msgs/Point.h>
00027 #include <geometry_msgs/Quaternion.h>
00028 #include <geometry_msgs/Vector3.h>
00029 #include <Eigen/Geometry>
00030
00031 namespace mav_msgs {
00032
00035 inline double MagnitudeOfGravity(const double height,
00036 const double latitude_radians) {
00037
00038 const double kGravity_0 = 9.780327;
00039 const double kGravity_a = 0.0053024;
00040 const double kGravity_b = 0.0000058;
00041 const double kGravity_c = 3.155 * 1e-7;
00042
00043 double sin_squared_latitude = sin(latitude_radians) * sin(latitude_radians);
00044 double sin_squared_twice_latitude =
00045 sin(2 * latitude_radians) * sin(2 * latitude_radians);
00046 return kGravity_0 * ((1 + kGravity_a * sin_squared_latitude -
00047 kGravity_b * sin_squared_twice_latitude) -
00048 kGravity_c * height);
00049 }
00050
00051 inline Eigen::Vector3d vector3FromMsg(const geometry_msgs::Vector3& msg) {
00052 return Eigen::Vector3d(msg.x, msg.y, msg.z);
00053 }
00054
00055 inline Eigen::Vector3d vector3FromPointMsg(const geometry_msgs::Point& msg) {
00056 return Eigen::Vector3d(msg.x, msg.y, msg.z);
00057 }
00058
00059 inline Eigen::Quaterniond quaternionFromMsg(
00060 const geometry_msgs::Quaternion& msg) {
00061
00062
00063 Eigen::Quaterniond quaternion(msg.w, msg.x, msg.y, msg.z);
00064 if (quaternion.norm() < std::numeric_limits<double>::epsilon()) {
00065 quaternion.setIdentity();
00066 } else {
00067 quaternion.normalize();
00068 }
00069 return quaternion;
00070 }
00071
00072 inline void vectorEigenToMsg(const Eigen::Vector3d& eigen,
00073 geometry_msgs::Vector3* msg) {
00074 assert(msg != NULL);
00075 msg->x = eigen.x();
00076 msg->y = eigen.y();
00077 msg->z = eigen.z();
00078 }
00079
00080 inline void pointEigenToMsg(const Eigen::Vector3d& eigen,
00081 geometry_msgs::Point* msg) {
00082 assert(msg != NULL);
00083 msg->x = eigen.x();
00084 msg->y = eigen.y();
00085 msg->z = eigen.z();
00086 }
00087
00088 inline void quaternionEigenToMsg(const Eigen::Quaterniond& eigen,
00089 geometry_msgs::Quaternion* msg) {
00090 assert(msg != NULL);
00091 msg->x = eigen.x();
00092 msg->y = eigen.y();
00093 msg->z = eigen.z();
00094 msg->w = eigen.w();
00095 }
00096
00103 inline double yawFromQuaternion(const Eigen::Quaterniond& q) {
00104 return atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
00105 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
00106 }
00107
00108 inline Eigen::Quaterniond quaternionFromYaw(double yaw) {
00109 return Eigen::Quaterniond(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()));
00110 }
00111
00112 inline void setQuaternionMsgFromYaw(double yaw,
00113 geometry_msgs::Quaternion* msg) {
00114 assert(msg != NULL);
00115 Eigen::Quaterniond q_yaw = quaternionFromYaw(yaw);
00116 msg->x = q_yaw.x();
00117 msg->y = q_yaw.y();
00118 msg->z = q_yaw.z();
00119 msg->w = q_yaw.w();
00120 }
00121
00122 inline void setAngularVelocityMsgFromYawRate(double yaw_rate,
00123 geometry_msgs::Vector3* msg) {
00124 assert(msg != NULL);
00125 msg->x = 0.0;
00126 msg->y = 0.0;
00127 msg->z = yaw_rate;
00128 }
00129
00130 inline void getEulerAnglesFromQuaternion(const Eigen::Quaternion<double>& q,
00131 Eigen::Vector3d* euler_angles) {
00132 {
00133 assert(euler_angles != NULL);
00134
00135 *euler_angles << atan2(2.0 * (q.w() * q.x() + q.y() * q.z()),
00136 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())),
00137 asin(2.0 * (q.w() * q.y() - q.z() * q.x())),
00138 atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
00139 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
00140 }
00141 }
00142
00143 inline double getShortestYawDistance(double yaw1, double yaw2) {
00144
00145 double yaw_mod = std::fmod(yaw1 - yaw2, 2 * M_PI);
00146 if (yaw_mod < -M_PI) {
00147 yaw_mod += 2 * M_PI;
00148 } else if (yaw_mod > M_PI) {
00149 yaw_mod -= 2 * M_PI;
00150 }
00151
00152 return yaw_mod;
00153 }
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178 inline void getSquaredRotorSpeedsFromAllocationAndState(
00179 const Eigen::MatrixXd& allocation_inv, const Eigen::Vector3d& inertia,
00180 double mass, const Eigen::Vector3d& angular_velocity_B,
00181 const Eigen::Vector3d& angular_acceleration_B,
00182 const Eigen::Vector3d& acceleration_B,
00183 Eigen::VectorXd* rotor_rates_squared) {
00184 const Eigen::Vector3d torque =
00185 inertia.asDiagonal() * angular_acceleration_B +
00186 angular_velocity_B.cross(inertia.asDiagonal() * angular_velocity_B);
00187 const double thrust_force = mass * acceleration_B.norm();
00188 Eigen::Vector4d input;
00189 input << torque, thrust_force;
00190 *rotor_rates_squared = allocation_inv * input;
00191 }
00192
00193 }
00194
00195 #endif // MAV_MSGS_COMMON_H
mav_msgs
Author(s): Simon Lynen, Markus Achtelik, Pascal Gohl, Sammy Omari, Michael Burri, Fadri Furrer, Helen Oleynikova, Mina Kamel
autogenerated on Thu Aug 17 2017 02:31:44