文章目录

前言

ROS Melodic
PX4 1.13.0

一、PX4添加自定义mavlink消息

修改下面路径下的common.xml文件
MAVROS发送自定义话题消息给PX4_自动驾驶
在下图位置添加

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

MAVROS发送自定义话题消息给PX4_人工智能_02
然后在~/PX4-Autopilot目录下先执行

make clean

再执行

make px4_sitl_default

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

mavlink_msg_key_command.h

MAVROS发送自定义话题消息给PX4_#include_03
这时自定义mavlink消息文件就成功生成了

二、PX4添加自定义uorb消息

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

uint64 timestamp
char cmd

MAVROS发送自定义话题消息给PX4_人工智能_04
修改下图文件
MAVROS发送自定义话题消息给PX4_数据_05
在下图位置添加:

key_command.msg

MAVROS发送自定义话题消息给PX4_自动驾驶_06

三、PX4接收自定义mavlink消息

修改下图​​mavlink_receiver.h​
MAVROS发送自定义话题消息给PX4_人工智能_07
下图位置添加:

#include <uORB/topics/key_command.h>

MAVROS发送自定义话题消息给PX4_数据_08
下图位置添加:

void handle_message_key_command(mavlink_message_t *msg);

MAVROS发送自定义话题消息给PX4_#include_09
下图位置添加:

orb_advert_t _key_command_pub{nullptr};

MAVROS发送自定义话题消息给PX4_#include_10
修改下图​​mavlink_receiver.cpp​
MAVROS发送自定义话题消息给PX4_自定义_11
下图位置添加:

case MAVLINK_MSG_ID_KEY_COMMAND:
handle_message_key_command(msg);
break;

MAVROS发送自定义话题消息给PX4_数据_12
下图位置添加:

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);
}
}

MAVROS发送自定义话题消息给PX4_数据_13

四、PX4打印自定义mavlink数据

在下图目录添加​​key_receiver​​​文件夹
MAVROS发送自定义话题消息给PX4_数据_14
在里面添加下图三个文件
MAVROS发送自定义话题消息给PX4_人工智能_15

各个文件的内容如下:
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;
}

修改下图文件
MAVROS发送自定义话题消息给PX4_自定义_16
在下图位置添加:

CONFIG_MODULES_KEY_RECEIVER=y

MAVROS发送自定义话题消息给PX4_人工智能_17
然后编译

make px4_sitl_default gazebo

在当前终端输入

help

能找到​​key_receiver​​​说明编译正常
MAVROS发送自定义话题消息给PX4_#include_18

五、MAVROS发送自定义mavlink数据

在下图位置添加​​keyboard_command.cpp​​​文件夹
MAVROS发送自定义话题消息给PX4_自定义_19
文件内容如下:

#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)

修改下图文件
MAVROS发送自定义话题消息给PX4_人工智能_20
在下图位置添加:

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

MAVROS发送自定义话题消息给PX4_自定义_21

修改下图文件
MAVROS发送自定义话题消息给PX4_数据_22
在下图位置添加:

src/plugins/keyboard_command.cpp

MAVROS发送自定义话题消息给PX4_自定义_23
修改下图​​​common.xml​​​文件
MAVROS发送自定义话题消息给PX4_人工智能_24
在下图位置添加:

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

MAVROS发送自定义话题消息给PX4_#include_25
然后在mavros工作空间下编译mavros

catkin build

六、测试

首先运行:

roslaunch px4 mavros_posix_sitl.launch

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

key_receiver start

如下图:
MAVROS发送自定义话题消息给PX4_数据_26
然后新开一个终端,执行:

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。
MAVROS发送自定义话题消息给PX4_自定义_27