Mavros sends a custom topic message to Px4

Preface

ROS Melodic
PX4 1.13.0

One 、PX4 Add custom mavlink news

Modify the following path common.xml file

Add... At the position shown below

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

And then in ~/PX4-Autopilot Execute first under directory

make clean

Re execution

make px4_sitl_default

After successful compilation, it will be found in the path below

mavlink_msg_key_command.h

At this time, you can customize mavlink The message file is successfully generated

Two 、PX4 Add custom uorb news

Add... In the following directory key_command.msg file
The contents of the document are as follows ：

uint64 timestamp
char cmd

Modify the following file

Add... At the position shown below ：

key_command.msg

3、 ... and 、PX4 Receive custom mavlink news

Modify the following figure mavlink_receiver.h

Add ：

#include <uORB/topics/key_command.h>

Add ：

void handle_message_key_command(mavlink_message_t *msg);

Add ：

orb_advert_t _key_command_pub{nullptr};

Modify the following figure mavlink_receiver.cpp

Add ：

case MAVLINK_MSG_ID_KEY_COMMAND:
 	   handle_message_key_command(msg);
 	   break;

Add ：

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

Four 、PX4 Print custom mavlink data

Add... In the following directory key_receiver Folder

Add the following three files to it

The contents of each document are as follows ：
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;
}

Modify the following file

Add... At the position shown below ：

CONFIG_MODULES_KEY_RECEIVER=y

Then compile

make px4_sitl_default gazebo

Enter... At the current terminal

help

Can find key_receiver Description compilation is normal

5、 ... and 、MAVROS Send custom mavlink data

Add... At the position shown below keyboard_command.cpp Folder

The contents of the document are as follows ：

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

Modify the following file

Add... At the position shown below ：

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

Modify the following file

Add... At the position shown below ：

src/plugins/keyboard_command.cpp

Modify the following figure common.xml file

Add... At the position shown below ：

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

And then in mavros Compile in workspace mavros

catkin build

6、 ... and 、 test

The first run ：

roslaunch px4 mavros_posix_sitl.launch 

And then at boot launch Execute under the terminal of the file

key_receiver start

Here's the picture ：

Then open a new terminal , perform ：

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

start-up launch The printing information of the document terminal will change from the original Got no data within a second become

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

Here's the picture , If the published data is changed to 98, Will print Recieved Char : b.