From 901a85c132a47dec2683e1d32bd338413f7abf69 Mon Sep 17 00:00:00 2001 From: Matej Petrlik Date: Wed, 25 Sep 2024 15:44:17 +0200 Subject: [PATCH] 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 {}; } }