How can I send custom message form PX4 Autopilot to mavros in Gazebo

PX4 Autopilot
1

For my work, I need to send custom message form MAVROS to PX4 and send same type custom message from PX4 to MAVROS.

Form testing, in run some test code by gazebo in Ubuntu

My test environment is like this:

System: ubuntu 20.04
PX4-Autopolit commit 37a40d3fc294ba497
mavlink commit 9d44a698c96a3ab
mavros commit 7f1a8c7adfdb58
ros is noetic

Now I can send custom message form ROS to MAVROS, MAVROS to PX4-Autopolit

But when I send same custom message form PX4-Autopolit to MAVROS, it’s not work. I think I need some help to debug.

2

When I send custom message form MAVROS to PX4-Autopolit I did something like this:

PX4 part

1、In PX4-Autopilot/src/modules/mavlink/mavlink/message_definitions/v1.0/common.xml add

<message id="12921" name="TEST_MSG_MAV_DATA">
      <description>defined by test message</description>
      <field type="uint64_t" name="time_usec" units="us">Timestamp</field>
      <field type="float[10]" name="f_32">float32_array</field>
      <field type="int32_t[10]" name="i_32">float32_array</field>
</message>

2、in PX4-Autopilot, run make px4_sitl_defalut, then in PX4-Autopilot/build/px4_sitl_default/mavlink/common can find mavlink_msg_test_msg_mav_data.h

3、In PX4-Autopilot/src/modules/mavlink/mavlink_receiver.h add

class MavlinkReceiver : public ModuleParams{
...
void handle_message_test_message_mav_data(mavlink_message_t *msg);
uORB::Publication<test_message_s> _test_message_pub{ORB_ID(test_message)};
uORB::Subscription _test_message_sub{ORB_ID(test_message)};
...
}

In PX4-Autopilot/src/modules/mavlink/mavlink_receiver.cpp add

void
MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
    ...
    switch (msg->msgid) {
    ...
    case MAVLINK_MSG_ID_TEST_MSG_MAV_DATA:
		handle_message_test_message_mav_data(msg);
		break;
    ...
    }
    ...
}

void MavlinkReceiver::handle_message_test_message_mav_data(mavlink_message_t * msg)
{
	mavlink_test_msg_mav_data_t test_message;
	mavlink_msg_test_msg_mav_data_decode(msg, &test_message);

	struct test_message_s data = {};

	data.timestamp = hrt_absolute_time();

	for (int i=0; i<9; ++i)
	{
		data.f_32[i] = test_message.f_32[i];
		data.i_32[i] = test_message.i_32[i];
		printf("px4 mavlink get %f, %d\n", (double)data.f_32[i], data.i_32[i]);
	}

	_test_message_pub.publish(data);
}
3

MAVROS part:

1、In mavros/mavros_msgs/msg create TestMessage.msg

std_msgs/Header header
float32[10] f_32
int32[10] i_32

In mavros/mavros_msgs/msg/CMakeLists.txt add

add_message_files(
...
TestMessage.msg
...
)

2、In mavlink/message_definitions/v1.0/common.xml add

<message id="12921" name="TEST_MSG_MAV_DATA">
      <description>defined by test message</description>
      <field type="uint64_t" name="time_usec" units="us">Timestamp</field>
      <field type="float[10]" name="f_32">float32_array</field>
      <field type="int32_t[10]" name="i_32">float32_array</field>      
    </message>

3、after catkin build, devel/include/mavros_msgs can find TestMessage.h; in devel/include/mavlink/v2.0/common can find mavlink_msg_test_msg_mav_data.h mavlink_msg_test_msg_mav_data.hpp

4、then in mavros/mavros_extras/src/plugins create test_message.cpp

#include <mavros/mavros_plugin.h>
#include <pluginlib/class_list_macros.h>
#include <iostream>
#include <mavros_msgs/TestMessage.h>

namespace mavros {
namespace extra_plugins {

class testMessagePlugin : public plugin::PluginBase {
public:
    testMessagePlugin() : PluginBase(),
        nh("~TestMessage")
    { };

    void initialize(UAS &uas_)
    {
        PluginBase::initialize(uas_);
        send_sub = nh.subscribe("send_data", 10, &testMessagePlugin::testMessage_cb, this);
    };

    Subscriptions get_subscriptions() override
    {
        return { };
    }

private:
    ros::NodeHandle nh;
    ros::Subscriber send_sub;

    void testMessage_cb(const mavros_msgs::TestMessage::ConstPtr& req)
    {
        mavros::UAS *m_usa_ = static_cast<testMessagePlugin*>(this)->m_uas;

        mavlink::common::msg::TEST_MSG_MAV_DATA send_data = {};

        for(int i=0; i<10; ++i)
        {
            send_data.f_32[i] = req->f_32[i];
            send_data.i_32[i] = req->i_32[i];
        }

        std::cout << "Mavros Got Data:" << req->f_32[0] << req->f_32[1] << req->f_32[2] << std::endl;
        UAS_FCU(m_uas)->send_message_ignore_drop(send_data);
    }
};
}
}

PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::testMessagePlugin, mavros::plugin::PluginBase)

5、In mavros/mavros_extras/CMakeLists.txt add

add_library(mavros_extras

…

src/plugins/test_message.cpp

…

)

In mavros/mavros_extras/mavros_plugins.xml add

<class name="test_message" type="mavros::extra_plugins::testMessagePlugin" base_class_type="mavros::plugin::PluginBase">
    <description>defined by test message</description>
  </class>

the catkin build
After all these work

I run

make px4_sitl_default gazebo

roslaunch mavros px4.launch fcu_url:=“udp://:14540@127.0.0.1:14557”

and I run an ros pkg pub topic /mavros/TestMessage/send_data, then I can see Mavros Got Data: in mavros, px4 mavlink get in px4_sitl_defalut gazebo

so I think I success to send custom message from MAVROS to PX4-Autopolit and the port is ok

4

When I try to send custom message form PX4 to MAVROS, I did like this
1、create TEST_MESSAGE.hpp in PX4-Autopilot/src/modules/mavlink/streams

#ifndef TEST_MESSAGE_HPP
#define TEST_MESSAGE_HPP

#include <uORB/topics/test_message.h>
#include <iostream>
class MavlinkStreamTestMessage : public MavlinkStream
{
public:
        static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamTestMessage(mavlink); }

        static constexpr const char *get_name_static() { return "TEST_MESSAGE"; }
        static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TEST_MSG_MAV_DATA; }

        const char *get_name() const override { return get_name_static(); }
        uint16_t get_id() override { return get_id_static(); }

        unsigned get_size() override
        {
                return MAVLINK_MSG_ID_TEST_MSG_MAV_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
        }

private:
        explicit MavlinkStreamTestMessage(Mavlink *mavlink) : MavlinkStream(mavlink) {}

        bool send() override
        {
                mavlink_test_msg_mav_data_t msg{};
                msg.time_usec = hrt_absolute_time();
                for (int i=0; i<10; ++i)
                {
                        msg.f_32[i] = m_send_data+i;
                        msg.i_32[i] = int(m_send_data+i);
                        std::cout << "Px4 Mavlink send f_32[" << i <<"]:" << msg.f_32[i] << ";i_32[" << i << "]:" << msg.i_32[i] << std::endl;
                }
                m_send_data += 0.01f;
                mavlink_msg_test_msg_mav_data_send_struct(_mavlink->get_channel(), &msg);
                return true;
        }

        float m_send_data = 0.0f;
};

#endif //TEST_MESSAGE_HPP

2、In PX4-Autopilot/src/modules/mavlink/mavlink_messages.cpp add

static const StreamListItem streams_list[] = {
...
#if defined(TEST_MESSAGE_HPP)
        create_stream_list_item<MavlinkStreamTestMessage>(),
#endif // TEST_MESSAGE_HPP
...
};

In PX4-Autopilot/src/modules/mavlink/mavlink_main.cpp add

int
Mavlink::configure_streams_to_default(const char *configure_single_stream)
{
...
    configure_stream_local("TEST_MESSAGE", 0.1f);
...
}

3、In mavros/mavros_extras/src/plugins I make test_message.cpp like this

#include <mavros/mavros_plugin.h>
#include <pluginlib/class_list_macros.h>
#include <iostream>
#include <mavros_msgs/TestMessage.h>

namespace mavros {
namespace extra_plugins {

class testMessagePlugin : public plugin::PluginBase {
public:
    testMessagePlugin() : PluginBase(),
        nh("~TestMessage")
    { };

    void initialize(UAS &uas_)
    {
        PluginBase::initialize(uas_);
        send_sub = nh.subscribe("send_data", 10, &testMessagePlugin::testMessage_cb, this);
        get_pub = nh.advertise<mavros_msgs::TestMessage>("test_message", 10);
    };

    Subscriptions get_subscriptions() override
    {
        return {
            make_handler(&testMessagePlugin::handle_testMessage)
        };
    }

private:
    ros::NodeHandle nh;
    ros::Subscriber send_sub;
    ros::Publisher get_pub;

    void testMessage_cb(const mavros_msgs::TestMessage::ConstPtr& req)
    {
        mavros::UAS *m_usa_ = static_cast<testMessagePlugin*>(this)->m_uas;

        mavlink::common::msg::TEST_MSG_MAV_DATA send_data = {};

        for(int i=0; i<10; ++i)
        {
            send_data.f_32[i] = req->f_32[i];
            send_data.i_32[i] = req->i_32[i];
        }

        std::cout << "Mavros Got Data:" << req->f_32[0] << req->f_32[1] << req->f_32[2] << std::endl;
        UAS_FCU(m_uas)->send_message_ignore_drop(send_data);
    }

    void handle_testMessage(const mavlink::mavlink_message_t *msg, mavlink::common::msg::TEST_MSG_MAV_DATA &test_msg)
    {
        auto test_msg_pub = boost::make_shared<mavros_msgs::TestMessage>();

        test_msg_pub->header.stamp = ros::Time::now();  //TODO: request add time_boot_ms to msg definition
        for (int i=0; i<10; ++i)
        {
            test_msg_pub->i_32[i] = test_msg.i_32[i];
            test_msg_pub->f_32[i] = test_msg.f_32[i];
            std::cout << "Get In Mavros f_32[" << i << "]:" << test_msg.f_32[i] << ";i_32[" << i << "]:" << test_msg.i_32[i] <<std::endl;
        }
        get_pub.publish(test_msg_pub);
    }
};
}
}

PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::testMessagePlugin, mavros::plugin::PluginBase)

After all these work and catkin build

I run

make px4_sitl_default gazebo

roslaunch mavros px4.launch fcu_url:=“udp://:14540@127.0.0.1:14557”

there is Px4 Mavlink send f_32 can see in px4_sitl_default gazebo

but no Get In Mavros f_32 in MAVROS, it seems that MAVROS doesn’t get PX4 mavlink message

I add log in other MAVROS extras plugins like GpsStatusPlugin::handle_gps_raw_int, it can be see in MAVROS, so I think px4 and mavros defalut message can work, only my custom message not, I don’t know why, is there any one can help me？

5

I’ve been resolved, I need make configure_stream_local(“TEST_MESSAGE”, 0.1f); in case MAVLINK_MODE_ONBOARD: