MAVROS发送自定义话题消息给PX4

前言

ROS Melodic
PX4 1.13.0

一、PX4添加自定义mavlink消息

修改下面路径下的common.xml文件

在下图位置添加

  <message id="229" name="KEY_COMMAND">
     <description>Keyboard char command.</description>
     <field type="char" name="command"> </field>
   </message>

然后在~/PX4-Autopilot目录下先执行

make clean

再执行

make px4_sitl_default

编译成功后会在下图的路径中找到

mavlink_msg_key_command.h

这时自定义mavlink消息文件就成功生成了

二、PX4添加自定义uorb消息

在下图目录添加key_command.msg文件
文件内容如下：

uint64 timestamp
char cmd

修改下图文件

在下图位置添加：

key_command.msg

三、PX4接收自定义mavlink消息

修改下图mavlink_receiver.h

下图位置添加：

#include <uORB/topics/key_command.h>

下图位置添加：

void handle_message_key_command(mavlink_message_t *msg);

下图位置添加：

orb_advert_t _key_command_pub{nullptr};

修改下图mavlink_receiver.cpp

下图位置添加：

case MAVLINK_MSG_ID_KEY_COMMAND:
 	   handle_message_key_command(msg);
 	   break;

下图位置添加：

 void
MavlinkReceiver::handle_message_key_command(mavlink_message_t *msg)
{
    mavlink_key_command_t man;
    mavlink_msg_key_command_decode(msg, &man);

struct key_command_s key = {};

    key.timestamp = hrt_absolute_time();
    key.cmd = man.command;

    if (_key_command_pub == nullptr) {
        _key_command_pub = orb_advertise(ORB_ID(key_command), &key);

    } else {
        orb_publish(ORB_ID(key_command), _key_command_pub, &key);
    }
}

四、PX4打印自定义mavlink数据

在下图目录添加key_receiver文件夹

在里面添加下图三个文件

各个文件的内容如下：
1

CMakeLists.txt

px4_add_module(
	MODULE modules__key_receiver
	MAIN key_receiver
	COMPILE_FLAGS
		#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output
		#-O0             # uncomment when debugging
	SRCS
		key_receiver.cpp
	DEPENDS
		px4_work_queue
	)

2

Kconfig

menuconfig MODULES_KEY_RECEIVER
	bool "key_receiver"
	default n
	---help---
		Enable support for key_receiver

3

key_receiver.cpp

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>

#include <uORB/uORB.h>
#include <uORB/topics/key_command.h>

extern "C" __EXPORT int key_receiver_main(int argc, char **argv);

int key_receiver_main(int argc, char **argv)
{
    int key_sub_fd = orb_subscribe(ORB_ID(key_command));
    orb_set_interval(key_sub_fd, 200); // limit the update rate to 200ms

    px4_pollfd_struct_t fds[1];
    fds[0].fd = key_sub_fd, fds[0].events = POLLIN;

    int error_counter = 0;

    while(true)
    {
        int poll_ret = px4_poll(fds, 1, 1000);

        if (poll_ret == 0)
        {
            PX4_ERR("Got no data within a second");
        }

        else if (poll_ret < 0)
        {
            if (error_counter < 10 || error_counter % 50 == 0)
            {
                PX4_ERR("ERROR return value from poll(): %d", poll_ret);
            }

            error_counter++;
        }

        else
        {
            if (fds[0].revents & POLLIN)
            {
                struct key_command_s input;
                orb_copy(ORB_ID(key_command), key_sub_fd, &input);
                PX4_INFO("Recieved Char : %c", input.cmd);
             }
        }
    }
    return 0;
}

修改下图文件

在下图位置添加：

CONFIG_MODULES_KEY_RECEIVER=y

然后编译

make px4_sitl_default gazebo

在当前终端输入

help

能找到key_receiver说明编译正常

五、MAVROS发送自定义mavlink数据

在下图位置添加keyboard_command.cpp文件夹

文件内容如下：

 #include <mavros/mavros_plugin.h>
 #include <pluginlib/class_list_macros.h>
 #include <iostream>
 #include <std_msgs/Char.h>

 namespace mavros {
 namespace extra_plugins{

 class KeyboardCommandPlugin : public plugin::PluginBase {
 public:
     KeyboardCommandPlugin() : PluginBase(),
         nh("~keyboard_command")

    { };

     void initialize(UAS &uas_)
     {
         PluginBase::initialize(uas_);
         keyboard_sub = nh.subscribe("keyboard_sub", 10, &KeyboardCommandPlugin::keyboard_cb, this);
     };

     Subscriptions get_subscriptions()
     {
         return {/* RX disabled */ };
     }

 private:
     ros::NodeHandle nh;
     ros::Subscriber keyboard_sub;

    void keyboard_cb(const std_msgs::Char::ConstPtr &req)
     {
         std::cout << "Got Char : " << req->data <<  std::endl;
mavlink::common::msg::KEY_COMMAND key_command{};
key_command.command=req->data;
         UAS_FCU(m_uas)->send_message_ignore_drop(key_command);
     }
 };
 }   // namespace extra_plugins
 }   // namespace mavros

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

修改下图文件

在下图位置添加：

<class name="keyboard_command" type="mavros::extra_plugins::KeyboardCommandPlugin" base_class_type="mavros::plugin::PluginBase">
     <description>Accepts keyboard command.</description>
</class>

修改下图文件

在下图位置添加：

src/plugins/keyboard_command.cpp

修改下图common.xml文件

在下图位置添加：

  <message id="229" name="KEY_COMMAND">
     <description>Keyboard char command.</description>
     <field type="char" name="command"> </field>
   </message>

然后在mavros工作空间下编译mavros

catkin build

六、测试

首先运行：

roslaunch px4 mavros_posix_sitl.launch 

然后在启动launch文件的终端下执行

key_receiver start

如下图：

然后新开一个终端，执行：

rostopic pub -r 10 /mavros/keyboard_command/keyboard_sub std_msgs/Char 97

启动launch文件的终端的打印信息会从原来的Got no data within a second变成

Got Char : a
INFO  [key_receiver] Recieved Char : a

如下图，如果发布的数据改成98，则会打印 Recieved Char : b。