From 901a85c132a47dec2683e1d32bd338413f7abf69 Mon Sep 17 00:00:00 2001 From: Matej Petrlik Date: Wed, 25 Sep 2024 15:44:17 +0200 Subject: [PATCH 1/9] added magnetic heading corrections --- .../estimators/correction.h | 180 +++++++++++++++--- 1 file changed, 155 insertions(+), 25 deletions(-) diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h index ec59593..a46675c 100644 --- a/include/mrs_uav_state_estimators/estimators/correction.h +++ b/include/mrs_uav_state_estimators/estimators/correction.h @@ -59,6 +59,7 @@ typedef enum RANGE, IMU, RTK_GPS, + MAG_HDG, POINT, VECTOR, QUAT, @@ -71,6 +72,7 @@ const std::map map_msg_type{{"nav_msgs/Odometry", Me {"sensor_msgs/Range", MessageType_t::RANGE}, {"sensor_msgs/Imu", MessageType_t::IMU}, {"mrs_msgs/RtkGps", MessageType_t::RTK_GPS}, + {"mrs_msgs/Float64Stamped", MessageType_t::MAG_HDG}, {"geometry_msgs/PointStamped", MessageType_t::POINT}, {"geometry_msgs/Vector3Stamped", MessageType_t::VECTOR}, {"geometry_msgs/QuaternionStamped", MessageType_t::QUAT}}; @@ -155,13 +157,19 @@ class Correction { mrs_lib::SubscribeHandler sh_quat_; measurement_t prev_hdg_measurement_; bool got_first_hdg_measurement_ = false; - bool init_hdg_in_zero_ = false; - double first_hdg_measurement_ = 0.0; + bool init_hdg_in_zero_ = false; + double first_hdg_measurement_ = 0.0; mrs_lib::SubscribeHandler sh_imu_; std::optional getCorrectionFromImu(const sensor_msgs::ImuConstPtr msg); void callbackImu(const sensor_msgs::Imu::ConstPtr msg); + mrs_lib::SubscribeHandler sh_mag_hdg_; + std::optional getCorrectionFromMagHeading(const mrs_msgs::Float64StampedConstPtr msg); + void callbackMagHeading(const mrs_msgs::Float64Stamped::ConstPtr msg); + double mag_hdg_previous_; + bool got_first_mag_hdg_; + mrs_lib::SubscribeHandler sh_range_; std::optional getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg); void callbackRange(const sensor_msgs::Range::ConstPtr msg); @@ -203,7 +211,9 @@ class Correction { std::optional getCorrectionFromQuat(const geometry_msgs::QuaternionStampedConstPtr msg); std::optional getZVelUntilted(const geometry_msgs::Vector3& msg, const std_msgs::Header& header); std::optional getVecInFrame(const geometry_msgs::Vector3& vec_in, const std_msgs::Header& source_header, const std::string target_frame); - std::optional getInFrame(const geometry_msgs::Point& vec_in, const std_msgs::Header& source_header, const std::string target_frame); + std::optional transformVecToFrame(const geometry_msgs::Vector3& vec_in, const std_msgs::Header& source_header, + const std::string target_frame); + std::optional getInFrame(const geometry_msgs::Point& vec_in, const std_msgs::Header& source_header, const std::string target_frame); std::optional transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const; @@ -350,6 +360,10 @@ Correction::Correction(ros::NodeHandle& nh, const std::string& e sh_rtk_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackRtk, this); break; } + case MessageType_t::MAG_HDG: { + sh_mag_hdg_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackMagHeading, this); + break; + } case MessageType_t::POINT: { sh_point_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackPoint, this); break; @@ -372,18 +386,17 @@ Correction::Correction(ros::NodeHandle& nh, const std::string& e } // | ------ subscribe orientation for obtaingin hdg rate ------ | - if (est_type_ == EstimatorType_t::HEADING && state_id_ == StateId_t::VELOCITY) { - ph->param_loader->loadParam("message/orientation_topic", orientation_topic_); - orientation_topic_ = "/" + ch_->uav_name + "/" + orientation_topic_; - sh_orientation_ = mrs_lib::SubscribeHandler(shopts, orientation_topic_); - } + /* if (est_type_ == EstimatorType_t::HEADING && state_id_ == StateId_t::VELOCITY) { */ + /* ph->param_loader->loadParam("message/orientation_topic", orientation_topic_); */ + /* orientation_topic_ = "/" + ch_->uav_name + "/" + orientation_topic_; */ + /* sh_orientation_ = mrs_lib::SubscribeHandler(shopts, orientation_topic_); */ + /* } */ if (est_type_ == EstimatorType_t::HEADING) { ph->param_loader->loadParam("init_hdg_in_zero", init_hdg_in_zero_, false); } - // | --------------- initialize publish handlers -------------- | if (ch_->debug_topics.correction) { ph_correction_raw_ = mrs_lib::PublisherHandler(nh, est_name_ + "/correction/" + getName() + "/raw", 10); @@ -598,7 +611,7 @@ std::optional::MeasurementStamped> Correctio if (res) { measurement_stamped.value = res.value(); } else { - ROS_ERROR_THROTTLE(1.0, "[%s]: could not get rtk correction", ros::this_node::getName().c_str()); + ROS_ERROR_THROTTLE(1.0, "[%s]: could not get imu correction", ros::this_node::getName().c_str()); return {}; } @@ -632,6 +645,28 @@ std::optional::MeasurementStamped> Correctio break; } + case MessageType_t::MAG_HDG: { + + if (!sh_mag_hdg_.hasMsg()) { + return {}; + } + + auto msg = sh_mag_hdg_.getMsg(); + measurement_stamped.stamp = msg->header.stamp; + /* checkMsgDelay(measurement_stamped.stamp); */ + + /* if (!is_delay_ok_) { */ + /* return {}; */ + /* } */ + auto res = getCorrectionFromMagHeading(msg); + if (res) { + measurement_stamped.value = res.value(); + } else { + return {}; + } + break; + } + case MessageType_t::POINT: { if (!sh_point_.hasMsg()) { @@ -918,9 +953,10 @@ std::optional::measurement_t> Correction::measurement_t> Correctiontransformer->getTransform(ch_->frames.ns_fcu_untilted, ch_->frames.ns_fcu, ros::Time::now()); + if (res) { + orientation = res.value().transform.rotation; + } else { + ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(), + ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str()); + return {}; + } + + measurement_t measurement; + measurement(0) = mrs_lib::AttitudeConverter(orientation).getHeadingRate(msg->angular_velocity); + return measurement; + break; + } + + default: { + ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromImu() switch", getPrintName().c_str()); + return {}; + } + } break; } } - ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str()); return {}; } @@ -1343,6 +1400,69 @@ std::optional::measurement_t> Correction +void Correction::callbackMagHeading(const mrs_msgs::Float64Stamped::ConstPtr msg) { + + if (!is_initialized_) { + return; + } + + auto res = getCorrectionFromMagHeading(msg); + if (res) { + applyCorrection(res.value(), msg->header.stamp); + } else { + ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain correction from Float64Stamped msg", getPrintName().c_str()); + } +} +/*//}*/ + +/*//{ getCorrectionFromMagHeading() */ +template +std::optional::measurement_t> Correction::getCorrectionFromMagHeading( + const mrs_msgs::Float64StampedConstPtr msg) { + + switch (est_type_) { + + // handle lateral estimators + case EstimatorType_t::LATERAL: { + + ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::LATERAL in getCorrectionFromMagHeading() not implemented", getPrintName().c_str()); + return {}; + break; + } + + // handle altitude estimators + case EstimatorType_t::ALTITUDE: { + + ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::ALTITUDE in getCorrectionFromMagHeading() not implemented", getPrintName().c_str()); + return {}; + break; + } + + // handle heading estimators + case EstimatorType_t::HEADING: { + + measurement_t measurement; + + const double mag_hdg = msg->value / 180 * M_PI; + + if (!got_first_mag_hdg_) { + mag_hdg_previous_ = mag_hdg; + got_first_mag_hdg_ = true; + } + + measurement(0) = -mrs_lib::geometry::radians::unwrap(mag_hdg, mag_hdg_previous_) + M_PI / 2; // may be weirdness of px4 heading (NED vs ENU or something) + mag_hdg_previous_ = mag_hdg; + return measurement; + } + } + + ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str()); + return {}; +} +/*//}*/ + /*//{ callbackPoint() */ template void Correction::callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg) { @@ -1665,6 +1785,23 @@ std::optional::measurement_t> Correction +std::optional Correction::transformVecToFrame(const geometry_msgs::Vector3& vec_in, + const std_msgs::Header& source_header, const std::string target_frame) { + + geometry_msgs::Vector3Stamped vec; + vec.header = source_header; + vec.vector = vec_in; + + auto res = ch_->transformer->transformSingle(vec, target_frame); + if (res) { + return res.value().vector; + } else { + ROS_WARN_THROTTLE(1.0, "[%s]: Transform of vector from %s to %s failed.", getPrintName().c_str(), vec.header.frame_id.c_str(), target_frame.c_str()); + return {}; + } +} + template std::optional::measurement_t> Correction::getVecInFrame(const geometry_msgs::Vector3& vec_in, const std_msgs::Header& source_header, @@ -1672,19 +1809,12 @@ std::optional::measurement_t> Correctiontransformer->transformSingle(vec, target_frame); + auto res = transformVecToFrame(vec_in, source_header, target_frame); if (res) { - transformed_vec = res.value(); - measurement(0) = transformed_vec.vector.x; - measurement(1) = transformed_vec.vector.y; + measurement(0) = res.value().x; + measurement(1) = res.value().y; return measurement; } else { - ROS_WARN_THROTTLE(1.0, "[%s]: Transform of vector from %s to %s failed.", getPrintName().c_str(), vec.header.frame_id.c_str(), target_frame.c_str()); return {}; } } From 535e1fe3c934e8eb3fcf0424a7f3da7e87db8e7f Mon Sep 17 00:00:00 2001 From: Matej Petrlik Date: Thu, 26 Sep 2024 15:33:25 +0200 Subject: [PATCH 2/9] added subtracting of magnetic declination from magnetic heading --- .../estimators/correction.h | 3 + .../mag_declination/geo_mag_declination.h | 90 +++++++++++ .../mag_declination/geo_magnetic_tables.hpp | 73 +++++++++ .../processors/proc_mag_declination.h | 147 ++++++++++++++++++ 4 files changed, 313 insertions(+) create mode 100644 include/mrs_uav_state_estimators/processors/mag_declination/geo_mag_declination.h create mode 100644 include/mrs_uav_state_estimators/processors/mag_declination/geo_magnetic_tables.hpp create mode 100644 include/mrs_uav_state_estimators/processors/proc_mag_declination.h diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h index a46675c..a0efd33 100644 --- a/include/mrs_uav_state_estimators/estimators/correction.h +++ b/include/mrs_uav_state_estimators/estimators/correction.h @@ -35,6 +35,7 @@ #include #include #include +#include #include @@ -2010,6 +2011,8 @@ std::shared_ptr> Correction::createPro return std::make_shared>(nh, getNamespacedName(), name, ch_, ph_); } else if (name == "tf_to_world") { return std::make_shared>(nh, getNamespacedName(), name, ch_, ph_); + } else if (name == "mag_declination") { + return std::make_shared>(nh, getNamespacedName(), name, ch_, ph_); } else { ROS_ERROR("[%s]: requested invalid processor %s", getPrintName().c_str(), name.c_str()); ros::shutdown(); 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 new file mode 100644 index 0000000..9ada7a4 --- /dev/null +++ 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 +#include + +namespace mrs_uav_state_estimators +{ + +class GeoMagDeclination { + +public: + /*//{ constrain() */ + template + 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 + static T degrees(T radians) { + return radians * (static_cast(180) / static_cast(M_PI)); + } + /*//}*/ + + /*//{ getLookupTableIndex() */ + static unsigned getLookupTableIndex(float *val, float min, float max) { + *val = GeoMagDeclination::constrain(*val, min, max - SAMPLING_RES); + + return static_cast((-(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 new file mode 100644 index 0000000..48feb91 --- /dev/null +++ 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 + +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 new file mode 100644 index 0000000..375edb1 --- /dev/null +++ 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 +#include + +#include + +#include +#include +#include + +namespace mrs_uav_state_estimators +{ + +using namespace mrs_uav_managers::estimation_manager; + +template +class ProcMagDeclination : public Processor { + +public: + typedef Eigen::Matrix measurement_t; + +public: + ProcMagDeclination(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, const std::shared_ptr& ch, + const std::shared_ptr& ph); + + std::tuple 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 sh_gnss_; + void callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg); +}; + +/*//{ constructor */ +template +ProcMagDeclination::ProcMagDeclination(ros::NodeHandle& nh, const std::string& correction_name, const std::string& name, + const std::shared_ptr& ch, const std::shared_ptr& ph) + : Processor(nh, correction_name, name, ch, ph) { + + ph->param_loader->setPrefix(ch->package_name + "/" + Support::toSnakeCase(ch->nodelet_name) + "/" + Processor::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::getPrintName().c_str()); + ros::shutdown(); + } + + // | -------------- initialize subscribe handlers ------------- | + mrs_lib::SubscribeHandlerOptions shopts; + shopts.nh = nh; + shopts.node_name = Processor::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(shopts, gnss_topic_, &ProcMagDeclination::callbackGnss, this); + + is_initialized_ = true; +} +/*//}*/ + +/*//{ callbackGnss() */ +template +void ProcMagDeclination::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::getPrintName().c_str()); + return; + } + + if (!std::isfinite(msg->longitude)) { + ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in GNSS variable \"msg->longitude\"!!!", Processor::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::getPrintName().c_str(), gnss_lat_, gnss_lon_); + got_gnss_ = true; +} +/*//}*/ + +/*//{ process() */ +template +std::tuple ProcMagDeclination::process(measurement_t& measurement) { + + if (!Processor::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::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 +void ProcMagDeclination::reset() { + got_gnss_ = false; +} +/*//}*/ +} // namespace mrs_uav_state_estimators + +#endif // PROCESSORS_PROC_MAG_DECLINATION From 88818256e35f0bf772dc65e749e74382be4fbc9d Mon Sep 17 00:00:00 2001 From: Matej Petrlik Date: Mon, 30 Sep 2024 16:33:57 +0200 Subject: [PATCH 3/9] added mag_field correction --- .../estimators/correction.h | 115 ++++++++++++++++++ 1 file changed, 115 insertions(+) diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h index a0efd33..ab3eb75 100644 --- a/include/mrs_uav_state_estimators/estimators/correction.h +++ b/include/mrs_uav_state_estimators/estimators/correction.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -61,6 +62,7 @@ typedef enum IMU, RTK_GPS, MAG_HDG, + MAG_FIELD, POINT, VECTOR, QUAT, @@ -74,6 +76,7 @@ const std::map map_msg_type{{"nav_msgs/Odometry", Me {"sensor_msgs/Imu", MessageType_t::IMU}, {"mrs_msgs/RtkGps", MessageType_t::RTK_GPS}, {"mrs_msgs/Float64Stamped", MessageType_t::MAG_HDG}, + {"sensor_msgs/MagneticField", MessageType_t::MAG_FIELD}, {"geometry_msgs/PointStamped", MessageType_t::POINT}, {"geometry_msgs/Vector3Stamped", MessageType_t::VECTOR}, {"geometry_msgs/QuaternionStamped", MessageType_t::QUAT}}; @@ -171,6 +174,10 @@ class Correction { double mag_hdg_previous_; bool got_first_mag_hdg_; + mrs_lib::SubscribeHandler sh_mag_field_; + std::optional getCorrectionFromMagField(const sensor_msgs::MagneticFieldConstPtr msg); + void callbackMagField(const sensor_msgs::MagneticField::ConstPtr msg); + mrs_lib::SubscribeHandler sh_range_; std::optional getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg); void callbackRange(const sensor_msgs::Range::ConstPtr msg); @@ -365,6 +372,10 @@ Correction::Correction(ros::NodeHandle& nh, const std::string& e sh_mag_hdg_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackMagHeading, this); break; } + case MessageType_t::MAG_FIELD: { + sh_mag_field_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackMagField, this); + break; + } case MessageType_t::POINT: { sh_point_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackPoint, this); break; @@ -668,6 +679,28 @@ std::optional::MeasurementStamped> Correctio break; } + case MessageType_t::MAG_FIELD: { + + if (!sh_mag_field_.hasMsg()) { + return {}; + } + + auto msg = sh_mag_field_.getMsg(); + measurement_stamped.stamp = msg->header.stamp; + /* checkMsgDelay(measurement_stamped.stamp); */ + + /* if (!is_delay_ok_) { */ + /* return {}; */ + /* } */ + auto res = getCorrectionFromMagField(msg); + if (res) { + measurement_stamped.value = res.value(); + } else { + return {}; + } + break; + } + case MessageType_t::POINT: { if (!sh_point_.hasMsg()) { @@ -1464,6 +1497,88 @@ std::optional::measurement_t> Correction +void Correction::callbackMagField(const sensor_msgs::MagneticField::ConstPtr msg) { + + if (!is_initialized_) { + return; + } + + auto res = getCorrectionFromMagField(msg); + if (res) { + applyCorrection(res.value(), msg->header.stamp); + } else { + ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain correction from sensor_msgs::MagneticField msg", getPrintName().c_str()); + } +} +/*//}*/ + +/*//{ getCorrectionFromMagField() */ +template +std::optional::measurement_t> Correction::getCorrectionFromMagField( + const sensor_msgs::MagneticFieldConstPtr msg) { + + switch (est_type_) { + + // handle lateral estimators + case EstimatorType_t::LATERAL: { + + ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::LATERAL in getCorrectionFromMagField() not implemented", getPrintName().c_str()); + return {}; + break; + } + + // handle altitude estimators + case EstimatorType_t::ALTITUDE: { + + ROS_ERROR_THROTTLE(1.0, "[%s]: EstimatorType_t::ALTITUDE in getCorrectionFromMagField() not implemented", getPrintName().c_str()); + return {}; + break; + } + + // handle heading estimators + case EstimatorType_t::HEADING: { + + Eigen::Matrix3d rot; + + auto res = ch_->transformer->getTransform(ch_->frames.ns_fcu, ch_->frames.ns_fcu_untilted, msg->header.stamp); + if (res) { + rot = Eigen::Matrix3d(mrs_lib::AttitudeConverter(res.value().transform.rotation)); + } else { + ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(), + ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str()); + return {}; + } + + const Eigen::Vector3d mag_vec(msg->magnetic_field.x, msg->magnetic_field.y, msg->magnetic_field.z); + const Eigen::Vector3d proj_mag_field = rot * mag_vec; + /* mrs_msgs::Float64Stamped hdg_stamped; */ + /* hdg_stamped.header = msg->header; */ + /* hdg_stamped.value = atan2(proj_mag_field.y(), proj_mag_field.x()); */ + const double mag_hdg = atan2(proj_mag_field.y(), proj_mag_field.x()); + + measurement_t measurement; + + /* const double mag_hdg = msg->value / 180 * M_PI; */ + + if (!got_first_mag_hdg_) { + mag_hdg_previous_ = mag_hdg; + got_first_mag_hdg_ = true; + } + + measurement(0) = -mrs_lib::geometry::radians::unwrap(mag_hdg, mag_hdg_previous_) + M_PI / 2; // may be weirdness of px4 heading (NED vs ENU or something) + /* measurement(0) = mrs_lib::geometry::radians::unwrap(mag_hdg, mag_hdg_previous_); */ + mag_hdg_previous_ = mag_hdg; + return measurement; + } + } + + ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str()); + return {}; +} +/*//}*/ + /*//{ callbackPoint() */ template void Correction::callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg) { From a4023851853c7354ed8a6abe4c7be8c5f736c90f Mon Sep 17 00:00:00 2001 From: Matej Petrlik Date: Mon, 30 Sep 2024 18:06:16 +0200 Subject: [PATCH 4/9] position (lateral) correction from NavSatFix msg working in sim --- .../estimators/correction.h | 193 ++++++++++++++++-- 1 file changed, 171 insertions(+), 22 deletions(-) diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h index ab3eb75..00a2695 100644 --- a/include/mrs_uav_state_estimators/estimators/correction.h +++ b/include/mrs_uav_state_estimators/estimators/correction.h @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -61,6 +62,7 @@ typedef enum RANGE, IMU, RTK_GPS, + NAVSATFIX, MAG_HDG, MAG_FIELD, POINT, @@ -75,6 +77,7 @@ const std::map map_msg_type{{"nav_msgs/Odometry", Me {"sensor_msgs/Range", MessageType_t::RANGE}, {"sensor_msgs/Imu", MessageType_t::IMU}, {"mrs_msgs/RtkGps", MessageType_t::RTK_GPS}, + {"sensor_msgs/NavSatFix", MessageType_t::NAVSATFIX}, {"mrs_msgs/Float64Stamped", MessageType_t::MAG_HDG}, {"sensor_msgs/MagneticField", MessageType_t::MAG_FIELD}, {"geometry_msgs/PointStamped", MessageType_t::POINT}, @@ -142,10 +145,15 @@ class Correction { mrs_lib::SubscribeHandler sh_rtk_; void callbackRtk(const mrs_msgs::RtkGps::ConstPtr msg); std::optional getCorrectionFromRtk(const mrs_msgs::RtkGpsConstPtr msg); - void getAvgRtkInitZ(const double rtk_z); - bool got_avg_init_rtk_z_ = false; - double rtk_init_z_avg_ = 0.0; - int got_rtk_counter_ = 0; + + mrs_lib::SubscribeHandler sh_navsatfix_; + void callbackNavSatFix(const sensor_msgs::NavSatFix::ConstPtr msg); + std::optional getCorrectionFromNavSatFix(const sensor_msgs::NavSatFixConstPtr msg); + + void getAvgInitZ(const double z); + bool got_avg_init_z_ = false; + double init_z_avg_ = 0.0; + int got_z_counter_ = 0; mrs_lib::SubscribeHandler sh_point_; void callbackPoint(const geometry_msgs::PointStamped::ConstPtr msg); @@ -175,8 +183,8 @@ class Correction { bool got_first_mag_hdg_; mrs_lib::SubscribeHandler sh_mag_field_; - std::optional getCorrectionFromMagField(const sensor_msgs::MagneticFieldConstPtr msg); - void callbackMagField(const sensor_msgs::MagneticField::ConstPtr msg); + std::optional getCorrectionFromMagField(const sensor_msgs::MagneticFieldConstPtr msg); + void callbackMagField(const sensor_msgs::MagneticField::ConstPtr msg); mrs_lib::SubscribeHandler sh_range_; std::optional getCorrectionFromRange(const sensor_msgs::RangeConstPtr msg); @@ -368,6 +376,10 @@ Correction::Correction(ros::NodeHandle& nh, const std::string& e sh_rtk_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackRtk, this); break; } + case MessageType_t::NAVSATFIX: { + sh_navsatfix_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackNavSatFix, this); + break; + } case MessageType_t::MAG_HDG: { sh_mag_hdg_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackMagHeading, this); break; @@ -657,6 +669,33 @@ std::optional::MeasurementStamped> Correctio break; } + case MessageType_t::NAVSATFIX: { + + if (!sh_navsatfix_.hasMsg()) { + ROS_ERROR_THROTTLE(1.0, " no navsatfix msgs so far"); + return {}; + } + + auto msg = sh_navsatfix_.getMsg(); + measurement_stamped.stamp = msg->header.stamp; + /* checkMsgDelay(measurement_stamped.stamp); */ + + /* if (!is_delay_ok_) { */ + /* ROS_ERROR("[%s]: rtk msg delay not ok", ros::this_node::getName().c_str()); */ + /* return {}; */ + /* } */ + + auto res = getCorrectionFromNavSatFix(msg); + if (res) { + measurement_stamped.value = res.value(); + } else { + ROS_ERROR_THROTTLE(1.0, "[%s]: could not get navsatfix correction", ros::this_node::getName().c_str()); + return {}; + } + + break; + } + case MessageType_t::MAG_HDG: { if (!sh_mag_hdg_.hasMsg()) { @@ -1405,11 +1444,11 @@ std::optional::measurement_t> Correction::measurement_t> Correction +void Correction::callbackNavSatFix(const sensor_msgs::NavSatFix::ConstPtr msg) { + + if (!is_initialized_) { + return; + } + + auto res = getCorrectionFromNavSatFix(msg); + if (res) { + applyCorrection(res.value(), msg->header.stamp); + } +} +/*//}*/ + +/*//{ getCorrectionFromNavSatFix() */ +template +std::optional::measurement_t> Correction::getCorrectionFromNavSatFix( + const sensor_msgs::NavSatFixConstPtr msg) { + + geometry_msgs::PointStamped navsatfix_pos; + + if (!std::isfinite(msg->latitude)) { + ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in NavSatFix variable \"msg->latitude\"!!!", getPrintName().c_str()); + return {}; + } + + if (!std::isfinite(msg->longitude)) { + ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in NavSatFix variable \"msg->longitude\"!!!", getPrintName().c_str()); + return {}; + } + + if (!std::isfinite(msg->altitude)) { + ROS_ERROR_THROTTLE(1.0, "[%s] NaN detected in NavSatFix variable \"msg->altitude\"!!!", getPrintName().c_str()); + return {}; + } + + if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) { + ROS_ERROR_THROTTLE(1.0, "[%s] NavSatFix has no GNSS fix!!!", getPrintName().c_str()); + return {}; + } + + navsatfix_pos.header = msg->header; + mrs_lib::UTM(msg->latitude, msg->longitude, &navsatfix_pos.point.x, &navsatfix_pos.point.y); + navsatfix_pos.point.x -= ch_->world_origin.x; + navsatfix_pos.point.y -= ch_->world_origin.y; + navsatfix_pos.point.z = msg->altitude; + + Correction::measurement_t measurement; + + // TODO transform position from GNSS antenna to FCU + + switch (est_type_) { + + // handle lateral estimators + case EstimatorType_t::LATERAL: { + + switch (state_id_) { + + case StateId_t::POSITION: { + measurement(0) = navsatfix_pos.point.x; + measurement(1) = navsatfix_pos.point.y; + return measurement; + break; + } + + default: { + ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromNavSatFix() switch", getPrintName().c_str()); + return {}; + } + } + break; + } + + // handle altitude estimators + case EstimatorType_t::ALTITUDE: { + + switch (state_id_) { + + case StateId_t::POSITION: { + measurement(0) = navsatfix_pos.point.z; + if (!got_avg_init_z_) { + getAvgInitZ(measurement(0)); + return {}; + } + measurement(0) -= init_z_avg_; + return measurement; + break; + } + + default: { + ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromNavSatFix() switch", getPrintName().c_str()); + return {}; + } + } + break; + } + + case EstimatorType_t::HEADING: { + ROS_ERROR_THROTTLE(1.0, "[%s]: should not be possible to get into this branch of getCorrectionFromNavSatFix() switch", getPrintName().c_str()); + return {}; + break; + } + } + + ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str()); + return {}; +} +/*//}*/ + /*//{ callbackMagHeading() */ template void Correction::callbackMagHeading(const mrs_msgs::Float64Stamped::ConstPtr msg) { @@ -1552,7 +1701,7 @@ std::optional::measurement_t> Correctionmagnetic_field.x, msg->magnetic_field.y, msg->magnetic_field.z); - const Eigen::Vector3d proj_mag_field = rot * mag_vec; + const Eigen::Vector3d proj_mag_field = rot * mag_vec; /* mrs_msgs::Float64Stamped hdg_stamped; */ /* hdg_stamped.header = msg->header; */ /* hdg_stamped.value = atan2(proj_mag_field.y(), proj_mag_field.x()); */ @@ -2003,27 +2152,27 @@ std::optional Correction::transformRtkToFcu } /*//}*/ -/*//{ getAvgRtkInitZ() */ +/*//{ getAvgInitZ() */ template -void Correction::getAvgRtkInitZ(const double rtk_z) { +void Correction::getAvgInitZ(const double z) { - if (!got_avg_init_rtk_z_) { + if (!got_avg_init_z_) { - double rtk_avg = rtk_init_z_avg_ / got_rtk_counter_; + double z_avg = init_z_avg_ / got_z_counter_; - if (got_rtk_counter_ < 10 || (got_rtk_counter_ < 300 && std::fabs(rtk_z - rtk_avg) > 0.1)) { + if (got_z_counter_ < 10 || (got_z_counter_ < 300 && std::fabs(z - z_avg) > 0.1)) { - rtk_init_z_avg_ += rtk_z; - got_rtk_counter_++; - rtk_avg = rtk_init_z_avg_ / got_rtk_counter_; - ROS_INFO("[%s]: RTK ASL altitude sample #%d: %.2f; avg: %.2f", getPrintName().c_str(), got_rtk_counter_, rtk_z, rtk_avg); + init_z_avg_ += z; + got_z_counter_++; + z_avg = init_z_avg_ / got_z_counter_; + ROS_INFO("[%s]: AMSL altitude sample #%d: %.2f; avg: %.2f", getPrintName().c_str(), got_z_counter_, z, z_avg); return; } else { - rtk_init_z_avg_ = rtk_avg; - got_avg_init_rtk_z_ = true; - ROS_INFO("[%s]: RTK ASL altitude avg: %f", getPrintName().c_str(), rtk_avg); + init_z_avg_ = z_avg; + got_avg_init_z_ = true; + ROS_INFO("[%s]: AMSL altitude avg: %f", getPrintName().c_str(), z_avg); } } } From 9397d4240d463ef230bc33ebb33b42a710295c83 Mon Sep 17 00:00:00 2001 From: Matej Petrlik Date: Tue, 1 Oct 2024 17:41:24 +0200 Subject: [PATCH 5/9] passthrough - kickoff compatibility fix --- .../mrs_uav_state_estimators/estimators/state/passthrough.h | 2 +- src/estimators/state/passthrough.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/mrs_uav_state_estimators/estimators/state/passthrough.h b/include/mrs_uav_state_estimators/estimators/state/passthrough.h index 08b9a1a..9e8811e 100644 --- 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 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/src/estimators/state/passthrough.cpp b/src/estimators/state/passthrough.cpp index 64fc5ef..8f81784 100644 --- 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_ptrparam_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 From a541da25816f00c63572a41914ce3214a7fd2452 Mon Sep 17 00:00:00 2001 From: Matej Petrlik Date: Thu, 17 Oct 2024 17:44:40 +0200 Subject: [PATCH 6/9] added latalt estimator type --- .../estimators/correction.h | 254 +++++++++++++++++- 1 file changed, 246 insertions(+), 8 deletions(-) diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h index 00a2695..3954c09 100644 --- a/include/mrs_uav_state_estimators/estimators/correction.h +++ b/include/mrs_uav_state_estimators/estimators/correction.h @@ -49,9 +49,10 @@ typedef enum { LATERAL, ALTITUDE, - HEADING + HEADING, + LATALT, } EstimatorType_t; -const int n_EstimatorType_t = 3; +const int n_EstimatorType_t = 4; typedef enum { @@ -1062,6 +1063,68 @@ std::optional::measurement_t> Correctionheader; + header.frame_id = transform_from_frame_; + auto res = getInFrame(msg->pose.pose.position, header, transform_to_frame_); + if (res) { + measurement_t measurement; + measurement(0) = res.value().x; + measurement(1) = res.value().y; + measurement(2) = res.value().z; + return measurement; + } else { + ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vel from odom", ros::this_node::getName().c_str()); + return {}; + } + + } else { + measurement(0) = msg->pose.pose.position.x; + measurement(1) = msg->pose.pose.position.y; + measurement(2) = msg->pose.pose.position.z; + } + return measurement; + break; + } + + case StateId_t::VELOCITY: { + if (is_in_body_frame_) { + std_msgs::Header header = msg->header; + header.frame_id = ch_->frames.ns_fcu; // message in odometry is published in body frame + auto res = getVecInFrame(msg->twist.twist.linear, header, ns_frame_id_ + "_att_only"); + if (res) { + measurement_t measurement; + measurement = res.value(); + return measurement; + } else { + ROS_WARN_THROTTLE(1.0, "[%s]: could not transform vel from odom", ros::this_node::getName().c_str()); + return {}; + } + } else { + measurement_t measurement; + measurement(0) = msg->twist.twist.linear.x; + measurement(1) = msg->twist.twist.linear.y; + measurement(2) = msg->twist.twist.linear.z; + return measurement; + } + break; + } + + default: { + ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); + return {}; + } + } + break; + } } ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str()); @@ -1165,6 +1228,29 @@ std::optional::measurement_t> Correctionpose.position.x; + measurement(1) = msg->pose.position.y; + measurement(2) = msg->pose.position.z; + return measurement; + break; + } + + default: { + ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoseStamped() switch", getPrintName().c_str()); + return {}; + } + } + break; + } + } ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str()); @@ -1297,7 +1383,7 @@ std::optional::measurement_t> Correctionlinear_acceleration, msg->header); if (res) { measurement_t measurement; - measurement = res.value(); + measurement = res.value(); measurement(0) -= gravity_norm_; return measurement; } else { @@ -1349,6 +1435,41 @@ std::optional::measurement_t> Correctionlinear_acceleration, msg->header, ns_frame_id_ + "_att_only"); + if (res) { + measurement_t measurement; + measurement = res.value(); + return measurement; + } else { + ROS_WARN_THROTTLE(1.0, "[%s]: Could not obtain IMU acceleration in frame: %s", getPrintName().c_str(), (ns_frame_id_ + "_att_only").c_str()); + return {}; + } + } else { + measurement_t measurement; + measurement(0) = msg->linear_acceleration.x; + measurement(1) = msg->linear_acceleration.y; + measurement(2) = msg->linear_acceleration.z; + return measurement; + } + break; + } + + default: { + ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromImu() switch", getPrintName().c_str()); + return {}; + } + } + break; + } + } ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str()); return {}; @@ -1466,6 +1587,27 @@ std::optional::measurement_t> Correction::measurement_t> Correction::measurement_t> Correction::measurement_t> Correction::measurement_t> Correction::measurement_t> Correctionpoint.x; + measurement(1) = msg->point.y; + measurement(2) = msg->point.z; + return measurement; + break; + } + + default: { + ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromPoint() switch", getPrintName().c_str()); return {}; } } @@ -1792,7 +2001,7 @@ std::optional::measurement_t> Correction::measurement_t> Correctionvector, msg->header, ns_frame_id_ + "_att_only"); + if (res) { + measurement_t measurement; + measurement = res.value(); + return measurement; + } else { + return {}; + } + break; + } + + default: { + ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str()); + return {}; + } + } + break; + } + } ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str()); @@ -2049,7 +2284,7 @@ std::optional::measurement_t> Correction std::optional Correction::transformVecToFrame(const geometry_msgs::Vector3& vec_in, const std_msgs::Header& source_header, const std::string target_frame) { @@ -2078,6 +2313,9 @@ std::optional::measurement_t> Correction Date: Wed, 23 Oct 2024 22:31:38 +0200 Subject: [PATCH 7/9] added possibility to transform mag field vector --- .../estimators/correction.h | 50 ++++++++++++------- 1 file changed, 33 insertions(+), 17 deletions(-) diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h index 3954c09..bbc9283 100644 --- a/include/mrs_uav_state_estimators/estimators/correction.h +++ b/include/mrs_uav_state_estimators/estimators/correction.h @@ -387,6 +387,7 @@ Correction::Correction(ros::NodeHandle& nh, const std::string& e } case MessageType_t::MAG_FIELD: { sh_mag_field_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackMagField, this); + break; } case MessageType_t::POINT: { @@ -1228,7 +1229,7 @@ std::optional::measurement_t> Correction::measurement_t> Correction::measurement_t> Correctionlinear_acceleration, msg->header); if (res) { measurement_t measurement; - measurement = res.value(); + measurement = res.value(); measurement(0) -= gravity_norm_; return measurement; } else { @@ -1469,7 +1469,6 @@ std::optional::measurement_t> Correction::measurement_t> Correction::measurement_t> Correctiontransformer->getTransform(ch_->frames.ns_fcu, ch_->frames.ns_fcu_untilted, msg->header.stamp); - if (res) { - rot = Eigen::Matrix3d(mrs_lib::AttitudeConverter(res.value().transform.rotation)); - } else { - ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(), - ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str()); - return {}; + /* auto res = ch_->transformer->getTransform(ch_->frames.ns_fcu, ch_->frames.ns_fcu_untilted, msg->header.stamp); */ + /* if (res) { */ + /* rot = Eigen::Matrix3d(mrs_lib::AttitudeConverter(res.value().transform.rotation)); */ + /* } else { */ + /* ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(), */ + /* ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str()); */ + /* return {}; */ + /* } */ + + geometry_msgs::Vector3 mag_vec; + mag_vec.x = msg->magnetic_field.x; + mag_vec.y = msg->magnetic_field.y; + mag_vec.z = msg->magnetic_field.z; + + if (transform_to_frame_enabled_) { + std_msgs::Header header = msg->header; + header.frame_id = transform_from_frame_; + auto res = transformVecToFrame(mag_vec, header, transform_to_frame_); + if (res) { + mag_vec.x = res.value().x; + mag_vec.y = res.value().y; + mag_vec.z = res.value().z; + } else { + ROS_WARN_THROTTLE(1.0, "[%s]: could not transform mag field vector", getPrintName().c_str()); + } } - const Eigen::Vector3d mag_vec(msg->magnetic_field.x, msg->magnetic_field.y, msg->magnetic_field.z); - const Eigen::Vector3d proj_mag_field = rot * mag_vec; + const double mag_hdg = atan2(mag_vec.y, mag_vec.x); + /* const Eigen::Vector3d mag_vec(mag_vec_pt.x, mag_vec_pt.y, mag_vec_pt.z); */ + /* const Eigen::Vector3d proj_mag_field = rot * mag_vec; */ /* mrs_msgs::Float64Stamped hdg_stamped; */ /* hdg_stamped.header = msg->header; */ /* hdg_stamped.value = atan2(proj_mag_field.y(), proj_mag_field.x()); */ - const double mag_hdg = atan2(proj_mag_field.y(), proj_mag_field.x()); + /* const double mag_hdg = atan2(proj_mag_field.y(), proj_mag_field.x()); */ measurement_t measurement; @@ -2142,7 +2159,6 @@ std::optional::measurement_t> Correction Date: Thu, 24 Oct 2024 23:45:40 +0200 Subject: [PATCH 8/9] mag field fusion seems working --- .../estimators/correction.h | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h index bbc9283..2a1a22f 100644 --- a/include/mrs_uav_state_estimators/estimators/correction.h +++ b/include/mrs_uav_state_estimators/estimators/correction.h @@ -199,6 +199,7 @@ class Correction { mrs_lib::PublisherHandler ph_correction_raw_; mrs_lib::PublisherHandler ph_correction_proc_; mrs_lib::PublisherHandler ph_delay_; + mrs_lib::PublisherHandler ph_mag_field_untilted_; const std::string est_name_; const std::string name_; @@ -387,6 +388,7 @@ Correction::Correction(ros::NodeHandle& nh, const std::string& e } case MessageType_t::MAG_FIELD: { sh_mag_field_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Correction::callbackMagField, this); + ph_mag_field_untilted_ = mrs_lib::PublisherHandler(nh, est_name_ + "/correction/" + getName() + "/fcu_untilted", 10); break; } @@ -1887,7 +1889,14 @@ std::optional::measurement_t> Correctionheader.stamp; + mag_vec_msg.header.frame_id = transform_to_frame_; + mag_vec_msg.point.x = mag_vec.x; + mag_vec_msg.point.y = mag_vec.y; + mag_vec_msg.point.z = mag_vec.z; + ph_mag_field_untilted_.publish(mag_vec_msg); const double mag_hdg = atan2(mag_vec.y, mag_vec.x); /* const Eigen::Vector3d mag_vec(mag_vec_pt.x, mag_vec_pt.y, mag_vec_pt.z); */ /* const Eigen::Vector3d proj_mag_field = rot * mag_vec; */ @@ -1905,7 +1914,7 @@ std::optional::measurement_t> Correction Date: Tue, 26 Nov 2024 11:09:12 +0100 Subject: [PATCH 9/9] moved readme to the docs --- README.md | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/README.md b/README.md index 8316ed2..925f3b3 100644 --- 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/).