diff --git a/config/public/transform_manager/transform_manager.yaml b/config/public/transform_manager/transform_manager.yaml index 5394069e..abf9037a 100644 --- a/config/public/transform_manager/transform_manager.yaml +++ b/config/public/transform_manager/transform_manager.yaml @@ -32,7 +32,7 @@ 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" - slam_mapping_origin_tf: + mapping_origin_tf: enabled: true lateral_topic: "slam/odom" # name of the topic used for x, y components of the tf (expects nav_msgs/Odometry topic type) altitude_topic: "slam/odom" # name of the topic used for z components of the tf (expects nav_msgs/Odometry topic type) @@ -40,7 +40,7 @@ mrs_uav_managers: inverted: true # publish as inverted tf (the default for inverted mrs tf tree convention) custom_frame_id: enabled: true - frame_id: "slam_mapping_origin" + frame_id: "mapping_origin" # the list of additional source topics from which the tfs will be constructed diff --git a/include/transform_manager/tf_mapping_origin.h b/include/transform_manager/tf_mapping_origin.h new file mode 100644 index 00000000..5b4c0b5a --- /dev/null +++ b/include/transform_manager/tf_mapping_origin.h @@ -0,0 +1,346 @@ +#pragma once +#ifndef TF_MAPPING_ORIGIN_H +#define TF_MAPPING_ORIGIN_H + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +namespace mrs_uav_managers +{ + +/*//{ class TfMappingOrigin */ +class TfMappingOrigin { + + using Support = estimation_manager::Support; + +public: + /*//{ constructor */ + TfMappingOrigin(ros::NodeHandle nh, const std::shared_ptr& broadcaster, + const std::shared_ptr ch) + : nh_(nh), broadcaster_(broadcaster), ch_(ch) { + + ROS_INFO("[%s]: initializing", getPrintName().c_str()); + + + mrs_lib::ParamLoader param_loader(nh_, getPrintName()); + + const std::string yaml_prefix = "mrs_uav_managers/transform_manager/mapping_origin_tf/"; + + /*//{ load mapping origin parameters */ + param_loader.loadParam(yaml_prefix + "lateral_topic", mapping_origin_tf_lateral_topic_); + param_loader.loadParam(yaml_prefix + "altitude_topic", mapping_origin_tf_altitude_topic_); + param_loader.loadParam(yaml_prefix + "orientation_topic", mapping_origin_tf_orientation_topic_); + param_loader.loadParam(yaml_prefix + "inverted", mapping_origin_tf_inverted_); + param_loader.loadParam(yaml_prefix + "custom_frame_id/enabled", mapping_origin_tf_custom_frame_id_enabled_); + if (mapping_origin_tf_custom_frame_id_enabled_) { + param_loader.loadParam(yaml_prefix + "custom_frame_id/frame_id", mapping_origin_tf_custom_frame_id_); + } + + if (!param_loader.loadedSuccessfully()) { + ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str()); + ros::shutdown(); + } + + /*//}*/ + + /*//{ initialize subscribers */ + mrs_lib::SubscribeHandlerOptions shopts; + shopts.nh = nh_; + shopts.node_name = getPrintName(); + shopts.no_message_timeout = mrs_lib::no_timeout; + shopts.threadsafe = true; + shopts.autostart = true; + shopts.queue_size = 10; + shopts.transport_hints = ros::TransportHints().tcpNoDelay(); + + sh_mapping_odom_lat_ = mrs_lib::SubscribeHandler(shopts, "/" + ch_->uav_name + "/" + mapping_origin_tf_lateral_topic_, + &TfMappingOrigin::callbackMappingOdomLat, this); + + if (mapping_origin_tf_orientation_topic_ != mapping_origin_tf_lateral_topic_) { + sh_mapping_odom_rot_ = mrs_lib::SubscribeHandler( + shopts, "/" + ch_->uav_name + "/" + mapping_origin_tf_orientation_topic_.c_str(), &TfMappingOrigin::callbackMappingOdomRot, this); + } + + if (mapping_origin_tf_altitude_topic_ != mapping_origin_tf_lateral_topic_) { + sh_mapping_odom_alt_ = mrs_lib::SubscribeHandler(shopts, "/" + ch_->uav_name + "/" + mapping_origin_tf_altitude_topic_.c_str(), + &TfMappingOrigin::callbackMappingOdomAlt, this); + } + /*//}*/ + + is_initialized_ = true; + ROS_INFO("[%s]: initialized", getPrintName().c_str()); + } + /*//}*/ + + /*//{ getName() */ + std::string getName() { + return name_; + } + /*//}*/ + + /*//{ getPrintName() */ + std::string getPrintName() { + return ch_->nodelet_name + "/" + name_; + } + /*//}*/ + +private: + const std::string name_ = "TfMappingOrigin"; + + ros::NodeHandle nh_; + + std::shared_ptr broadcaster_; + + std::shared_ptr ch_; + + std::atomic_bool is_initialized_ = false; + + bool mapping_origin_tf_inverted_; + bool mapping_origin_tf_custom_frame_id_enabled_; + std::string mapping_origin_tf_custom_frame_id_; + double mapping_origin_tf_cache_duration_; + + std::string mapping_origin_tf_lateral_topic_; + mrs_lib::SubscribeHandler sh_mapping_odom_lat_; + std::vector vec_mapping_odom_lat_; + + /*//{ callbackMappingOdomLat() */ + + void callbackMappingOdomLat(const nav_msgs::Odometry::ConstPtr msg) { + + if (!is_initialized_) { + return; + } + + if (!got_mapping_odom_rot_ && mapping_origin_tf_orientation_topic_ == mapping_origin_tf_lateral_topic_) { + got_mapping_odom_rot_ = true; + } + + if (!got_mapping_odom_alt_ && mapping_origin_tf_altitude_topic_ == mapping_origin_tf_lateral_topic_) { + got_mapping_odom_alt_ = true; + } + + mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TfMappingOrigin::callbackMappingOdomLat", ch_->scope_timer.logger, ch_->scope_timer.enabled); + + const double hdg_mapping_old = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeading(); + + /* publish aloam mapping origin tf //{ */ + + bool clear_needed = false; + + if (got_mapping_odom_rot_ && got_mapping_odom_alt_) { + std::scoped_lock lock(mtx_mapping_odom_rot_); + + // Copy mapping odometry + nav_msgs::Odometry mapping_odom; + mapping_odom = *msg; + + // Find corresponding orientation + tf2::Quaternion tf2_rot; + geometry_msgs::QuaternionStamped rot_tmp = *sh_mapping_odom_rot_.getMsg(); // start with newest msg + + for (size_t i = 0; i < vec_mapping_odom_rot_.size(); i++) { + if (mapping_odom.header.stamp < vec_mapping_odom_rot_.at(i).header.stamp) { + + // Choose an orientation with closest timestamp + double time_diff = std::fabs(vec_mapping_odom_rot_.at(i).header.stamp.toSec() - mapping_odom.header.stamp.toSec()); + double time_diff_prev = std::numeric_limits::max(); + if (i > 0) { + time_diff_prev = std::fabs(vec_mapping_odom_rot_.at(i - 1).header.stamp.toSec() - mapping_odom.header.stamp.toSec()); + } + if (time_diff_prev < time_diff && i > 0) { + i = i - 1; + } + + // Cache is too small if it is full and its oldest element is used + if (clear_needed && i == 0) { + ROS_WARN_THROTTLE(1.0, "[%s] Mapping orientation cache is too small.", getPrintName().c_str()); + } + rot_tmp = vec_mapping_odom_rot_.at(i); + break; + } + } + + tf2_rot = mrs_lib::AttitudeConverter(rot_tmp.quaternion); + + // Obtain heading from orientation + double hdg = 0; + try { + hdg = mrs_lib::AttitudeConverter(rot_tmp.quaternion).getHeading(); + } + catch (...) { + ROS_WARN("[%s]: failed to getHeading() from rot_tmp", getPrintName().c_str()); + } + + // Build rotation matrix from difference between new heading and old heading + tf2::Matrix3x3 rot_mat = mrs_lib::AttitudeConverter(Eigen::AngleAxisd(hdg_mapping_old - hdg, Eigen::Vector3d::UnitZ())); + + // Transform the mavros orientation by the rotation matrix + geometry_msgs::Quaternion new_orientation = mrs_lib::AttitudeConverter(tf2::Transform(rot_mat) * tf2_rot); + + // Set new orientation + mapping_odom.pose.pose.orientation = new_orientation; + + + // Find corresponding local odom + double odom_alt = msg->pose.pose.position.z; + for (size_t i = 0; i < vec_mapping_odom_alt_.size(); i++) { + if (mapping_odom.header.stamp < vec_mapping_odom_alt_.at(i).header.stamp) { + + // Choose orientation with closest timestamp + double time_diff = std::fabs(vec_mapping_odom_alt_.at(i).header.stamp.toSec() - mapping_odom.header.stamp.toSec()); + double time_diff_prev = std::numeric_limits::max(); + if (i > 0) { + time_diff_prev = std::fabs(vec_mapping_odom_alt_.at(i - 1).header.stamp.toSec() - mapping_odom.header.stamp.toSec()); + } + if (time_diff_prev < time_diff && i > 0) { + i = i - 1; + } + // Cache is too small if it is full and its oldest element is used + if (clear_needed && i == 0) { + ROS_WARN_THROTTLE(1.0, "[%s] mapping orientation cache (for slam mapping tf) is too small.", getPrintName().c_str()); + } + odom_alt = vec_mapping_odom_alt_.at(i).pose.pose.position.z; + break; + } + } + + // Set altitude + mapping_odom.pose.pose.position.z = odom_alt; + + // Get inverse transform + tf2::Transform tf_mapping_inv = Support::tf2FromPose(mapping_odom.pose.pose).inverse(); + geometry_msgs::Pose pose_mapping_inv = Support::poseFromTf2(tf_mapping_inv); + + geometry_msgs::TransformStamped tf_mapping; + tf_mapping.header.stamp = mapping_odom.header.stamp; + tf_mapping.header.frame_id = ch_->frames.ns_fcu; + if (mapping_origin_tf_custom_frame_id_enabled_) { + tf_mapping.child_frame_id = ch_->uav_name + "/" + mapping_origin_tf_custom_frame_id_; + } else { + tf_mapping.child_frame_id = mapping_odom.header.frame_id; + } + tf_mapping.transform.translation = Support::pointToVector3(pose_mapping_inv.position); + tf_mapping.transform.rotation = pose_mapping_inv.orientation; + + if (Support::noNans(tf_mapping)) { + try { + broadcaster_->sendTransform(tf_mapping); + } + catch (...) { + ROS_ERROR("[%s]: Exception caught during publishing TF: %s - %s.", getPrintName().c_str(), tf_mapping.child_frame_id.c_str(), + tf_mapping.header.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_mapping.header.frame_id.c_str(), + tf_mapping.child_frame_id.c_str()); + } + } + + //} + } + /*//}*/ + + std::string mapping_origin_tf_orientation_topic_; + mrs_lib::SubscribeHandler sh_mapping_odom_rot_; + std::vector vec_mapping_odom_rot_; + std::mutex mtx_mapping_odom_rot_; + bool got_mapping_odom_rot_ = false; + + /*//{ callbackMappingOdomRot() */ + void callbackMappingOdomRot(const geometry_msgs::QuaternionStamped::ConstPtr msg) { + + if (!is_initialized_) { + return; + } + std::scoped_lock lock(mtx_mapping_odom_rot_); + + const ros::Time time_now = ros::Time::now(); + + // Add new data + vec_mapping_odom_rot_.push_back(*msg); + + // Delete old data + size_t index_delete = 0; + bool clear_needed = false; + for (size_t i = 0; i < vec_mapping_odom_rot_.size(); i++) { + if (time_now - vec_mapping_odom_rot_.at(i).header.stamp > ros::Duration(mapping_origin_tf_cache_duration_)) { + index_delete = i; + clear_needed = true; + } else { + break; + } + } + if (clear_needed) { + for (int i = (int)index_delete; i >= 0; i--) { + vec_mapping_odom_rot_.erase(vec_mapping_odom_rot_.begin() + i); + } + clear_needed = false; + } + + if (!got_mapping_odom_rot_) { + got_mapping_odom_rot_ = true; + } + } + /*//}*/ + + std::string mapping_origin_tf_altitude_topic_; + mrs_lib::SubscribeHandler sh_mapping_odom_alt_; + std::vector vec_mapping_odom_alt_; + std::mutex mtx_mapping_odom_alt_; + bool got_mapping_odom_alt_ = false; + + /*//{ callbackMappingOdomAlt() */ + void callbackMappingOdomAlt(const nav_msgs::Odometry::ConstPtr msg) { + + if (!is_initialized_) { + return; + } + + std::scoped_lock lock(mtx_mapping_odom_alt_); + + const ros::Time time_now = ros::Time::now(); + + // Add new data + vec_mapping_odom_alt_.push_back(*msg); + + // Delete old data + size_t index_delete = 0; + bool clear_needed = false; + for (size_t i = 0; i < vec_mapping_odom_alt_.size(); i++) { + if (time_now - vec_mapping_odom_alt_.at(i).header.stamp > ros::Duration(mapping_origin_tf_cache_duration_)) { + index_delete = i; + clear_needed = true; + } else { + break; + } + } + if (clear_needed) { + for (int i = (int)index_delete; i >= 0; i--) { + vec_mapping_odom_alt_.erase(vec_mapping_odom_alt_.begin() + i); + } + clear_needed = false; + } + + if (!got_mapping_odom_alt_) { + got_mapping_odom_alt_ = true; + } + } + /*//}*/ +}; +/*//}*/ + +} // namespace mrs_uav_managers + +#endif diff --git a/src/transform_manager/transform_manager.cpp b/src/transform_manager/transform_manager.cpp index 3de7c94a..9a34bb1b 100644 --- a/src/transform_manager/transform_manager.cpp +++ b/src/transform_manager/transform_manager.cpp @@ -28,6 +28,7 @@ #include #include #include +#include /*//}*/ @@ -90,13 +91,14 @@ class TransformManager : public nodelet::Nodelet { std::mutex mtx_broadcast_utm_origin_; std::mutex mtx_broadcast_world_origin_; - ros::NodeHandle nh_; std::shared_ptr ch_; std::shared_ptr broadcaster_; + std::unique_ptr tf_mapping_origin_; + void timeoutCallback(const std::string& topic, const ros::Time& last_msg); mrs_lib::SubscribeHandler sh_uav_state_; @@ -115,32 +117,6 @@ class TransformManager : public nodelet::Nodelet { void callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg); std::atomic got_utm_offset_ = false; - - bool slam_mapping_origin_tf_enabled_; - bool slam_mapping_origin_tf_inverted_; - bool slam_mapping_origin_tf_custom_frame_id_enabled_; - std::string slam_mapping_origin_tf_custom_frame_id_; - double slam_mapping_origin_tf_cache_duration_; - - std::string slam_mapping_origin_tf_lateral_topic_; - mrs_lib::SubscribeHandler sh_mapping_odom_lat_; - void callbackMappingOdomLat(const nav_msgs::Odometry::ConstPtr msg); - std::vector vec_mapping_odom_lat_; - - std::string slam_mapping_origin_tf_orientation_topic_; - mrs_lib::SubscribeHandler sh_mapping_odom_rot_; - void callbackMappingOdomRot(const geometry_msgs::QuaternionStamped::ConstPtr msg); - std::vector vec_mapping_odom_rot_; - std::mutex mtx_mapping_odom_rot_; - bool got_mapping_odom_rot_ = false; - - std::string slam_mapping_origin_tf_altitude_topic_; - mrs_lib::SubscribeHandler sh_mapping_odom_alt_; - void callbackMappingOdomAlt(const nav_msgs::Odometry::ConstPtr msg); - std::vector vec_mapping_odom_alt_; - std::mutex mtx_mapping_odom_alt_; - bool got_mapping_odom_alt_ = false; - void publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg); }; /*//}*/ @@ -259,20 +235,6 @@ void TransformManager::onInit() { param_loader.loadParam(yaml_prefix + "fcu_untilted_tf/enabled", publish_fcu_untilted_tf_); /*//}*/ - /*//{ load slam mapping origin parameters */ - param_loader.loadParam(yaml_prefix + "/slam_mapping_origin_tf/enabled", slam_mapping_origin_tf_enabled_); - if (slam_mapping_origin_tf_enabled_) { - param_loader.loadParam(yaml_prefix + "/slam_mapping_origin_tf/lateral_topic", slam_mapping_origin_tf_lateral_topic_); - param_loader.loadParam(yaml_prefix + "/slam_mapping_origin_tf/altitude_topic", slam_mapping_origin_tf_altitude_topic_); - param_loader.loadParam(yaml_prefix + "/slam_mapping_origin_tf/orientation_topic", slam_mapping_origin_tf_orientation_topic_); - param_loader.loadParam(yaml_prefix + "/slam_mapping_origin_tf/inverted", slam_mapping_origin_tf_inverted_); - param_loader.loadParam(yaml_prefix + "/slam_mapping_origin_tf/custom_frame_id/enabled", slam_mapping_origin_tf_custom_frame_id_enabled_); - if (slam_mapping_origin_tf_custom_frame_id_enabled_) { - param_loader.loadParam(yaml_prefix + "/slam_mapping_origin_tf/custom_frame_id/frame_id", slam_mapping_origin_tf_custom_frame_id_); - } - } - /*//}*/ - param_loader.loadParam("mrs_uav_managers/estimation_manager/state_estimators", estimator_names_); param_loader.loadParam(yaml_prefix + "tf_sources", tf_source_names_); @@ -300,6 +262,13 @@ void TransformManager::onInit() { ROS_INFO("[%s]: loading tf source of estimator: %s", getPrintName().c_str(), estimator_name.c_str()); tf_sources_.push_back(std::make_unique(estimator_name, nh_, broadcaster_, ch_, is_utm_source)); } + + // initialize mapping_origin tf + bool mapping_origin_tf_enabled; + param_loader.loadParam(yaml_prefix + "mapping_origin_tf/enabled", mapping_origin_tf_enabled); + if (mapping_origin_tf_enabled) { + tf_mapping_origin_ = std::make_unique(nh_, broadcaster_, ch_); + } //} /*//{ initialize subscribers */ @@ -320,21 +289,6 @@ void TransformManager::onInit() { mrs_lib::SubscribeHandler(shopts, "orientation_in", &TransformManager::callbackHwApiOrientation, this); sh_gnss_ = mrs_lib::SubscribeHandler(shopts, "gnss_in", &TransformManager::callbackGnss, this); - - if (slam_mapping_origin_tf_enabled_) { - sh_mapping_odom_lat_ = mrs_lib::SubscribeHandler(shopts, "/" + ch_->uav_name + "/" + slam_mapping_origin_tf_lateral_topic_, - &TransformManager::callbackMappingOdomLat, this); - - if (slam_mapping_origin_tf_orientation_topic_ != slam_mapping_origin_tf_lateral_topic_) { - sh_mapping_odom_rot_ = mrs_lib::SubscribeHandler( - shopts, "/" + ch_->uav_name + "/" + slam_mapping_origin_tf_orientation_topic_.c_str(), &TransformManager::callbackMappingOdomRot, this); - } - - if (slam_mapping_origin_tf_altitude_topic_ != slam_mapping_origin_tf_lateral_topic_) { - sh_mapping_odom_alt_ = mrs_lib::SubscribeHandler(shopts, "/" + ch_->uav_name + "/" + slam_mapping_origin_tf_altitude_topic_.c_str(), - &TransformManager::callbackMappingOdomAlt, this); - } - } /*//}*/ if (!param_loader.loadedSuccessfully()) { @@ -642,218 +596,6 @@ void TransformManager::callbackGnss(const sensor_msgs::NavSatFix::ConstPtr msg) } /*//}*/ -/*//{ callbackMappingOdomLat() */ - -void TransformManager::callbackMappingOdomLat(const nav_msgs::Odometry::ConstPtr msg) { - - if (!is_initialized_) { - return; - } - - if (!got_mapping_odom_rot_ && slam_mapping_origin_tf_orientation_topic_ == slam_mapping_origin_tf_lateral_topic_) { - got_mapping_odom_rot_ = true; - } - - if (!got_mapping_odom_alt_ && slam_mapping_origin_tf_altitude_topic_ == slam_mapping_origin_tf_lateral_topic_) { - got_mapping_odom_alt_ = true; - } - - mrs_lib::ScopeTimer scope_timer = mrs_lib::ScopeTimer("TransformManager::callbackMappingOdomLat", ch_->scope_timer.logger, ch_->scope_timer.enabled); - - const double hdg_mapping_old = mrs_lib::AttitudeConverter(msg->pose.pose.orientation).getHeading(); - - /* publish aloam mapping origin tf //{ */ - - bool clear_needed = false; - - if (got_mapping_odom_rot_ && got_mapping_odom_alt_) { - std::scoped_lock lock(mtx_mapping_odom_rot_); - - // Copy mapping odometry - nav_msgs::Odometry mapping_odom; - mapping_odom = *msg; - - // Find corresponding orientation - tf2::Quaternion tf2_rot; - geometry_msgs::QuaternionStamped rot_tmp = *sh_mapping_odom_rot_.getMsg(); // start with newest msg - - for (size_t i = 0; i < vec_mapping_odom_rot_.size(); i++) { - if (mapping_odom.header.stamp < vec_mapping_odom_rot_.at(i).header.stamp) { - - // Choose an orientation with closest timestamp - double time_diff = std::fabs(vec_mapping_odom_rot_.at(i).header.stamp.toSec() - mapping_odom.header.stamp.toSec()); - double time_diff_prev = std::numeric_limits::max(); - if (i > 0) { - time_diff_prev = std::fabs(vec_mapping_odom_rot_.at(i - 1).header.stamp.toSec() - mapping_odom.header.stamp.toSec()); - } - if (time_diff_prev < time_diff && i > 0) { - i = i - 1; - } - - // Cache is too small if it is full and its oldest element is used - if (clear_needed && i == 0) { - ROS_WARN_THROTTLE(1.0, "[%s] Mapping orientation cache is too small.", getPrintName().c_str()); - } - rot_tmp = vec_mapping_odom_rot_.at(i); - break; - } - } - - tf2_rot = mrs_lib::AttitudeConverter(rot_tmp.quaternion); - - // Obtain heading from orientation - double hdg = 0; - try { - hdg = mrs_lib::AttitudeConverter(rot_tmp.quaternion).getHeading(); - } - catch (...) { - ROS_WARN("[%s]: failed to getHeading() from rot_tmp", getPrintName().c_str()); - } - - // Build rotation matrix from difference between new heading and old heading - tf2::Matrix3x3 rot_mat = mrs_lib::AttitudeConverter(Eigen::AngleAxisd(hdg_mapping_old - hdg, Eigen::Vector3d::UnitZ())); - - // Transform the mavros orientation by the rotation matrix - geometry_msgs::Quaternion new_orientation = mrs_lib::AttitudeConverter(tf2::Transform(rot_mat) * tf2_rot); - - // Set new orientation - mapping_odom.pose.pose.orientation = new_orientation; - - - // Find corresponding local odom - double odom_alt = msg->pose.pose.position.z; - for (size_t i = 0; i < vec_mapping_odom_alt_.size(); i++) { - if (mapping_odom.header.stamp < vec_mapping_odom_alt_.at(i).header.stamp) { - - // Choose orientation with closest timestamp - double time_diff = std::fabs(vec_mapping_odom_alt_.at(i).header.stamp.toSec() - mapping_odom.header.stamp.toSec()); - double time_diff_prev = std::numeric_limits::max(); - if (i > 0) { - time_diff_prev = std::fabs(vec_mapping_odom_alt_.at(i - 1).header.stamp.toSec() - mapping_odom.header.stamp.toSec()); - } - if (time_diff_prev < time_diff && i > 0) { - i = i - 1; - } - // Cache is too small if it is full and its oldest element is used - if (clear_needed && i == 0) { - ROS_WARN_THROTTLE(1.0, "[%s] mapping orientation cache (for slam mapping tf) is too small.", getPrintName().c_str()); - } - odom_alt = vec_mapping_odom_alt_.at(i).pose.pose.position.z; - break; - } - } - - // Set altitude - mapping_odom.pose.pose.position.z = odom_alt; - - // Get inverse transform - tf2::Transform tf_mapping_inv = Support::tf2FromPose(mapping_odom.pose.pose).inverse(); - geometry_msgs::Pose pose_mapping_inv = Support::poseFromTf2(tf_mapping_inv); - - geometry_msgs::TransformStamped tf_mapping; - tf_mapping.header.stamp = mapping_odom.header.stamp; - tf_mapping.header.frame_id = ch_->frames.ns_fcu; - if (slam_mapping_origin_tf_custom_frame_id_enabled_) { - tf_mapping.child_frame_id = ch_->uav_name + "/" + slam_mapping_origin_tf_custom_frame_id_; - } else { - tf_mapping.child_frame_id = mapping_odom.header.frame_id; - } - tf_mapping.transform.translation = Support::pointToVector3(pose_mapping_inv.position); - tf_mapping.transform.rotation = pose_mapping_inv.orientation; - - if (Support::noNans(tf_mapping)) { - try { - broadcaster_->sendTransform(tf_mapping); - } - catch (...) { - ROS_ERROR("[%s]: Exception caught during publishing TF: %s - %s.", getPrintName().c_str(), tf_mapping.child_frame_id.c_str(), - tf_mapping.header.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_mapping.header.frame_id.c_str(), - tf_mapping.child_frame_id.c_str()); - } - } - - //} -} -/*//}*/ - -/*//{ callbackMappingOdomRot() */ -void TransformManager::callbackMappingOdomRot(const geometry_msgs::QuaternionStamped::ConstPtr msg) { - - if (!is_initialized_) { - return; - } - std::scoped_lock lock(mtx_mapping_odom_rot_); - - const ros::Time time_now = ros::Time::now(); - - // Add new data - vec_mapping_odom_rot_.push_back(*msg); - - // Delete old data - size_t index_delete = 0; - bool clear_needed = false; - for (size_t i = 0; i < vec_mapping_odom_rot_.size(); i++) { - if (time_now - vec_mapping_odom_rot_.at(i).header.stamp > ros::Duration(slam_mapping_origin_tf_cache_duration_)) { - index_delete = i; - clear_needed = true; - } else { - break; - } - } - if (clear_needed) { - for (int i = (int)index_delete; i >= 0; i--) { - vec_mapping_odom_rot_.erase(vec_mapping_odom_rot_.begin() + i); - } - clear_needed = false; - } - - if (!got_mapping_odom_rot_) { - got_mapping_odom_rot_ = true; - } -} -/*//}*/ - -/*//{ callbackMappingOdomAlt() */ -void TransformManager::callbackMappingOdomAlt(const nav_msgs::Odometry::ConstPtr msg) { - - if (!is_initialized_) { - return; - } - - std::scoped_lock lock(mtx_mapping_odom_alt_); - - const ros::Time time_now = ros::Time::now(); - - // Add new data - vec_mapping_odom_alt_.push_back(*msg); - - // Delete old data - size_t index_delete = 0; - bool clear_needed = false; - for (size_t i = 0; i < vec_mapping_odom_alt_.size(); i++) { - if (time_now - vec_mapping_odom_alt_.at(i).header.stamp > ros::Duration(slam_mapping_origin_tf_cache_duration_)) { - index_delete = i; - clear_needed = true; - } else { - break; - } - } - if (clear_needed) { - for (int i = (int)index_delete; i >= 0; i--) { - vec_mapping_odom_alt_.erase(vec_mapping_odom_alt_.begin() + i); - } - clear_needed = false; - } - - if (!got_mapping_odom_alt_) { - got_mapping_odom_alt_ = true; - } -} -/*//}*/ - /*//{ publishFcuUntiltedTf() */ void TransformManager::publishFcuUntiltedTf(const geometry_msgs::QuaternionStampedConstPtr& msg) {