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_map_id_des("map"),
                fcu_odom_parent_id_des("odom"),
                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, uas_.get_odom_frame_id());
                odom_nh.param<std::string>("fcu/odom_child_id_des", fcu_odom_child_id_des, uas_.get_base_link_frame_id());
                odom_nh.param<std::string>("fcu/map_id_des", fcu_map_id_des, uas_.get_map_frame_id());
 
                // 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;                      
        std::string fcu_map_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_map_id_des, fcu_map_id_des+"_ned", tf_parent2parent_des);
                lookup_static_transform( fcu_odom_child_id_des, fcu_odom_child_id_des+"_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_map_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(fcu_odom_parent_id_des+"_ned", odom->header.frame_id, tf_parent2parent_des);
                lookup_static_transform(fcu_odom_child_id_des+"_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.hpp>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::OdometryPlugin, mavros::plugin::PluginBase)