将自定义消息从 MOVROS 发送到 PX4
按照 Source Installation 中的指导,从 mavlink/mavros 安装“ROS Kinetic”版本。
- Ubuntu: 20.04
- ROS: Noetic
- PX4 Firmware: v1.13
However these steps are fairly general and so it should work with other distros/versions with little to no modifications. :::
MAVROS 安装
Follow Source Installation instructions from mavlink/mavros to install "ROS Kinetic".
MAVROS
首先,我们创建一个新的MAVROS 插件,在keyboard_command.cpp(workspace/src/mavros/mavros_extras/src/plugins)示例中添加以下代码:
代码功能是从ROS消息主题
/mavros/keyboard_command/keyboard_sub
中订阅了一个字符消息,并且将其作为MAVLink 消息发送出去。c#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 kc {}; kc.command = req->data; UAS_FCU(m_uas)->send_message_ignore_drop(kc); } }; } // namespace extra_plugins } // namespace mavros PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::KeyboardCommandPlugin, mavros::plugin::PluginBase)
Edit mavros_plugins.xml (in workspace/src/mavros/mavros_extras) and add the following lines:
xml<class name="keyboard_command" type="mavros::extra_plugins::KeyboardCommandPlugin" base_class_type="mavros::plugin::PluginBase"> <description>Accepts keyboard command.</description> </class>
Edit CMakeLists.txt (in workspace/src/mavros/mavros_extras) and add the following line in
add_library
.cmakeadd_library( ... src/plugins/keyboard_command.cpp )
Inside common.xml in (workspace/src/mavlink/message_definitions/v1.0), copy the following lines to add your MAVLink message:
xml... <message id="229" name="KEY_COMMAND"> <description>Keyboard char command.</description> <field type="char" name="command"> </field> </message> ...
PX4 修改
- Inside common.xml (in PX4-Autopilot/src/modules/mavlink/mavlink/message_definitions/v1.0), add your MAVLink message as following (same procedure as for MAVROS section above):xml
... <message id="229" name="KEY_COMMAND"> <description>Keyboard char command.</description> <field type="char" name="command"> </field> </message> ...
WARNING
Make sure that the common.xml files in the following directories are exactly the same:
PX4-Autopilot/src/modules/mavlink/mavlink/message_definitions/v1.0
workspace/src/mavlink/message_definitions/v1.0
are exactly the same.
Make your own uORB message file key_command.msg in (PX4-Autopilot/msg). 示例中的“key_command.msg”文件只包含以下代码:
uint64 timestamp # time since system start (microseconds) char cmd
Then, in CMakeLists.txt (in PX4-Autopilot/msg), include:
cmakeset( ... key_command.msg )
Edit mavlink_receiver.h (in PX4-Autopilot/src/modules/mavlink)
cpp... #include <uORB/topics/key_command.h> ... class MavlinkReceiver { ... private: void handle_message_key_command(mavlink_message_t *msg); ... orb_advert_t _key_command_pub{nullptr}; }
Edit mavlink_receiver.cpp (in PX4-Autopilot/src/modules/mavlink). 这是 PX4 接收 ROS 发送过来的 MAVLink 消息的地方,并且将消息作为 uORB 主题发布。
cpp... void MavlinkReceiver::handle_message(mavlink_message_t *msg) { ... 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); } }
像其他示例一样订阅你自己的uORB主题。 For this example lets create the model in (/PX4-Autopilot/src/modules/key_receiver). In this directory, create three files CMakeLists.txt, key_receiver.cpp, Kconfig Each one looks like the following.
-CMakeLists.txt
cmakepx4_add_module( MODULE modules__key_receiver MAIN key_receiver STACK_MAIN 2500 STACK_MAX 4000 SRCS key_receiver.cpp DEPENDS )
-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[] = { { .fd = key_sub_fd, .events = POLLIN }, }; int error_counter = 0; for (int i = 0; i < 10; i++) { 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("Received Char : %c", input.cmd); } } } return 0; }
-Kconfig
menuconfig MODULES_KEY_RECEIVER bool "key_receiver" default n ---help--- Enable support for key_receiver
For a more detailed explanation see the topic Writing your first application.
Lastly, add your module in the default.px4board file correspondent to your board in PX4-Autopilot/boards/. For example: -for the Pixhawk 4, add the following code in PX4-Autopilot/boards/px4/fmu-v5/default.px4board: -for the SITL, add the following code in PX4-Autopilot/boards/px4/sitl/default.px4board
CONFIG_MODULES_KEY_RECEIVER=y
MODULES ...
Building
Build for ROS
In your workspace enter:
catkin build
.Beforehand, you have to set your "px4.launch" in (/workspace/src/mavros/mavros/launch). Edit "px4.launch" as below. If you are using USB to connect your computer with Pixhawk, you have to set "fcu_url" as shown below. But, if you are using CP2102 to connect your computer with Pixhawk, you have to replace "ttyACM0" with "ttyUSB0". And if you are using the SITL to connect to your terminal, you have to replace "/dev/ttyACM0:57600" with "udp://:[email protected]:14557". Modifying "gcs_url" is to connect your Pixhawk with UDP, because serial communication cannot accept MAVROS, and your nutshell connection simultaneously.
Write your IP address at "xxx.xx.xxx.xxx"
xml... <arg name="fcu_url" default="/dev/ttyACM0:57600" /> <arg name="gcs_url" default="udp://:[email protected]:14557" /> ...
Build for PX4
Clean the previously built PX4-Autopilot directory. In the root of PX4-Autopilot directory:
shmake clean
Build PX4-Autopilot and upload in the normal way.
For example:
- to build for Pixhawk 4/FMUv5 execute the following command in the root of the PX4-Autopilot directory:
shmake px4_fmu-v5_default upload
- to build for SITL execute the following command in the root of the PX4-Autopilot directory (using jmavsim simulation):
shmake px4_sitl jmavsim
构建
测试是否从你的 ROS 话题中接收到 a
字符。
PX4 编译
- In a terminal entersh
roslaunch mavros px4.launch
- In a second terminal run:shThis means, publish 97 ('a' in ASCII) to ROS topic "/mavros/keyboard_command/keyboard_sub" in message type "std_msgs/Char". "-r 10" means to publish continuously in "10Hz".
rostopic pub -r 10 /mavros/keyboard_command/keyboard_sub std_msgs/Char 97
Running PX4
Enter the Pixhawk nutshell through UDP. Replace xxx.xx.xxx.xxx with your IP.
shcd PX4-Autopilot/Tools ./mavlink_shell.py xxx.xx.xxx.xxx:14557 --baudrate 57600
After few seconds, press Enter a couple of times. You should see a prompt in the terminal as below:
shrostopic pub -r 10 /mavros/keyboard_command/keyboard_sub std_msgs/Char 97
Type "key_receiver", to run your subscriber module.
nsh> key_receiver
Check if it successfully receives a
from your ROS topic.