diff --git a/config/public/transform_manager/transform_manager.yaml b/config/public/transform_manager/transform_manager.yaml index 82d1102f..dcf925a2 100644 --- a/config/public/transform_manager/transform_manager.yaml +++ b/config/public/transform_manager/transform_manager.yaml @@ -32,6 +32,11 @@ mrs_uav_managers: parent: "fcu" # fcu should be the parent if using the default inverted mrs tf tree convention with fcu as the root of the tf tree child: "fcu_untilted" + altitude_amsl_tf: + enabled: true # amsl tf should be published by default unless some other node already publishes it + parent: "fcu_untilted" # fcu should be the parent if using the default inverted mrs tf tree convention with fcu as the root of the tf tree + child: "amsl_origin" + # the list of additional source topics from which the tfs will be constructed tf_sources: [] diff --git a/include/mrs_uav_managers/estimation_manager/common_handlers.h b/include/mrs_uav_managers/estimation_manager/common_handlers.h index 66c2b2b2..9bea9dcb 100644 --- a/include/mrs_uav_managers/estimation_manager/common_handlers.h +++ b/include/mrs_uav_managers/estimation_manager/common_handlers.h @@ -20,6 +20,8 @@ struct CommonFrames_t std::string ns_fcu_untilted; std::string rtk_antenna; std::string ns_rtk_antenna; + std::string amsl; + std::string ns_amsl; }; struct WorldOrigin_t diff --git a/launch/transform_manager.launch b/launch/transform_manager.launch index 86542910..3f905a63 100644 --- a/launch/transform_manager.launch +++ b/launch/transform_manager.launch @@ -61,6 +61,7 @@ + diff --git a/src/transform_manager/transform_manager.cpp b/src/transform_manager/transform_manager.cpp index 68cf3c4c..7c03106d 100644 --- a/src/transform_manager/transform_manager.cpp +++ b/src/transform_manager/transform_manager.cpp @@ -13,6 +13,7 @@ #include #include +#include #include #include @@ -83,6 +84,10 @@ class TransformManager : public nodelet::Nodelet { 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_; + bool publish_amsl_origin_tf_; + std::string world_origin_units_; geometry_msgs::Point world_origin_; @@ -114,6 +119,9 @@ class TransformManager : public nodelet::Nodelet { mrs_lib::SubscribeHandler sh_height_agl_; void callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg); + mrs_lib::SubscribeHandler sh_altitude_amsl_; + void callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::ConstPtr msg); + mrs_lib::SubscribeHandler sh_hw_api_orientation_; void callbackHwApiOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg); @@ -259,6 +267,18 @@ void TransformManager::onInit() { param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/enabled", publish_fcu_untilted_tf_); /*//}*/ +/*//{ load amsl_origin parameters*/ + std::string amsl_parent_frame_id, amsl_child_frame_id; + param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/enabled", publish_amsl_origin_tf_); + param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/parent", amsl_parent_frame_id); + param_loader.loadParam(yaml_prefix + "altitude_amsl_tf/child", amsl_child_frame_id); + ch_->frames.amsl = amsl_child_frame_id; + ch_->frames.ns_amsl = ch_->uav_name + "/" + amsl_child_frame_id; + ns_amsl_origin_parent_frame_id_ = ch_->uav_name + "/" + amsl_parent_frame_id; + ns_amsl_origin_child_frame_id_ = ch_->uav_name + "/" + amsl_child_frame_id; + +/*//}*/ + param_loader.loadParam("mrs_uav_managers/estimation_manager/state_estimators", estimator_names_); param_loader.loadParam(yaml_prefix + "tf_sources", tf_source_names_); @@ -371,6 +391,8 @@ void TransformManager::onInit() { sh_height_agl_ = mrs_lib::SubscribeHandler(shopts, "height_agl_in", &TransformManager::callbackHeightAgl, this); + sh_altitude_amsl_ = mrs_lib::SubscribeHandler(shopts, "altitude_amsl_in", &TransformManager::callbackAltitudeAmsl, this); + sh_hw_api_orientation_ = mrs_lib::SubscribeHandler(shopts, "orientation_in", &TransformManager::callbackHwApiOrientation, this); @@ -595,7 +617,7 @@ void TransformManager::callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPt return; } - mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::publish>HeightAgl", ch_->scope_timer.logger, ch_->scope_timer.enabled); + mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackHeightAgl", ch_->scope_timer.logger, ch_->scope_timer.enabled); geometry_msgs::TransformStamped tf_msg; tf_msg.header.stamp = msg->header.stamp; @@ -626,6 +648,45 @@ void TransformManager::callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPt } /*//}*/ +/*//{ callbackAltitudeAmsl() */ + +void TransformManager::callbackAltitudeAmsl(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); + + geometry_msgs::TransformStamped tf_msg; + tf_msg.header.stamp = msg->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.rotation.x = 0; + tf_msg.transform.rotation.y = 0; + tf_msg.transform.rotation.z = 0; + tf_msg.transform.rotation.w = 1; + + 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()); +} +/*//}*/ + /*//{ callbackHwApiOrientation() */ void TransformManager::callbackHwApiOrientation(const geometry_msgs::QuaternionStamped::ConstPtr msg) {