Skip to content

Commit

Permalink
amsl_origin and utm_origin should now correctly use altitude from RTK
Browse files Browse the repository at this point in the history 
instead of GNSS
  • Loading branch information
@petrlmat
petrlmat committed Nov 7, 2024
1 parent e31d226 commit 66342a0
Show file tree
Hide file tree
Showing 2 changed files with 46 additions and 49 deletions.
13 changes: 1 addition & 12 deletions include/transform_manager/tf_source.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,10 +121,6 @@ class TfSource {
sh_tf_source_att_ = mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(shopts, full_topic_attitude_, &TfSource::callbackTfSourceAtt, this);
}

if (is_utm_source_) {
sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
}

}
/*//}*/

Expand Down Expand Up @@ -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<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in");
}
is_utm_source_ = is_utm_source;
}
Expand Down Expand Up @@ -249,7 +244,6 @@ class TfSource {

mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_tf_source_odom_;
mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> sh_tf_source_att_;
mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude> sh_altitude_amsl_;
nav_msgs::OdometryConstPtr first_msg_;
bool got_first_msg_ = false;

Expand Down Expand Up @@ -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_;
Expand Down
82 changes: 45 additions & 37 deletions src/transform_manager/transform_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down Expand Up @@ -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);
};
/*//}*/

Expand Down Expand Up @@ -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;
Expand All @@ -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());

}
/*//}*/

Expand All @@ -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;
Expand All @@ -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++) {
Expand All @@ -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)) {
Expand Down Expand Up @@ -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) {
Expand All @@ -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++) {
Expand Down Expand Up @@ -896,6 +896,14 @@ void TransformManager::publishLocalTf() {
/*//{ transformRtkToFcu() */
std::optional<geometry_msgs::Pose> 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
Expand Down

0 comments on commit 66342a0

Please sign in to comment.