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。