# Sending a Custom Message from MAVROS to PX4
WARNING
This article has been tested against:
- Ubuntu: 18.04
- ROS: Melodic
- PX4 Firmware: 1.9.0
However these steps are fairly general and so it should work with other distros/versions with little to no modifications.
# MAVROS Installation
Follow Source Installation instructions from mavlink/mavros (opens new window) to install "ROS Kinetic".
# MAVROS
We start by creating a new MAVROS plugin, in this example named keyboard_command.cpp (in workspace/src/mavros/mavros_extras/src/plugins) by using the code below:
The code subscribes a 'char' message from ROS topic
/mavros/keyboard_command/keyboard_sub
and sends it as a MAVLink message.#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; UAS_FCU(m_uas)->send_message_ignore_drop(req->data); } }; } // 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:
<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
.add_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:
... <message id="229" name="KEY_COMMAND"> <description>Keyboard char command.</description> <field type="char" name="command"> </field> </message> ...
# PX4 Changes
Inside common.xml (in PX4-Autopilot/mavlink/include/mavlink/v2.0/message_definitions), add your MAVLink message as following (same procedure as for MAVROS section above):
... <message id="229" name="KEY_COMMAND"> <description>Keyboard char command.</description> <field type="char" name="command"> </field> </message> ...
Remove common, standard directories in (PX4-Autopilot/mavlink/include/mavlink/v2.0).
rm -r common rm -r standard
Git clone "mavlink_generator" to any directory you want and execute it.
git clone https://github.com/mavlink/mavlink mavlink-generator cd mavlink-generator python mavgenerate.py
You will see a "MAVLink Generator" popup:
- For XML, "Browse" to /PX4-Autopilot/mavlink/include/mavlink/v2.0/message_definitions/standard.xml.
- For Out, "Browse" to /PX4-Autopilot/mavlink/include/mavlink/v2.0/.
- Select Language C
- Select Protocol 2.0
- Check Validate
Then, press Generate. You will see common, and standard directories created in /PX4-Autopilot/mavlink/include/mavlink/v2.0/.
Make your own uORB message file key_command.msg in (PX4-Autopilot/msg). For this example the "key_command.msg" has only the code:
char cmd
Then, in CMakeLists.txt (in PX4-Autopilot/msg), include
set( ... key_command.msg )
Edit mavlink_receiver.h (in PX4-Autopilot/src/modules/mavlink)
... #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). This is where PX4 receives the MAVLink message sent from ROS, and publishes it as a uORB topic.
... 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); } }
Make your own uORB topic subscriber just like any example subscriber module. For this example lets create the model in (/PX4-Autopilot/src/modules/key_receiver). In this directory, create two files CMakeLists.txt, key_receiver.cpp. Each one looks like following.
-CMakeLists.txt
px4_add_module( MODULE modules__key_receiver MAIN key_receiver STACK_MAIN 2500 STACK_MAX 4000 SRCS key_receiver.cpp DEPENDS platforms__common )
-key_receiver.cpp
#include <px4_config.h> #include <px4_tasks.h> #include <px4_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; }
For a more detailed explanation see the topic Writing your first application.
Lastly add your module in the default.cmake 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.cmake:
MODULES ... key_receiver ...
Now you are ready to build all your work!
# 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". 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"
... <arg name="fcu_url" default="/dev/ttyACM0:57600" /> <arg name="gcs_url" default="udp://:14550@xxx.xx.xxx.xxx:14557" /> ...
# Build for PX4
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:
make px4_fmu-v5_default upload
# Running the Code
Next test if the MAVROS message is sent to PX4.
# Running ROS
- In a terminal enter
roslaunch mavros px4.launch
- In a second terminal run:This 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.
cd 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:
nsh> nsh>
Type "key_receiver", to run your subscriber module.
nsh> key_receiver
Check if it successfully receives a
from your ROS topic.