Skip to content

Commit

Permalink
changed local_origin frame to be consistent with the description on the
Browse files Browse the repository at this point in the history 
wiki (before this commit the tf still depended on the original estimator
after switching to a new one)
  • Loading branch information
@petrlmat
petrlmat committed Jan 25, 2024
1 parent a611daf commit 249073a
Show file tree
Hide file tree
Showing 2 changed files with 47 additions and 48 deletions.
2 changes: 1 addition & 1 deletion include/transform_manager/tf_source.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
93 changes: 46 additions & 47 deletions src/transform_manager/transform_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;

Expand All @@ -106,6 +107,7 @@ class TransformManager : public nodelet::Nodelet {
std::shared_ptr<estimation_manager::CommonHandlers_t> ch_;

std::shared_ptr<mrs_lib::TransformBroadcaster> broadcaster_;
tf2_ros::StaticTransformBroadcaster static_broadcaster_;

std::unique_ptr<TfMappingOrigin> tf_mapping_origin_;

Expand All @@ -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<mrs_msgs::Float64Stamped> sh_height_agl_;
void callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg);
Expand All @@ -136,6 +139,8 @@ class TransformManager : public nodelet::Nodelet {
std::optional<geometry_msgs::Pose> transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const;

void publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg);

void publishLocalTf();
};
/*//}*/

Expand Down Expand Up @@ -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_) {
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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<geometry_msgs::Pose> TransformManager::transformRtkToFcu(const geometry_msgs::PoseStamped& pose_in) const {

Expand Down

0 comments on commit 249073a

Please sign in to comment.