diff --git a/mavros_extras/src/plugins/landing_target.cpp b/mavros_extras/src/plugins/landing_target.cpp index 601062096..31ff37014 100644 --- a/mavros_extras/src/plugins/landing_target.cpp +++ b/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(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