From 249073a674033e9662a7df94170639fad615fd0a Mon Sep 17 00:00:00 2001 From: Matej Petrlik Date: Thu, 25 Jan 2024 14:42:07 +0100 Subject: [PATCH] changed local_origin frame to be consistent with the description on the wiki (before this commit the tf still depended on the original estimator after switching to a new one) --- include/transform_manager/tf_source.h | 2 +- src/transform_manager/transform_manager.cpp | 93 ++++++++++----------- 2 files changed, 47 insertions(+), 48 deletions(-) diff --git a/include/transform_manager/tf_source.h b/include/transform_manager/tf_source.h index 6725d84..36233b1 100644 --- a/include/transform_manager/tf_source.h +++ b/include/transform_manager/tf_source.h @@ -208,7 +208,7 @@ class TfSource { bool is_inverted_; bool is_utm_based_; - bool publish_local_tf_ = true; + bool publish_local_tf_ = false; bool publish_utm_tf_ = false; bool publish_world_tf_ = false; std::atomic_bool is_utm_source_ = false; diff --git a/src/transform_manager/transform_manager.cpp b/src/transform_manager/transform_manager.cpp index ff839b7..2a1ad19 100644 --- a/src/transform_manager/transform_manager.cpp +++ b/src/transform_manager/transform_manager.cpp @@ -82,6 +82,7 @@ class TransformManager : public nodelet::Nodelet { std::string ns_fixed_origin_parent_frame_id_; std::string ns_fixed_origin_child_frame_id_; bool publish_fixed_origin_tf_; + geometry_msgs::Pose pose_first_; geometry_msgs::Pose pose_fixed_; geometry_msgs::Pose pose_fixed_diff_; @@ -106,6 +107,7 @@ class TransformManager : public nodelet::Nodelet { std::shared_ptr ch_; std::shared_ptr broadcaster_; + tf2_ros::StaticTransformBroadcaster static_broadcaster_; std::unique_ptr tf_mapping_origin_; @@ -115,7 +117,8 @@ class TransformManager : public nodelet::Nodelet { void callbackUavState(const mrs_msgs::UavState::ConstPtr msg); std::string first_frame_id_; std::string last_frame_id_; - bool is_first_frame_id_set_ = false; + bool is_first_frame_id_set_ = false; + bool is_local_static_tf_published_ = false; mrs_lib::SubscribeHandler sh_height_agl_; void callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg); @@ -136,6 +139,8 @@ class TransformManager : public nodelet::Nodelet { std::optional transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const; void publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg); + + void publishLocalTf(); }; /*//}*/ @@ -444,55 +449,15 @@ void TransformManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) first_frame_id_ = msg->header.frame_id; last_frame_id_ = msg->header.frame_id; pose_fixed_ = msg->pose; + pose_first_ = msg->pose; pose_fixed_diff_.orientation.w = 1; - is_first_frame_id_set_ = true; - } - - if (publish_local_origin_tf_) { - /*//{ publish local_origin tf*/ - geometry_msgs::TransformStamped tf_msg; - tf_msg.header.stamp = msg->header.stamp; - tf_msg.header.frame_id = ns_local_origin_parent_frame_id_; - tf_msg.child_frame_id = ns_local_origin_child_frame_id_; - - // transform pose to first frame_id - geometry_msgs::PoseStamped pose; - pose.header = msg->header; - pose.pose = msg->pose; - - if (pose.pose.orientation.w == 0 && pose.pose.orientation.z == 0 && pose.pose.orientation.y == 0 && pose.pose.orientation.x == 0) { - ROS_WARN_ONCE("[%s]: Uninitialized quaternion detected during publishing stable_origin tf of %s. Setting w=1", getPrintName().c_str(), - pose.header.frame_id.c_str()); - pose.pose.orientation.w = 1.0; - } - auto res = ch_->transformer->transformSingle(pose, first_frame_id_.substr(0, first_frame_id_.find("_origin")) + "_local_origin"); - - if (res) { - const tf2::Transform tf = Support::tf2FromPose(res->pose); - const tf2::Transform tf_inv = tf.inverse(); - const geometry_msgs::Pose pose_inv = Support::poseFromTf2(tf_inv); - tf_msg.transform.translation = Support::pointToVector3(pose_inv.position); - tf_msg.transform.rotation = pose_inv.orientation; + is_first_frame_id_set_ = true; + } - if (Support::noNans(tf_msg)) { - try { - broadcaster_->sendTransform(tf_msg); - } - catch (...) { - ROS_ERROR("exception caught "); - } - } else { - ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(), - tf_msg.child_frame_id.c_str()); - } - 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()); - } else { - ROS_ERROR_THROTTLE(1.0, "[%s]: Could not transform pose to %s. Not publishing local_origin transform.", getPrintName().c_str(), first_frame_id_.c_str()); - return; - } - /*//}*/ + // publish static tf from fixed_origin to local_origin based on the first message + if (publish_local_origin_tf_ && !is_local_static_tf_published_) { + publishLocalTf(); } if (publish_stable_origin_tf_) { @@ -582,6 +547,7 @@ void TransformManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) /*//{ choose another source of utm and world tfs after estimator switch */ if (msg->header.frame_id != last_frame_id_) { + const std::string last_estimator_name = Support::frameIdToEstimatorName(last_frame_id_); const std::string current_estimator_name = Support::frameIdToEstimatorName(msg->header.frame_id); @@ -890,6 +856,39 @@ void TransformManager::publishFcuUntiltedTf(const geometry_msgs::QuaternionStamp } /*//}*/ +/* publishLocalTf() //{*/ +void TransformManager::publishLocalTf() { + + mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer(getPrintName() + "::publishLocalTf", ch_->scope_timer.logger, ch_->scope_timer.enabled); + + geometry_msgs::TransformStamped tf_msg; + tf_msg.header.stamp = ros::Time::now(); + + tf_msg.header.frame_id = ns_fixed_origin_child_frame_id_; + tf_msg.child_frame_id = ns_local_origin_child_frame_id_; + tf_msg.transform.translation = Support::pointToVector3(pose_first_.position); + tf_msg.transform.rotation = pose_first_.orientation; + + if (Support::noNans(tf_msg)) { + + try { + static_broadcaster_.sendTransform(tf_msg); + } + catch (...) { + ROS_ERROR("[%s]: exception caught while publishing tf from %s to %s", getPrintName().c_str(), tf_msg.header.frame_id.c_str(), + tf_msg.child_frame_id.c_str()); + } + + } else { + ROS_WARN_THROTTLE(1.0, "[%s]: NaN detected in transform from %s to %s. Not publishing tf.", getPrintName().c_str(), tf_msg.header.frame_id.c_str(), + tf_msg.child_frame_id.c_str()); + } + 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()); + is_local_static_tf_published_ = true; +} +/*//}*/ + /*//{ transformRtkToFcu() */ std::optional TransformManager::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {