stuff

umrover · Mar 6, 2024 · 111bf1d · 111bf1d
1 parent f21a226
commit 111bf1d
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/src/teleoperation/lander_align/lander_align.cpp b/src/teleoperation/lander_align/lander_align.cpp
@@ -255,6 +255,11 @@ namespace mrover {
 
         SE3Conversions::pushToTfTree(mTfBroadcaster, "offset", mMapFrameId, offsetLocationSE3);
 
+        //For the normal
+        manif::SE3d mNormalLocInZED = {{mBestNormalInWorld.value().x(), mBestNormalInWorld.value().y(), mBestNormalInWorld.value().z()}, manif::SO3d{Eigen::Quaterniond{rot}.normalized()}};//TODO: THIS IS A RANDOM ROTATION MATRIX
+
+        SE3Conversions::pushToTfTree(mTfBroadcaster, "normal", mMapFrameId, mNormalLocInZED);
+
 
         ROS_INFO("Max inliers: %i", minInliers);
         ROS_INFO("THE LOCATION OF THE PLANE IS AT: %f, %f, %f with normal vector %f, %f, %f", mBestLocationInZED.value().x(), mBestLocationInZED.value().y(), mBestLocationInZED.value().z(), mBestNormalInZED.value().x(), mBestNormalInZED.value().y(), mBestNormalInZED.value().z());