vision_pose_estimate.cpp

Go to the documentation of this file.

/*
 * Copyright 2014 M.H.Kabir.
 *
 * This file is part of the mavros package and subject to the license terms
 * in the top-level LICENSE file of the mavros repository.
 * https://github.com/mavlink/mavros/tree/master/LICENSE.md
 */

#include <mavros/mavros_plugin.h>
#include <mavros/setpoint_mixin.h>
#include <eigen_conversions/eigen_msg.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

namespace mavros {
namespace extra_plugins{
class VisionPoseEstimatePlugin : public plugin::PluginBase,
        private plugin::TF2ListenerMixin<VisionPoseEstimatePlugin> {
public:
        VisionPoseEstimatePlugin() : PluginBase(),
                sp_nh("~vision_pose"),
                tf_rate(10.0)
        { }

        void initialize(UAS &uas_)
        {
                PluginBase::initialize(uas_);

                bool tf_listen;

                // tf params
                sp_nh.param("tf/listen", tf_listen, false);
                sp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
                sp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "vision_estimate");
                sp_nh.param("tf/rate_limit", tf_rate, 10.0);

                if (tf_listen) {
                        ROS_INFO_STREAM_NAMED("vision_pose", "Listen to vision transform " << tf_frame_id
                                                << " -> " << tf_child_frame_id);
                        tf2_start("VisionPoseTF", &VisionPoseEstimatePlugin::transform_cb);
                }
                else {
                        vision_sub = sp_nh.subscribe("pose", 10, &VisionPoseEstimatePlugin::vision_cb, this);
                        vision_cov_sub = sp_nh.subscribe("pose_cov", 10, &VisionPoseEstimatePlugin::vision_cov_cb, this);
                }
        }

        Subscriptions get_subscriptions()
        {
                return { /* Rx disabled */ };
        }

private:
        friend class TF2ListenerMixin;
        ros::NodeHandle sp_nh;

        ros::Subscriber vision_sub;
        ros::Subscriber vision_cov_sub;

        std::string tf_frame_id;
        std::string tf_child_frame_id;
        double tf_rate;
        ros::Time last_transform_stamp;

        /* -*- low-level send -*- */
        void send_vision_estimate(const ros::Time &stamp, const Eigen::Affine3d &tr, const geometry_msgs::PoseWithCovariance::_covariance_type &cov)
        {
                if (last_transform_stamp == stamp) {
                        ROS_DEBUG_THROTTLE_NAMED(10, "vision_pose", "Vision: Same transform as last one, dropped.");
                        return;
                }
                last_transform_stamp = stamp;

                auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
                auto rpy = ftf::quaternion_to_rpy(
                                ftf::transform_orientation_enu_ned(
                                ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))));

                auto cov_ned = ftf::transform_frame_enu_ned(cov);
                ftf::EigenMapConstCovariance6d cov_map(cov_ned.data());

                auto urt_view = Eigen::Matrix<double, 6, 6>(cov_map.triangularView<Eigen::Upper>());
                ROS_DEBUG_STREAM_NAMED("vision_pose", "Vision: Covariance URT: " << std::endl << urt_view);

                mavlink::common::msg::VISION_POSITION_ESTIMATE vp{};

                vp.usec = stamp.toNSec() / 1000;
                // [[[cog:
                // for f in "xyz":
                //     cog.outl("vp.%s = position.%s();" % (f, f))
                // for a, b in zip("xyz", ('roll', 'pitch', 'yaw')):
                //     cog.outl("vp.%s = rpy.%s();" % (b, a))
                // ]]]
                vp.x = position.x();
                vp.y = position.y();
                vp.z = position.z();
                vp.roll = rpy.x();
                vp.pitch = rpy.y();
                vp.yaw = rpy.z();
                // [[[end]]] (checksum: 2048daf411780847e77f08fe5a0b9dd3)

                // just the URT of the 6x6 Pose Covariance Matrix, given
                // that the matrix is symmetric
                ftf::covariance_urt_to_mavlink(cov_map, vp.covariance);

                UAS_FCU(m_uas)->send_message_ignore_drop(vp);
        }

        /* -*- callbacks -*- */

        /* common TF listener moved to mixin */

        void transform_cb(const geometry_msgs::TransformStamped &transform)
        {
                Eigen::Affine3d tr;
                tf::transformMsgToEigen(transform.transform, tr);
                ftf::Covariance6d cov {};       // zero initialized

                send_vision_estimate(transform.header.stamp, tr, cov);
        }

        void vision_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
        {
                Eigen::Affine3d tr;
                tf::poseMsgToEigen(req->pose, tr);
                ftf::Covariance6d cov {};       // zero initialized

                send_vision_estimate(req->header.stamp, tr, cov);
        }

        void vision_cov_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &req)
        {
                Eigen::Affine3d tr;
                tf::poseMsgToEigen(req->pose.pose, tr);

                send_vision_estimate(req->header.stamp, tr, req->pose.covariance);
        }
};
}       // namespace extra_plugins
}       // namespace mavros

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::VisionPoseEstimatePlugin, mavros::plugin::PluginBase)