broadcasting amsl_origin tf

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: []
 

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

launch/transform_manager.launch

@@ -61,6 +61,7 @@
       <remap from="~height_agl_in" to="estimation_manager/height_agl" />
       <remap from="~orientation_in" to="hw_api/orientation" />
       <remap from="~gnss_in" to="hw_api/gnss" />
+      <remap from="~altitude_amsl_in" to="hw_api/altitude" />
 
       <!-- Publishers -->
       <remap from="~profiler" to="profiler" />

src/transform_manager/transform_manager.cpp

@@ -13,6 +13,7 @@
 
 #include <mrs_msgs/UavState.h>
 #include <mrs_msgs/Float64Stamped.h>
+#include <mrs_msgs/HwApiAltitude.h>
 
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/static_transform_broadcaster.h>
@@ -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<mrs_msgs::Float64Stamped> sh_height_agl_;
   void                                                callbackHeightAgl(const mrs_msgs::Float64Stamped::ConstPtr msg);
 
+  mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude> sh_altitude_amsl_;
+  void                                                callbackAltitudeAmsl(const mrs_msgs::HwApiAltitude::ConstPtr msg);
+
   mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped> 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<mrs_msgs::Float64Stamped>(shopts, "height_agl_in", &TransformManager::callbackHeightAgl, this);
 
+  sh_altitude_amsl_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiAltitude>(shopts, "altitude_amsl_in", &TransformManager::callbackAltitudeAmsl, this);
+
   sh_hw_api_orientation_ =
       mrs_lib::SubscribeHandler<geometry_msgs::QuaternionStamped>(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) {