Not able to see any output from custom plugin LOCAL_POSITION_NED_COV

[muralivnv]

Issue details

I wrote a custom plugin that subscribes to mavlink message LOCAL_POSITION_NED_COV, so that i can view the pixhawk EKF covariances. I'm able to see the topic in "rostopic list", but after echoing the topic in terminal I'm not able to see any message details. LOCAL_POSITION_NED is giving the output but the above LOCAL_POSITION_NED_COV is not.

This is how i wrote the plugin, First of all I wrote the new plugin and added it under "src/plugins/" Then i changed the "mavros_plugins.xml" to specify the new plugin Then added the plugin in "CMakeLists.Txt" The following the program for custom plugin "LOCAL_POSITION_NED_COV"

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

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/AccelWithCovarianceStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <geometry_msgs/TransformStamped.h>

#include <nav_msgs/Odometry.h>

namespace mavros {
namespace std_plugins {

/**
 * @brief Local position covariance plugin.
 * Publish local position to TF, PositionWithCovarianceStamped, TwistWithCovarianceStamped
 * and AccelerationWithCovarianceStamped
**/

class LocalPositionCovPlugin : public plugin::PluginBase {
public:
    LocalPositionCovPlugin() : PluginBase(),
        lpc_nh("~local_position_ned_cov"),
        tf_send(false)
    { }

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

        // header frame_id.
        // default to map (world-fixed,ENU as per REP-105).
        lpc_nh.param<std::string>("frame_id", frame_id, "map");

        // Important tf subsection
        // Report the transform from world to base_link here.
        lpc_nh.param("tf/send", tf_send, true);
        lpc_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
        lpc_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "base_link");

        // Debug tf info
        // broadcast the following transform: (can be expanded to more if desired)
        // NED -> aircraft
        lpc_nh.param("tf/send_fcu",tf_send_fcu,false);

        local_position_cov = lpc_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 10);
        local_velocity_cov = lpc_nh.advertise<geometry_msgs::TwistWithCovarianceStamped>("velocity", 10);
        local_accel_cov = lpc_nh.advertise<geometry_msgs::AccelWithCovarianceStamped>("acceleration",10);
    }

    Subscriptions get_subscriptions() {
        return {
               make_handler(&LocalPositionCovPlugin::handle_local_position_ned_cov)
        };
    }

private:
    ros::NodeHandle lpc_nh;

    ros::Publisher local_position_cov;
    ros::Publisher local_velocity_cov;
    ros::Publisher local_accel_cov;

    std::string frame_id;       //!< frame for Pose
    std::string tf_frame_id;    //!< origin for TF
    std::string tf_child_frame_id;  //!< frame for TF
    bool tf_send;
    bool tf_send_fcu;   //!< report NED->aircraft in tf tree

    void handle_local_position_ned_cov(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LOCAL_POSITION_NED_COV &pos_ned_cov)
    {
        auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(pos_ned_cov.x, pos_ned_cov.y, pos_ned_cov.z));

        //--------------- Generate Message Pointers ---------------//
        auto pose_with_cov = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
        auto twist_with_cov = boost::make_shared<geometry_msgs::TwistWithCovarianceStamped>();
        auto accel_with_cov = boost::make_shared<geometry_msgs::AccelWithCovarianceStamped>();

        // pos_ned_cov.covariance is an array of 45 elements. 45 elements corresponds to upper right triangular matrix of size (9x9) with states
        //                                              x, y, z, vx, vy, vz, ax, ay, az

        // covariance pose = float[36] with covariance X,Y,Z, rotation about X, rotation about Y, rotation about Z
        // create covariance for Position
        pose_with_cov->pose.covariance = {  pos_ned_cov.covariance[0], pos_ned_cov.covariance[1],  pos_ned_cov.covariance[2],  0.0, 0.0, 0.0,
                                            pos_ned_cov.covariance[1], pos_ned_cov.covariance[9],  pos_ned_cov.covariance[10], 0.0, 0.0, 0.0,
                                            pos_ned_cov.covariance[2], pos_ned_cov.covariance[10], pos_ned_cov.covariance[17], 0.0, 0.0, 0.0,
                                            0.0,                       0.0,                        0.0,                        1.0, 0.0, 0.0,
                                            0.0,                       0.0,                        0.0,                        0.0, 1.0, 0.0,
                                            0.0,                       0.0,                        0.0,                        0.0, 0.0, 1.0 };

        // create covariance for Velocity
        twist_with_cov->twist.covariance = {    pos_ned_cov.covariance[24], pos_ned_cov.covariance[25],  pos_ned_cov.covariance[26],  0.0, 0.0, 0.0,
                                                pos_ned_cov.covariance[25], pos_ned_cov.covariance[30],  pos_ned_cov.covariance[31],  0.0, 0.0, 0.0,
                                                pos_ned_cov.covariance[26], pos_ned_cov.covariance[31],  pos_ned_cov.covariance[35],  0.0, 0.0, 0.0,
                                                0.0,                        0.0,                        0.0,                          1.0, 0.0, 0.0,
                                                0.0,                        0.0,                        0.0,                          0.0, 1.0, 0.0,
                                                0.0,                        0.0,                        0.0,                          0.0, 0.0, 1.0};

        // create covariance for Acceleration
        accel_with_cov->accel.covariance = {    pos_ned_cov.covariance[39], pos_ned_cov.covariance[40],  pos_ned_cov.covariance[41],  0.0, 0.0, 0.0,
                                                pos_ned_cov.covariance[40], pos_ned_cov.covariance[42],  pos_ned_cov.covariance[43],  0.0, 0.0, 0.0,
                                                pos_ned_cov.covariance[41], pos_ned_cov.covariance[43],  pos_ned_cov.covariance[44],  0.0, 0.0, 0.0,
                                                0.0,                        0.0,                         0.0,                         1.0, 0.0, 0.0,
                                                0.0,                        0.0,                         0.0,                         0.0, 1.0, 0.0,
                                                0.0,                        0.0,                         0.0,                         0.0, 0.0, 1.0};

        //--------------- Get Odom Information ---------------//
        // Note this orientation describes baselink->ENU transform
        auto enu_orientation_msg = m_uas->get_attitude_orientation();
        auto baselink_angular_msg = m_uas->get_attitude_angular_velocity();

        Eigen::Quaterniond enu_orientation;
        tf::quaternionMsgToEigen(enu_orientation_msg, enu_orientation);

        // timestamp messages
        // synchronize time_boot_ms (or) time_usec
        pose_with_cov->header = m_uas->synchronized_header(frame_id, pos_ned_cov.time_usec);
        twist_with_cov->header = pose_with_cov->header;
        accel_with_cov->header = pose_with_cov->header;

        // Assign Pose, orientation data and associated covariance with it //
        pose_with_cov->pose.pose.position.x = pos_ned_cov.x;
        pose_with_cov->pose.pose.position.y = pos_ned_cov.y;
        pose_with_cov->pose.pose.position.z = pos_ned_cov.z;

        pose_with_cov->pose.pose.orientation = enu_orientation_msg;

        // Assign Velocity, angular velocity data and associated covariance with it //
        twist_with_cov->twist.twist.linear.x = pos_ned_cov.vx;
        twist_with_cov->twist.twist.linear.y = pos_ned_cov.vy;
        twist_with_cov->twist.twist.linear.z = pos_ned_cov.vz;

        twist_with_cov->twist.twist.angular = baselink_angular_msg;

        // Assign Linear acceleration data and associated covariance with it //
        accel_with_cov->header.frame_id = tf_frame_id;
        accel_with_cov->accel.accel.angular = baselink_angular_msg;

        accel_with_cov->accel.accel.linear.x = pos_ned_cov.ax;
        accel_with_cov->accel.accel.linear.y = pos_ned_cov.ay;
        accel_with_cov->accel.accel.linear.z = pos_ned_cov.az;

        //--------------- Publish Data ---------------//
        local_accel_cov.publish(accel_with_cov);
        local_position_cov.publish(pose_with_cov);
        local_velocity_cov.publish(twist_with_cov);

        if (tf_send) {
            geometry_msgs::TransformStamped transform;

            transform.header.stamp = pose_with_cov->header.stamp;
            transform.header.frame_id = tf_frame_id;
            transform.child_frame_id = tf_child_frame_id;

            transform.transform.rotation = enu_orientation_msg;
            tf::vectorEigenToMsg(enu_position, transform.transform.translation);

            m_uas->tf2_broadcaster.sendTransform(transform);
        }
        if (tf_send_fcu) {
            //--------------- Report NED->aircraft transform ---------------//
            geometry_msgs::TransformStamped ned_aircraft_tf;

            ned_aircraft_tf.header.stamp = pose_with_cov->header.stamp;
            ned_aircraft_tf.header.frame_id = "NED";
            ned_aircraft_tf.child_frame_id = "aircraft";

            //Don't just report the data from the mavlink message,
            //actually perform rotations to see if anything is
            //wrong.
            auto ned_position = ftf::transform_frame_enu_ned(enu_position);
            tf::vectorEigenToMsg(ned_position, ned_aircraft_tf.transform.translation);

            auto ned_orientation = ftf::transform_orientation_enu_ned(
                        ftf::transform_orientation_baselink_aircraft(enu_orientation));
            tf::quaternionEigenToMsg(ned_orientation,ned_aircraft_tf.transform.rotation);
            m_uas->tf2_broadcaster.sendTransform(ned_aircraft_tf);
        }
    }
};
}   // namespace std_plugins
}   // namespace mavros

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

MAVROS version and platform

Mavros: 0.18.4 ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

[ ] ArduPilot Version: 3.7.1

[vooon]

I don't see any usage of _COV in APM code.

[TSC21]

Covariance msgs are not sent through Mavlink stream so it would never work unless you add that feature on Ardupilot Firmware.

[muralivnv]

When I looked inside common.xml under msgs in mavlink msgs package, I can see the message LOCAL_POSITION_NED_COV defined. Just to confirm what you said @TSC21, so you are saying that APM doesn't support this message and this message is defined for px4 stack autopilots. Is this right?

[TSC21]

One thing is having the msg defined. Another is the autopilot using it. Neither PX4 is sending it (eventhough is able to receive it).

[muralivnv]

Thank you @TSC21 for clarifying. I had this doubt for long time. I'm closing this issue.