From 66342a07276d56645647f5adf8f73003459b38f1 Mon Sep 17 00:00:00 2001 From: Matej Petrlik Date: Thu, 7 Nov 2024 15:54:37 +0100 Subject: [PATCH] amsl_origin and utm_origin should now correctly use altitude from RTK instead of GNSS --- include/transform_manager/tf_source.h | 13 +--- src/transform_manager/transform_manager.cpp | 82 +++++++++++---------- 2 files changed, 46 insertions(+), 49 deletions(-) diff --git a/include/transform_manager/tf_source.h b/include/transform_manager/tf_source.h index 07890dd..01661b9 100644 --- a/include/transform_manager/tf_source.h +++ b/include/transform_manager/tf_source.h @@ -121,10 +121,6 @@ class TfSource { sh_tf_source_att_ = mrs_lib::SubscribeHandler(shopts, full_topic_attitude_, &TfSource::callbackTfSourceAtt, this); } - if (is_utm_source_) { - sh_altitude_amsl_ = mrs_lib::SubscribeHandler(shopts, "altitude_amsl_in"); - } - } /*//}*/ @@ -172,7 +168,6 @@ class TfSource { shopts.autostart = true; shopts.queue_size = 10; shopts.transport_hints = ros::TransportHints().tcpNoDelay(); - sh_altitude_amsl_ = mrs_lib::SubscribeHandler(shopts, "altitude_amsl_in"); } is_utm_source_ = is_utm_source; } @@ -249,7 +244,6 @@ class TfSource { mrs_lib::SubscribeHandler sh_tf_source_odom_; mrs_lib::SubscribeHandler sh_tf_source_att_; - mrs_lib::SubscribeHandler sh_altitude_amsl_; nav_msgs::OdometryConstPtr first_msg_; bool got_first_msg_ = false; @@ -372,12 +366,7 @@ class TfSource { geometry_msgs::Pose pose_utm = odom->pose.pose; pose_utm.position.x += utm_origin_.x - first_msg_->pose.pose.position.x; pose_utm.position.y += utm_origin_.y - first_msg_->pose.pose.position.y; - - if (sh_altitude_amsl_.hasMsg()) { - pose_utm.position.z = sh_altitude_amsl_.getMsg()->amsl; - } else { - ROS_WARN_THROTTLE(5.0, "[%s]: AMSL altitude not available.", getPrintName().c_str()); - } + pose_utm.position.z += utm_origin_.z - first_msg_->pose.pose.position.z; tf_utm_msg.header.stamp = odom->header.stamp; tf_utm_msg.header.frame_id = ns_utm_origin_parent_frame_id_; diff --git a/src/transform_manager/transform_manager.cpp b/src/transform_manager/transform_manager.cpp index 87c81f2..0ab42f0 100644 --- a/src/transform_manager/transform_manager.cpp +++ b/src/transform_manager/transform_manager.cpp @@ -79,12 +79,12 @@ class TransformManager : public nodelet::Nodelet { std::string ns_stable_origin_child_frame_id_; bool publish_stable_origin_tf_; - std::string ns_fixed_origin_parent_frame_id_; - std::string ns_fixed_origin_child_frame_id_; - bool publish_fixed_origin_tf_; + std::string ns_fixed_origin_parent_frame_id_; + std::string ns_fixed_origin_child_frame_id_; + bool publish_fixed_origin_tf_; geometry_msgs::PoseStamped pose_first_; - geometry_msgs::Pose pose_fixed_; - geometry_msgs::Pose pose_fixed_diff_; + geometry_msgs::Pose pose_fixed_; + geometry_msgs::Pose pose_fixed_diff_; std::string ns_amsl_origin_parent_frame_id_; std::string ns_amsl_origin_child_frame_id_; @@ -141,6 +141,8 @@ class TransformManager : public nodelet::Nodelet { void publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg); void publishLocalTf(); + + void publishAmslTf(const double altitude, const ros::Time& stamp); }; /*//}*/ @@ -637,24 +639,36 @@ void TransformManager::callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPt } /*//}*/ -/*//{ callbackAltitudeAmsl() */ +/*//{ callbackAmslAltitude() */ -void TransformManager::callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::ConstPtr msg) { +void TransformManager::callbackAltitudeAmsl([[maybe_unused]] const mrs_msgs::HwApiAltitude::ConstPtr msg) { if (!is_initialized_) { return; } - mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackAltitudeAmsl", ch_->scope_timer.logger, ch_->scope_timer.enabled); + // Currently not used. Not clear what this message from hw_api should be for. Transform manager publishes the AMSL altitude from RTK or GNSS messages. + +} +/*//}*/ + +/*//{ publishAmslTf() */ + +void TransformManager::publishAmslTf(const double altitude, const ros::Time& stamp) { + if (!is_initialized_) { + return; + } + + // Currently not used. Not clear what this message should be for. Altitude data is taken eiter from RTK or GNSS. geometry_msgs::TransformStamped tf_msg; - tf_msg.header.stamp = msg->stamp; + tf_msg.header.stamp = stamp; tf_msg.header.frame_id = ch_->frames.ns_fcu_untilted; tf_msg.child_frame_id = ch_->frames.ns_amsl; tf_msg.transform.translation.x = 0; tf_msg.transform.translation.y = 0; - tf_msg.transform.translation.z = -msg->amsl; + tf_msg.transform.translation.z = -altitude; tf_msg.transform.rotation.x = 0; tf_msg.transform.rotation.y = 0; tf_msg.transform.rotation.z = 0; @@ -673,6 +687,7 @@ void TransformManager::callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::Const } ROS_INFO_ONCE("[%s]: Broadcasting transform from parent frame: %s to child frame: %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(), tf_msg.child_frame_id.c_str()); + } /*//}*/ @@ -698,10 +713,6 @@ void TransformManager::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) return; } - if (got_utm_offset_) { - return; - } - mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackGnss", ch_->scope_timer.logger, ch_->scope_timer.enabled); double out_x; @@ -724,6 +735,12 @@ void TransformManager::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) utm_origin.y = out_y; utm_origin.z = msg->altitude; + publishAmslTf(msg->altitude, msg->header.stamp); + + if (got_utm_offset_) { + return; + } + ROS_INFO("[%s]: utm_origin position calculated as: x: %.2f, y: %.2f, z: %.2f from GNSS", getPrintName().c_str(), utm_origin.x, utm_origin.y, utm_origin.z); for (size_t i = 0; i < tf_sources_.size(); i++) { @@ -741,15 +758,8 @@ void TransformManager::callbackRtkGps(const mrs_msgs::RtkGps::ConstPtr msg) { return; } - if (got_utm_offset_) { - return; - } - mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackRtkGps", ch_->scope_timer.logger, ch_->scope_timer.enabled); - double out_x; - double out_y; - geometry_msgs::PoseStamped rtk_pos; if (!std::isfinite(msg->gps.latitude)) { @@ -777,10 +787,6 @@ void TransformManager::callbackRtkGps(const mrs_msgs::RtkGps::ConstPtr msg) { rtk_pos.pose.position.z = msg->gps.altitude; rtk_pos.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0); - rtk_pos.pose.position.x -= ch_->world_origin.x; - rtk_pos.pose.position.y -= ch_->world_origin.y; - - // transform the RTK position from antenna to FCU auto res = transformRtkToFcu(rtk_pos); if (res) { @@ -790,23 +796,17 @@ void TransformManager::callbackRtkGps(const mrs_msgs::RtkGps::ConstPtr msg) { return; } - mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &out_x, &out_y); + geometry_msgs::Point utm_origin; + utm_origin.x = rtk_pos.pose.position.x; + utm_origin.y = rtk_pos.pose.position.y; + utm_origin.z = rtk_pos.pose.position.z; - if (!std::isfinite(out_x)) { - ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_x\"!!!", getPrintName().c_str()); - return; - } + publishAmslTf(rtk_pos.pose.position.z, msg->header.stamp); - if (!std::isfinite(out_y)) { - ROS_ERROR_THROTTLE(1.0, "[%s]: NaN detected in UTM variable \"out_y\"!!!", getPrintName().c_str()); + if (got_utm_offset_) { return; } - geometry_msgs::Point utm_origin; - utm_origin.x = out_x; - utm_origin.y = out_y; - utm_origin.z = msg->gps.altitude; - ROS_INFO("[%s]: utm_origin position calculated as: x: %.2f, y: %.2f, z: %.2f from RTK msg", getPrintName().c_str(), utm_origin.x, utm_origin.y, utm_origin.z); for (size_t i = 0; i < tf_sources_.size(); i++) { @@ -896,6 +896,14 @@ void TransformManager::publishLocalTf() { /*//{ transformRtkToFcu() */ std::optional TransformManager::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const { + // Check if the RTK antenna static tf is defined + auto res_tf_rtk = ch_->transformer->getTransform(ch_->frames.ns_rtk_antenna, ch_->frames.ns_fcu, ros::Time::now()); + if (!res_tf_rtk) { + ROS_ERROR("[%s]: The transform from FCU to RTK antenna is not defined. Please provide static tf from %s to %s.", getPrintName().c_str(), ch_->frames.ns_fcu.c_str(), ch_->frames.ns_rtk_antenna.c_str()); + ros::shutdown(); + return {}; + } + geometry_msgs::PoseStamped pose_tmp = pose_in; // inject current orientation into rtk pose