odom.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 James Goppert
 * Copyright 2017,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 <tf2_eigen/tf2_eigen.h>
#include <boost/algorithm/string.hpp>

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>

namespace mavros {
namespace extra_plugins {
using mavlink::common::MAV_FRAME;
using mavlink::common::MAV_ESTIMATOR_TYPE;
using Matrix6d = Eigen::Matrix<double, 6, 6, Eigen::RowMajor>;

class OdometryPlugin : public plugin::PluginBase {
public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW         // XXX(vooon): added to try to fix #1223. Not sure that it is needed because class do not have Eigen:: fields.

        OdometryPlugin() : PluginBase(),
                odom_nh("~odometry"),
                fcu_odom_parent_id_des("map"),
                fcu_odom_child_id_des("base_link")
        { }

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

                // frame params:
                odom_nh.param<std::string>("fcu/odom_parent_id_des", fcu_odom_parent_id_des, "map");
                odom_nh.param<std::string>("fcu/odom_child_id_des", fcu_odom_child_id_des, "base_link");

                // publishers
                odom_pub = odom_nh.advertise<nav_msgs::Odometry>("in", 10);

                // subscribers
                odom_sub = odom_nh.subscribe("out", 1, &OdometryPlugin::odom_cb, this);
        }

        Subscriptions get_subscriptions() override
        {
                return {
                        make_handler(&OdometryPlugin::handle_odom)
                };
        }

private:
        ros::NodeHandle odom_nh;                        
        ros::Publisher odom_pub;                        
        ros::Subscriber odom_sub;                       

        std::string fcu_odom_parent_id_des;                     
        std::string fcu_odom_child_id_des;                      

        void lookup_static_transform(const std::string &target, const std::string &source,
                                Eigen::Affine3d &tf_source2target)
        {
                try {
                        // transform lookup at current time.
                        tf_source2target = tf2::transformToEigen(m_uas->tf2_buffer.lookupTransform(
                                                        target, source, ros::Time(0)));
                } catch (tf2::TransformException &ex) {
                        ROS_ERROR_THROTTLE_NAMED(1, "odom", "ODOM: Ex: %s", ex.what());
                        return;
                }
        }

        void handle_odom(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ODOMETRY &odom_msg)
        {
                Eigen::Affine3d tf_parent2parent_des;
                Eigen::Affine3d tf_child2child_des;

                lookup_static_transform(fcu_odom_parent_id_des, "map_ned", tf_parent2parent_des);
                lookup_static_transform( fcu_odom_child_id_des, "base_link_frd", tf_child2child_des);

                Matrix6d cov_pose = Matrix6d::Zero();
                ftf::mavlink_urt_to_covariance_matrix(odom_msg.pose_covariance, cov_pose);

                Matrix6d cov_vel = Matrix6d::Zero();
                ftf::mavlink_urt_to_covariance_matrix(odom_msg.velocity_covariance, cov_vel);

                Eigen::Vector3d position {};            
                Eigen::Quaterniond orientation {};      
                Eigen::Vector3d lin_vel {};             
                Eigen::Vector3d ang_vel {};             
                Matrix6d r_pose = Matrix6d::Zero();     
                Matrix6d r_vel = Matrix6d::Zero();      

                auto odom = boost::make_shared<nav_msgs::Odometry>();

                odom->header = m_uas->synchronized_header(fcu_odom_parent_id_des, odom_msg.time_usec);
                odom->child_frame_id = fcu_odom_child_id_des;

                position = Eigen::Vector3d(tf_parent2parent_des.linear() * Eigen::Vector3d(odom_msg.x, odom_msg.y, odom_msg.z));
                tf::pointEigenToMsg(position, odom->pose.pose.position);

                Eigen::Quaterniond q_child2parent(ftf::mavlink_to_quaternion(odom_msg.q));
                Eigen::Affine3d tf_childDes2parentDes = tf_parent2parent_des * q_child2parent * tf_child2child_des.inverse();
                orientation = Eigen::Quaterniond(tf_childDes2parentDes.linear());
                tf::quaternionEigenToMsg(orientation, odom->pose.pose.orientation);

                lin_vel = Eigen::Vector3d(tf_child2child_des.linear() * Eigen::Vector3d(odom_msg.vx, odom_msg.vy, odom_msg.vz));
                ang_vel = Eigen::Vector3d(tf_child2child_des.linear() * Eigen::Vector3d(odom_msg.rollspeed, odom_msg.pitchspeed, odom_msg.yawspeed));
                tf::vectorEigenToMsg(lin_vel, odom->twist.twist.linear);
                tf::vectorEigenToMsg(ang_vel, odom->twist.twist.angular);

                r_pose.block<3, 3>(0, 0) = r_pose.block<3, 3>(3, 3) = tf_parent2parent_des.linear();
                cov_pose = r_pose * cov_pose * r_pose.transpose();
                Eigen::Map<Matrix6d>(odom->pose.covariance.data(), cov_pose.rows(), cov_pose.cols()) = cov_pose;

                r_vel.block<3, 3>(0, 0) = r_vel.block<3, 3>(3, 3) = tf_child2child_des.linear();
                cov_vel = r_vel * cov_vel * r_vel.transpose();
                Eigen::Map<Matrix6d>(odom->twist.covariance.data(), cov_vel.rows(), cov_vel.cols()) = cov_vel;

                odom_pub.publish(odom);
        }

        void odom_cb(const nav_msgs::Odometry::ConstPtr &odom)
        {
                Eigen::Affine3d tf_parent2parent_des;
                Eigen::Affine3d tf_child2child_des;

                lookup_static_transform("odom_ned", odom->header.frame_id, tf_parent2parent_des);
                lookup_static_transform("base_link_frd", odom->child_frame_id, tf_child2child_des);

                ftf::Covariance6d cov_pose = odom->pose.covariance;
                ftf::EigenMapCovariance6d cov_pose_map(cov_pose.data());

                ftf::Covariance6d cov_vel = odom->twist.covariance;
                ftf::EigenMapCovariance6d cov_vel_map(cov_vel.data());

                Eigen::Vector3d position {};            
                Eigen::Quaterniond orientation {};      
                Eigen::Vector3d lin_vel {};             
                Eigen::Vector3d ang_vel {};             
                Matrix6d r_pose = Matrix6d::Zero();     
                Matrix6d r_vel = Matrix6d::Zero();      

                mavlink::common::msg::ODOMETRY msg {};
                msg.frame_id = utils::enum_value(MAV_FRAME::LOCAL_FRD);
                msg.child_frame_id = utils::enum_value(MAV_FRAME::BODY_FRD);
                msg.estimator_type = utils::enum_value(MAV_ESTIMATOR_TYPE::VISION);

                position = Eigen::Vector3d(tf_parent2parent_des.linear() * ftf::to_eigen(odom->pose.pose.position));

                Eigen::Quaterniond q_child2parent(ftf::to_eigen(odom->pose.pose.orientation));
                Eigen::Affine3d tf_childDes2parentDes = tf_parent2parent_des * q_child2parent * tf_child2child_des.inverse();
                orientation = Eigen::Quaterniond(tf_childDes2parentDes.linear());

                lin_vel = Eigen::Vector3d(tf_child2child_des.linear() * ftf::to_eigen(odom->twist.twist.linear));
                ang_vel = Eigen::Vector3d(tf_child2child_des.linear() * ftf::to_eigen(odom->twist.twist.angular));

                r_pose.block<3, 3>(0, 0) = r_pose.block<3, 3>(3, 3) = tf_parent2parent_des.linear();
                r_vel.block<3, 3>(0, 0) = r_vel.block<3, 3>(3, 3) = tf_child2child_des.linear();
                cov_pose_map = r_pose * cov_pose_map * r_pose.transpose();
                cov_vel_map  = r_vel  * cov_vel_map  * r_vel.transpose();

                ROS_DEBUG_STREAM_NAMED("odom", "ODOM: output: pose covariance matrix:" << std::endl << cov_pose_map);
                ROS_DEBUG_STREAM_NAMED("odom", "ODOM: output: velocity covariance matrix:" << std::endl << cov_vel_map);

                /* -*- ODOMETRY msg parser -*- */
                msg.time_usec = odom->header.stamp.toNSec() / 1e3;

                // [[[cog:
                // for a, b in (('', 'position'), ('v', 'lin_vel')):
                //     for f in 'xyz':
                //         cog.outl("msg.{a}{f} = {b}.{f}();".format(**locals()))
                // for a, b in zip("xyz", ('rollspeed', 'pitchspeed', 'yawspeed')):
                //     cog.outl("msg.{b} = ang_vel.{a}();".format(**locals()))
                // ]]]
                msg.x = position.x();
                msg.y = position.y();
                msg.z = position.z();
                msg.vx = lin_vel.x();
                msg.vy = lin_vel.y();
                msg.vz = lin_vel.z();
                msg.rollspeed = ang_vel.x();
                msg.pitchspeed = ang_vel.y();
                msg.yawspeed = ang_vel.z();
                // [[[end]]] (checksum: ead24a1a6a14496c9de6c1951ccfbbd7)

                ftf::quaternion_to_mavlink(orientation, msg.q);
                ftf::covariance_urt_to_mavlink(cov_pose_map, msg.pose_covariance);
                ftf::covariance_urt_to_mavlink(cov_vel_map, msg.velocity_covariance);

                // send ODOMETRY msg
                UAS_FCU(m_uas)->send_message_ignore_drop(msg);
        }
};
}       // namespace extra_plugins
}       // namespace mavros

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