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
00033 const double kNumNanosecondsPerSecond = 1.e9;
00034
00037 inline double MagnitudeOfGravity(const double height,
00038 const double latitude_radians) {
00039
00040 const double kGravity_0 = 9.780327;
00041 const double kGravity_a = 0.0053024;
00042 const double kGravity_b = 0.0000058;
00043 const double kGravity_c = 3.155 * 1e-7;
00044
00045 double sin_squared_latitude = sin(latitude_radians) * sin(latitude_radians);
00046 double sin_squared_twice_latitude =
00047 sin(2 * latitude_radians) * sin(2 * latitude_radians);
00048 return kGravity_0 * ((1 + kGravity_a * sin_squared_latitude -
00049 kGravity_b * sin_squared_twice_latitude) -
00050 kGravity_c * height);
00051 }
00052
00053 inline Eigen::Vector3d vector3FromMsg(const geometry_msgs::Vector3& msg) {
00054 return Eigen::Vector3d(msg.x, msg.y, msg.z);
00055 }
00056
00057 inline Eigen::Vector3d vector3FromPointMsg(const geometry_msgs::Point& msg) {
00058 return Eigen::Vector3d(msg.x, msg.y, msg.z);
00059 }
00060
00061 inline Eigen::Quaterniond quaternionFromMsg(
00062 const geometry_msgs::Quaternion& msg) {
00063
00064
00065 Eigen::Quaterniond quaternion(msg.w, msg.x, msg.y, msg.z);
00066 if (quaternion.norm() < std::numeric_limits<double>::epsilon()) {
00067 quaternion.setIdentity();
00068 } else {
00069 quaternion.normalize();
00070 }
00071 return quaternion;
00072 }
00073
00074 inline void vectorEigenToMsg(const Eigen::Vector3d& eigen,
00075 geometry_msgs::Vector3* msg) {
00076 assert(msg != NULL);
00077 msg->x = eigen.x();
00078 msg->y = eigen.y();
00079 msg->z = eigen.z();
00080 }
00081
00082 inline void pointEigenToMsg(const Eigen::Vector3d& eigen,
00083 geometry_msgs::Point* msg) {
00084 assert(msg != NULL);
00085 msg->x = eigen.x();
00086 msg->y = eigen.y();
00087 msg->z = eigen.z();
00088 }
00089
00090 inline void quaternionEigenToMsg(const Eigen::Quaterniond& eigen,
00091 geometry_msgs::Quaternion* msg) {
00092 assert(msg != NULL);
00093 msg->x = eigen.x();
00094 msg->y = eigen.y();
00095 msg->z = eigen.z();
00096 msg->w = eigen.w();
00097 }
00098
00105 inline double yawFromQuaternion(const Eigen::Quaterniond& q) {
00106 return atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
00107 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
00108 }
00109
00110 inline Eigen::Quaterniond quaternionFromYaw(double yaw) {
00111 return Eigen::Quaterniond(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()));
00112 }
00113
00114 inline void setQuaternionMsgFromYaw(double yaw,
00115 geometry_msgs::Quaternion* msg) {
00116 assert(msg != NULL);
00117 Eigen::Quaterniond q_yaw = quaternionFromYaw(yaw);
00118 msg->x = q_yaw.x();
00119 msg->y = q_yaw.y();
00120 msg->z = q_yaw.z();
00121 msg->w = q_yaw.w();
00122 }
00123
00124 inline void setAngularVelocityMsgFromYawRate(double yaw_rate,
00125 geometry_msgs::Vector3* msg) {
00126 assert(msg != NULL);
00127 msg->x = 0.0;
00128 msg->y = 0.0;
00129 msg->z = yaw_rate;
00130 }
00131
00132 inline void getEulerAnglesFromQuaternion(const Eigen::Quaternion<double>& q,
00133 Eigen::Vector3d* euler_angles) {
00134 {
00135 assert(euler_angles != NULL);
00136
00137 *euler_angles << atan2(2.0 * (q.w() * q.x() + q.y() * q.z()),
00138 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())),
00139 asin(2.0 * (q.w() * q.y() - q.z() * q.x())),
00140 atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
00141 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
00142 }
00143 }
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168 inline void getSquaredRotorSpeedsFromAllocationAndState(
00169 const Eigen::MatrixXd& allocation_inv, const Eigen::Vector3d& inertia,
00170 double mass, const Eigen::Vector3d& angular_velocity_B,
00171 const Eigen::Vector3d& angular_acceleration_B,
00172 const Eigen::Vector3d& acceleration_B,
00173 Eigen::VectorXd* rotor_rates_squared) {
00174 const Eigen::Vector3d torque =
00175 inertia.asDiagonal() * angular_acceleration_B +
00176 angular_velocity_B.cross(inertia.asDiagonal() * angular_velocity_B);
00177 const double thrust_force = mass * acceleration_B.norm();
00178 Eigen::Vector4d input;
00179 input << torque, thrust_force;
00180 *rotor_rates_squared = allocation_inv * input;
00181 }
00182
00183 inline double nanosecondsToSeconds(int64_t nanoseconds) {
00184 double seconds = nanoseconds / kNumNanosecondsPerSecond;
00185 return seconds;
00186 }
00187
00188 inline int64_t secondsToNanoseconds(double seconds) {
00189 int64_t nanoseconds =
00190 static_cast<int64_t>(seconds * kNumNanosecondsPerSecond);
00191 return nanoseconds;
00192 }
00193
00194 }
00195
00196 #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, Karen Bodie, Rik Bähnemann
autogenerated on Fri Jun 14 2019 19:31:56