fix inverse for utm & part of wgs84

swri-robotics · Jul 11, 2024 · c1760cc · c1760cc
1 parent 0edc8d4
commit c1760cc
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 35 deletions.
diff --git a/swri_transform_util/src/utm_transformer.cpp b/swri_transform_util/src/utm_transformer.cpp
@@ -206,40 +206,13 @@ namespace swri_transform_util
 
   TransformImplPtr UtmToTfTransform::Inverse() const
   {
-    RCLCPP_ERROR(logger_, "DJA: Entering UtmToTfTransform::Inverse");
     tf2::Stamped<tf2::Transform> inverse_transform = GetStampedTransform();
     inverse_transform.setData(inverse_transform.inverse());
 
     geometry_msgs::msg::TransformStamped inverse_tf_msg;
+    tf2::convert(inverse_transform, inverse_tf_msg);
     inverse_tf_msg.header.frame_id = transform_.child_frame_id;
     inverse_tf_msg.child_frame_id = transform_.header.frame_id;
-
-    auto origin = inverse_transform.getOrigin();
-    auto orientation = inverse_transform.getRotation();
-    RCLCPP_ERROR(
-      logger_,
-      "DJA Frames: %s, %s",
-      inverse_tf_msg.header.frame_id.c_str(),
-      inverse_tf_msg.child_frame_id.c_str());
-    RCLCPP_ERROR(
-      logger_,
-      "DJA Inverse Origin: %f, %f, %f",
-      origin.x(),
-      origin.y(),
-      origin.z());
-
-    RCLCPP_ERROR(
-      logger_,
-      "DJA Inverse Orientation: %f, %f, %f, %f",
-      orientation.x(),
-      orientation.y(),
-      orientation.z(),
-      orientation.w());
-
-
-
-
-
     TransformImplPtr inverse = std::make_shared<TfToUtmTransform>(
         inverse_tf_msg,
         utm_util_,
@@ -249,11 +222,6 @@ namespace swri_transform_util
 
     auto xform_origin = inverse->GetOrigin();
     auto xform_orientation = inverse->GetOrientation();
-    RCLCPP_ERROR(logger_, "DJA Xform Inverse Origin: %f, %f, %f", xform_origin.x(), xform_origin.y(), xform_origin.z());
-    RCLCPP_ERROR(logger_, "DJA Xform Inverse Orientation: %f, %f, %f, %f", xform_orientation.x(), xform_orientation.y(), xform_orientation.z(), xform_orientation.w());
-
-
-    RCLCPP_ERROR(logger_, "DJA: Leaving UtmToTfTransform::Inverse");
     return inverse;
   }
 
@@ -301,7 +269,7 @@ namespace swri_transform_util
     inverse_transform.setData(inverse_transform.inverse());
 
     geometry_msgs::msg::TransformStamped inverse_tf_msg;
-
+    tf2::convert(inverse_transform, inverse_tf_msg);
     inverse_tf_msg.header.frame_id = transform_.child_frame_id;
     inverse_tf_msg.child_frame_id = transform_.header.frame_id;
     TransformImplPtr inverse = std::make_shared<UtmToTfTransform>(

diff --git a/swri_transform_util/src/wgs84_transformer.cpp b/swri_transform_util/src/wgs84_transformer.cpp
@@ -179,7 +179,6 @@ namespace swri_transform_util
     double x, y;
     local_xy_util_->ToLocalXy(v_in.y(), v_in.x(), x, y);
     v_out.setValue(x, y, v_in.z());
-
     // Transform from the LocalXY coordinate frame using the TF transform.
     v_out = GetStampedTransform() * v_out;
   }
@@ -201,6 +200,7 @@ namespace swri_transform_util
     inverse_transform.setData(inverse_transform.inverse());
 
     geometry_msgs::msg::TransformStamped inverse_tf_msg;
+    tf2::convert(inverse_transform, inverse_tf_msg);
     inverse_tf_msg.header.frame_id = transform_.child_frame_id;
     inverse_tf_msg.child_frame_id = transform_.header.frame_id;
     TransformImplPtr inverse = std::make_shared<TfToWgs84Transform>(