Merge branch 'master' of github.com:ctu-mrs/mrs_uav_state_estimators

ctu-mrs · Dec 17, 2024 · 2eca095 · 2eca095
2 parents 37ba303 + fd502ba
commit 2eca095
Show file tree

Hide file tree

Showing 7 changed files with 1,025 additions and 58 deletions.
diff --git a/README.md b/README.md
@@ -1,7 +1,3 @@
 # MRS UAV State Estimators
 
-Estimator plugins for the [Estimation Manager](https://github.com/ctu-mrs/mrs_uav_managers).
-
-> :warning: **Attention please: This README needs work.**
->
-> The MRS UAV System 1.5 is being released and this page needs updating. Please, keep in mind that the information on this page might not be valid.
+Please follow to the [documentation page](https://ctu-mrs.github.io/docs/features/estimators/).
diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h
diff --git a/include/mrs_uav_state_estimators/estimators/state/passthrough.h b/include/mrs_uav_state_estimators/estimators/state/passthrough.h
@@ -37,7 +37,6 @@ class Passthrough : public mrs_uav_managers::StateEstimator {
   using CommonHandlers_t = mrs_uav_managers::estimation_manager::CommonHandlers_t;
 
 private:
-
   const std::string package_name_ = "mrs_uav_state_estimators";
 
   const std::string est_lat_name_ = "lat_passthrough";
@@ -58,6 +57,7 @@ class Passthrough : public mrs_uav_managers::StateEstimator {
   std::atomic<int> counter_odom_msgs_ = 0;
   ros::WallTime    t_check_hz_last_;
   double           prev_avg_hz_ = 0;
+  bool             kickoff_     = false;
 
   ros::Timer                 timer_update_;
   void                       timerUpdate(const ros::TimerEvent &event);

diff --git a/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h b/include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h
@@ -0,0 +1,90 @@
+#pragma once
+#ifndef GEO_MAG_DECLINATION_H
+#define GEO_MAG_DECLINATION_H
+
+#include "geo_magnetic_tables.hpp"
+
+#include <math.h>
+#include <stdint.h>
+
+namespace mrs_uav_state_estimators
+{
+
+class GeoMagDeclination {
+
+public:
+  /*//{ constrain() */
+  template <typename T>
+  static T constrain(T val, T min_val, T max_val) {
+    return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);
+  }
+  /*//}*/
+
+  /*//{ degrees() */
+  template <typename T>
+  static T degrees(T radians) {
+    return radians * (static_cast<T>(180) / static_cast<T>(M_PI));
+  }
+  /*//}*/
+
+  /*//{ getLookupTableIndex() */
+  static unsigned getLookupTableIndex(float *val, float min, float max) {
+    *val = GeoMagDeclination::constrain(*val, min, max - SAMPLING_RES);
+
+    return static_cast<unsigned>((-(min) + *val) / SAMPLING_RES);
+  }
+  /*//}*/
+
+  /*//{ getTableData() */
+  static float getTableData(float lat, float lon, const int16_t table[LAT_DIM][LON_DIM]) {
+
+    lat = GeoMagDeclination::constrain(lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
+
+    if (lon > SAMPLING_MAX_LON) {
+      lon -= 360;
+    }
+
+    if (lon < SAMPLING_MIN_LON) {
+      lon += 360;
+    }
+
+    /* round down to nearest sampling resolution */
+    float min_lat = floorf(lat / SAMPLING_RES) * SAMPLING_RES;
+    float min_lon = floorf(lon / SAMPLING_RES) * SAMPLING_RES;
+
+    /* find index of nearest low sampling point */
+    unsigned min_lat_index = GeoMagDeclination::getLookupTableIndex(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
+    unsigned min_lon_index = GeoMagDeclination::getLookupTableIndex(&min_lon, SAMPLING_MIN_LON, SAMPLING_MAX_LON);
+
+    const float data_sw = table[min_lat_index][min_lon_index];
+    const float data_se = table[min_lat_index][min_lon_index + 1];
+    const float data_ne = table[min_lat_index + 1][min_lon_index + 1];
+    const float data_nw = table[min_lat_index + 1][min_lon_index];
+
+    /* perform bilinear interpolation on the four grid corners */
+    const float lat_scale = GeoMagDeclination::constrain((lat - min_lat) / SAMPLING_RES, 0.f, 1.f);
+    const float lon_scale = GeoMagDeclination::constrain((lon - min_lon) / SAMPLING_RES, 0.f, 1.f);
+
+    const float data_min = lon_scale * (data_se - data_sw) + data_sw;
+    const float data_max = lon_scale * (data_ne - data_nw) + data_nw;
+
+    return lat_scale * (data_max - data_min) + data_min;
+  }
+  /*//}*/
+
+  static float getMagDeclinationRadians(float lat, float lon) {
+    return GeoMagDeclination::getTableData(lat, lon, declination_table) * 1e-4f;  // declination table stored as 10^-4 radians
+  }
+
+  static float getMagDeclinationDegrees(float lat, float lon) {
+    return GeoMagDeclination::degrees(GeoMagDeclination::getMagDeclinationRadians(lat, lon));
+  }
+
+private:
+  GeoMagDeclination(){};
+};
+
+}  // namespace mrs_uav_state_estimators
+
+
+#endif
diff --git a/include/mrs_uav_state_estimators/processors/mag_declination/geo_magnetic_tables.hpp b/include/mrs_uav_state_estimators/processors/mag_declination/geo_magnetic_tables.hpp
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <stdint.h>
+
+static constexpr float SAMPLING_RES = 10;
+static constexpr float SAMPLING_MIN_LAT = -90;
+static constexpr float SAMPLING_MAX_LAT = 90;
+static constexpr float SAMPLING_MIN_LON = -180;
+static constexpr float SAMPLING_MAX_LON = 180;
+
+static constexpr int LAT_DIM = 19;
+static constexpr int LON_DIM = 37;
+
+
+// *INDENT-OFF*
+// Magnetic declination data in 0.005451 degrees
+// Model: WMM-2020,
+// Version: 0.5.1.11,
+// Date: 2024.41257,
+static constexpr const int16_t declination_table[19][37] {
+	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
+	/* LAT: -90 */ {  27264, 25429, 23595, 21761, 19926, 18092, 16258, 14423, 12589, 10754,  8920,  7086,  5251,  3417,  1583,  -252, -2086, -3921, -5755, -7589, -9424,-11258,-13092,-14927,-16761,-18596,-20430,-22264,-24099,-25933,-27768,-29602,-31436, 32767, 30933, 29098, 27264, },
+	/* LAT: -80 */ {  23650, 21416, 19382, 17521, 15799, 14184, 12647, 11165,  9721,  8303,  6904,  5519,  4143,  2772,  1397,     9, -1403, -2848, -4333, -5863, -7437, -9056,-10718,-12423,-14175,-15984,-17864,-19835,-21922,-24152,-26545,-29105,-31803, 31464, 28718, 26092, 23650, },
+	/* LAT: -70 */ {  15755, 14282, 13094, 12077, 11159, 10281,  9392,  8457,  7454,  6381,  5257,  4109,  2971,  1865,   794,  -267, -1359, -2527, -3797, -5172, -6632, -8142, -9669,-11188,-12690,-14181,-15686,-17253,-18969,-20997,-23675,-27707, 32110, 25303, 20617, 17720, 15755, }, // WARNING! black out zone
+	/* LAT: -60 */ {   8903,  8634,  8328,  8030,  7755,  7481,  7150,  6689,  6037,  5171,  4116,  2948,  1775,   704,  -211,  -998, -1763, -2635, -3706, -4982, -6396, -7846, -9240,-10513,-11630,-12572,-13320,-13839,-14019,-13549,-11322, -3523,  5353,  8167,  8943,  9057,  8903, }, // WARNING! black out zone
+	/* LAT: -50 */ {   5806,  5839,  5774,  5672,  5587,  5541,  5499,  5359,  4991,  4286,  3216,  1875,   470,  -754, -1647, -2218, -2623, -3091, -3835, -4934, -6270, -7627, -8830, -9771,-10388,-10623,-10400, -9587, -7986, -5492, -2431,   460,  2679,  4178,  5097,  5594,  5806, },
+	/* LAT: -40 */ {   4186,  4282,  4284,  4229,  4159,  4118,  4119,  4103,  3914,  3342,  2253,   728,  -912, -2273, -3151, -3597, -3767, -3819, -4016, -4665, -5740, -6891, -7817, -8365, -8453, -8028, -7069, -5603, -3811, -2034,  -509,   772,  1872,  2790,  3486,  3942,  4186, },
+	/* LAT: -30 */ {   3161,  3250,  3275,  3251,  3182,  3094,  3026,  2988,  2851,  2337,  1229,  -384, -2070, -3364, -4111, -4446, -4503, -4252, -3790, -3609, -4059, -4874, -5597, -5938, -5777, -5133, -4109, -2845, -1594,  -619,    83,   710,  1372,  2016,  2561,  2948,  3161, },
+	/* LAT: -20 */ {   2485,  2532,  2542,  2534,  2481,  2375,  2255,  2172,  2018,  1486,   353, -1228, -2777, -3865, -4390, -4481, -4240, -3623, -2707, -1902, -1669, -2082, -2768, -3241, -3254, -2856, -2179, -1328,  -531,   -35,   229,   542,  1013,  1529,  1985,  2318,  2485, },
+	/* LAT: -10 */ {   2072,  2064,  2032,  2018,  1980,  1883,  1758,  1657,  1461,   874,  -261, -1719, -3046, -3887, -4129, -3855, -3229, -2397, -1510,  -734,  -274,  -342,  -854, -1379, -1586, -1478, -1143,  -624,  -116,   121,   159,   321,   720,  1197,  1625,  1939,  2072, },
+	/* LAT:   0 */ {   1847,  1810,  1742,  1723,  1703,  1621,  1500,  1371,  1102,   444,  -660, -1952, -3043, -3628, -3589, -3035, -2219, -1401,  -724,  -169,   254,   350,    38,  -401,  -658,  -711,  -608,  -338,   -43,    36,   -42,    45,   413,   896,  1354,  1703,  1847, },
+	/* LAT:  10 */ {   1699,  1705,  1653,  1662,  1681,  1620,  1480,  1275,   878,   122,  -954, -2080, -2929, -3256, -3012, -2356, -1536,  -796,  -265,   129,   466,   610,   425,    85,  -155,  -268,  -299,  -222,  -121,  -175,  -335,  -312,    14,   509,  1030,  1470,  1699, },
+	/* LAT:  20 */ {   1494,  1649,  1708,  1799,  1884,  1855,  1681,  1355,   781,  -117, -1209, -2197, -2803, -2893, -2528, -1885, -1137,  -461,    13,   332,   601,   746,   640,   383,   178,    51,   -55,  -135,  -228,  -437,  -694,  -758,  -503,   -19,   561,  1114,  1494, },
+	/* LAT:  30 */ {   1160,  1544,  1816,  2050,  2215,  2221,  2017,  1567,   802,  -275, -1438, -2341, -2756, -2673, -2242, -1629,  -939,  -294,   184,   499,   738,   885,   855,   696,   542,   408,   230,    -8,  -317,  -718, -1109, -1275, -1099,  -642,   -30,   613,  1160, },
+	/* LAT:  40 */ {    765,  1379,  1900,  2314,  2577,  2622,  2393,  1831,   876,  -403, -1685, -2574, -2892, -2722, -2248, -1623,  -936,  -278,   252,   629,   908,  1109,  1199,  1184,  1104,   944,   647,   195,  -387, -1030, -1576, -1826, -1696, -1253,  -625,    77,   765, },
+	/* LAT:  50 */ {    437,  1218,  1936,  2528,  2919,  3036,  2799,  2116,   928,  -623, -2091, -3030, -3327, -3121, -2604, -1925, -1180,  -449,   193,   719,  1153,  1519,  1805,  1977,  1991,  1784,  1297,   527,  -430, -1381, -2085, -2373, -2234, -1771, -1112,  -355,   437, },
+	/* LAT:  60 */ {    198,  1087,  1936,  2674,  3216,  3452,  3243,  2411,   855, -1158, -2926, -3941, -4206, -3936, -3339, -2560, -1696,  -817,    27,   812,  1534,  2191,  2754,  3160,  3315,  3098,  2397,  1190,  -317, -1699, -2593, -2902, -2725, -2214, -1502,  -680,   198, },
+	/* LAT:  70 */ {   -111,   863,  1803,  2648,  3305,  3634,  3405,  2285,    58, -2678, -4700, -5589, -5627, -5143, -4352, -3383, -2317, -1205,   -83,  1026,  2098,  3108,  4014,  4748,  5204,  5214,  4533,  2927,   575, -1644, -2988, -3441, -3276, -2730, -1962, -1068,  -111, }, // WARNING! black out zone
+	/* LAT:  80 */ {  -1054,   -90,   801,  1527,  1946,  1812,   731, -1632, -4654, -6842, -7732, -7698, -7114, -6204, -5096, -3864, -2554, -1197,   184,  1572,  2947,  4292,  5581,  6779,  7828,  8630,  8988,  8486,  6313,  2026, -1910, -3677, -4009, -3636, -2914, -2019, -1054, }, // WARNING! black out zone
+	/* LAT:  90 */ { -30607,-28773,-26938,-25104,-23269,-21435,-19601,-17766,-15932,-14097,-12263,-10429, -8594, -6760, -4926, -3091, -1257,   578,  2412,  4246,  6081,  7915,  9749, 11584, 13418, 15253, 17087, 18921, 20756, 22590, 24424, 26259, 28093, 29928, 31762,-32441,-30607, }, // WARNING! black out zone
+};
+
diff --git a/include/mrs_uav_state_estimators/processors/proc_mag_declination.h b/include/mrs_uav_state_estimators/processors/proc_mag_declination.h
@@ -0,0 +1,147 @@
+#pragma once
+#ifndef PROCESSORS_MAG_DECLINATION_H
+#define PROCESSORS_MAG_DECLINATION_H
+
+#include <mrs_uav_state_estimators/processors/processor.h>
+#include <mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h>
+
+#include <sensor_msgs/NavSatFix.h>
+
+#include <mrs_lib/gps_conversions.h>
+#include <mrs_lib/subscribe_handler.h>
+#include <mrs_lib/param_loader.h>
+
+namespace mrs_uav_state_estimators
+{
+
+using namespace mrs_uav_managers::estimation_manager;
+
+template <int n_measurements>
+class ProcMagDeclination : public Processor<n_measurements> {
+
+public:
+  typedef Eigen::Matrix<double, n_measurements, 1> measurement_t;
+
+public:
+  ProcMagDeclination(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr<CommonHandlers_t>& ch,
+                     const std::shared_ptr<PrivateHandlers_t>& ph);
+
+  std::tuple<bool, bool> process(measurement_t& measurement) override;
+  void                   reset();
+
+private:
+  bool is_initialized_ = false;
+
+  bool        use_gnss_msg_;
+  std::string gnss_topic_;
+
+  std::mutex       mtx_gnss_;
+  std::atomic_bool got_gnss_ = false;
+  double           gnss_lat_;
+  double           gnss_lon_;
+
+  mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+  void                                              callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg);
+};
+
+/*//{ constructor */
+template <int n_measurements>
+ProcMagDeclination<n_measurements>::ProcMagDeclination(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name,
+                                             const std::shared_ptr<CommonHandlers_t>& ch, const std::shared_ptr<PrivateHandlers_t>& ph)
+    : Processor<n_measurements>(nh, correction_name, name, ch, ph) {
+
+  ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor<n_measurements>::getNamespacedName() + "/");
+
+  ph->param_loader->loadParam("use_gnss_msg", use_gnss_msg_);
+  if (use_gnss_msg_) {
+    ph->param_loader->loadParam("gnss_topic", gnss_topic_);
+  } else {
+    ph->param_loader->loadParam("latitude", gnss_lat_);
+    ph->param_loader->loadParam("longitude", gnss_lon_);
+  }
+
+  gnss_topic_ = "/" + ch->uav_name + "/" + gnss_topic_;
+
+  if (!ph->param_loader->loadedSuccessfully()) {
+    ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", Processor<n_measurements>::getPrintName().c_str());
+    ros::shutdown();
+  }
+
+  // | -------------- initialize subscribe handlers ------------- |
+  mrs_lib::SubscribeHandlerOptions shopts;
+  shopts.nh                 = nh;
+  shopts.node_name          = Processor<n_measurements>::getPrintName();
+  shopts.no_message_timeout = mrs_lib::no_timeout;
+  shopts.threadsafe         = true;
+  shopts.autostart          = true;
+  shopts.queue_size         = 10;
+  shopts.transport_hints    = ros::TransportHints().tcpNoDelay();
+
+  sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, gnss_topic_, &ProcMagDeclination::callbackGnss, this);
+
+  is_initialized_ = true;
+}
+/*//}*/
+
+/*//{ callbackGnss() */
+template <int n_measurements>
+void ProcMagDeclination<n_measurements>::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) {
+
+  if (!is_initialized_) {
+    return;
+  }
+
+  if (got_gnss_) {
+    return;
+  }
+
+  if (!std::isfinite(msg->latitude)) {
+    ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->latitude\"!!!", Processor<n_measurements>::getPrintName().c_str());
+    return;
+  }
+
+  if (!std::isfinite(msg->longitude)) {
+    ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->longitude\"!!!", Processor<n_measurements>::getPrintName().c_str());
+    return;
+  }
+
+  std::scoped_lock lock(mtx_gnss_);
+  gnss_lat_ = msg->latitude;
+  gnss_lon_ = msg->longitude;
+  ROS_INFO("[%s]: First GNSS obtained: lat:%.2f lon:%.2f", Processor<n_measurements>::getPrintName().c_str(), gnss_lat_, gnss_lon_);
+  got_gnss_ = true;
+}
+/*//}*/
+
+/*//{ process() */
+template <int n_measurements>
+std::tuple<bool, bool> ProcMagDeclination<n_measurements>::process(measurement_t& measurement) {
+
+  if (!Processor<n_measurements>::enabled_) {
+    return {true, true};
+  }
+
+  std::scoped_lock lock(mtx_gnss_);
+
+  if (!got_gnss_) {
+    ROS_WARN_THROTTLE(1.0, "[%s]: Missing GNSS data on topic: %s", Processor<n_measurements>::getPrintName().c_str(), gnss_topic_.c_str());
+    return {false, false};
+  }
+
+  float mag_declination = GeoMagDeclination::getMagDeclinationRadians(gnss_lat_, gnss_lon_);
+
+  measurement(0) -= mag_declination;
+
+  return {true, true};
+}
+/*//}*/
+
+/*//{ reset() */
+template <int n_measurements>
+void ProcMagDeclination<n_measurements>::reset() {
+  got_gnss_                  = false;
+}
+/*//}*/
+}  // namespace mrs_uav_state_estimators
+
+#endif  // PROCESSORS_PROC_MAG_DECLINATION
diff --git a/src/estimators/state/passthrough.cpp b/src/estimators/state/passthrough.cpp
@@ -33,6 +33,7 @@ void Passthrough::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<C
   // | --------------------- load parameters -------------------- |
   ph_->param_loader->loadParam("max_flight_z", max_flight_z_);
   ph_->param_loader->loadParam("message/topic", msg_topic_);
+  ph_->param_loader->loadParam("kickoff", kickoff_, false);
   msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;
 
   // | --------------- subscribers initialization --------------- |
@@ -171,9 +172,9 @@ void Passthrough::timerCheckPassthroughOdomHz([[maybe_unused]] const ros::TimerE
       }
 
       // the message rate must be higher than required by the control manager
-      if (avg_hz < ch_->desired_uav_state_rate) {
+      if (!kickoff_ && avg_hz < ch_->desired_uav_state_rate) {
         ROS_ERROR(
-            "[%s]: rate of passthrough odom: %.2f Hz lower than desired uav_state rate: %.2f Hz. Flight not allowed. Provide higher passthrough odometry rate "
+            "[%s]: rate of passthrough odom: %.2f Hz is lower than desired uav_state rate: %.2f Hz. Flight not allowed. Provide higher passthrough odometry rate "
             "or use a higher-level controller.",
             getPrintName().c_str(), avg_hz, ch_->desired_uav_state_rate);
         // note: might run the publishing asynchronously on the desired rate in this case to still be able to fly