Support specified frames of reference in landing target module

mavros_extras/src/plugins/landing_target.cpp

@@ -461,22 +461,55 @@ class LandingTargetPlugin : public plugin::Plugin,
     Eigen::Affine3d tr;
     tf2::fromMsg(req->pose, tr);
 
-    /** @todo these transforms should be applied according to the MAV_FRAME */
-    auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
-    auto orientation = ftf::transform_orientation_enu_ned(
-      ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())));
-
+    Eigen::Vector3d position = Eigen::Vector3d::Zero();
+    Eigen::Quaterniond orientation = Eigen::Quaterniond::Identity();
+    bool position_valid = false;
+
+    const auto data_frame = static_cast<MAV_FRAME>(req->frame);
+    switch(data_frame) {
+      case MAV_FRAME::LOCAL_NED: {
+        position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
+        orientation = ftf::transform_orientation_enu_ned(
+          ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))
+        );
+        position_valid = true;
+        break;
+      }
+      case MAV_FRAME::BODY_FRD: {
+        position = ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(tr.translation()));
+        orientation = ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()));
+        position_valid = true;
+        break;
+      }
+      default: {
+        //Raise a warning if a non-zero frame value is provided
+        //XXX:  This is no ideal given that "0" is "MAV_FRAME::GLOBAL"
+        //      however this would be the "default value" for anyone
+        //      using this interface without setting frame correctly 
+        //
+        //      The better option would be to expose "position_valid"
+        //      in mavros_msgs::msg::LandingTarget, but this will at
+        //      least allow people to use the angle interface without
+        //      getting a constant error stream.
+        if(data_frame != MAV_FRAME::GLOBAL) 
+          RCLCPP_WARN_STREAM(
+            get_logger(),
+            "LT: Landing target frame '" << req->frame << "' is not supported"
+          );
+      }
+    }
+
     landing_target(
       rclcpp::Time(req->header.stamp).nanoseconds() / 1000,
       req->target_num,
-      req->frame,               // by default, in LOCAL_NED
+      req->frame,
       Eigen::Vector2f(req->angle[0], req->angle[1]),
       req->distance,
       Eigen::Vector2f(req->size[0], req->size[1]),
       position,
       orientation,
       req->type,
-      1);                       // position is valid from the first received msg
+      position_valid);                       // position is valid from the first received msg
   }
 };
 }       // namespace extra_plugins