vision_speed_estimate.cpp

Go to the documentation of this file.

/*
 * Copyright 2014, 2018 Nuno Marques.
 *
 * 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 <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>

#include <geometry_msgs/Vector3Stamped.h>

namespace mavros {
namespace extra_plugins {
class VisionSpeedEstimatePlugin : public plugin::PluginBase {
public:
        VisionSpeedEstimatePlugin() : PluginBase(),
                sp_nh("~vision_speed"),
                listen_twist(true),
                twist_cov(true)
        { }

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

                sp_nh.param("listen_twist", listen_twist, true);
                sp_nh.param("twist_cov", twist_cov, true);

                if (listen_twist) {
                        if (twist_cov)
                                vision_twist_cov_sub = sp_nh.subscribe("speed_twist_cov", 10, &VisionSpeedEstimatePlugin::twist_cov_cb, this);
                        else
                                vision_twist_sub = sp_nh.subscribe("speed_twist", 10, &VisionSpeedEstimatePlugin::twist_cb, this);
                }
                else
                        vision_vector_sub = sp_nh.subscribe("speed_vector", 10, &VisionSpeedEstimatePlugin::vector_cb, this);
        }

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

private:
        ros::NodeHandle sp_nh;

        bool listen_twist;                      
        bool twist_cov;                         

        ros::Subscriber vision_twist_sub;       
        ros::Subscriber vision_twist_cov_sub;   
        ros::Subscriber vision_vector_sub;      

        /* -*- low-level send -*- */
        void send_vision_speed_estimate(const uint64_t usec, const Eigen::Vector3d &v, const ftf::Covariance3d &cov)
        {
                mavlink::common::msg::VISION_SPEED_ESTIMATE vs {};

                vs.usec = usec;

                // [[[cog:
                // for f in "xyz":
                //     cog.outl("vs.%s = v.%s();" % (f, f))
                // ]]]
                vs.x = v.x();
                vs.y = v.y();
                vs.z = v.z();
                // [[[end]]] (checksum: aee3cc9a73a2e736b7bc6c83ea93abdb)

                ftf::covariance_to_mavlink(cov, vs.covariance);

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

        /* -*- mid-level helpers -*- */
        void convert_vision_speed(const ros::Time &stamp, const Eigen::Vector3d &vel_enu, const ftf::Covariance3d &cov_enu)
        {
                // Send transformed data from local ENU to NED frame
                send_vision_speed_estimate(stamp.toNSec() / 1000,
                                        ftf::transform_frame_enu_ned(vel_enu),
                                        ftf::transform_frame_enu_ned(cov_enu));
        }

        /* -*- callbacks -*- */
        void twist_cb(const geometry_msgs::TwistStamped::ConstPtr &req) {
                ftf::Covariance3d cov {};       // zero initialized

                convert_vision_speed(req->header.stamp, ftf::to_eigen(req->twist.linear), cov);
        }

        void twist_cov_cb(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &req) {
                ftf::Covariance3d cov3d {};     // zero initialized

                ftf::EigenMapCovariance3d cov3d_map(cov3d.data());
                ftf::EigenMapConstCovariance6d cov6d_map(req->twist.covariance.data());

                // only the linear velocity will be sent
                cov3d_map = cov6d_map.block<3, 3>(0, 0);

                convert_vision_speed(req->header.stamp, ftf::to_eigen(req->twist.twist.linear), cov3d);
        }

        void vector_cb(const geometry_msgs::Vector3Stamped::ConstPtr &req) {
                ftf::Covariance3d cov {};       // zero initialized

                convert_vision_speed(req->header.stamp, ftf::to_eigen(req->vector), cov);
        }
};
}       // namespace extra_plugins
}       // namespace mavros

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