Manual

[JoonmoAhn]

Version :

Ubuntu : 14.04
ROS : Indigo
PX4 Firmware : 1.6.2 dev, px4fmu-v2_default

MAVROS installation : Simply follow instruction "Source Installation" from this link Even with ROS Indigo on your computer, just install mavlink in "kinetic" version

MAVROS LEVEL :

1. Make "keyboard_command.cpp" as mavros plugin in (workspace/src/mavros/mavros_extras/src/plugins). Following is sample code for "keyboard_command.cpp". This code subscribes a 'char' message from ROS topic "/mavros/keyboard_command/keyboard_sub" and send it as Mavlink message.

#include <mavros/mavros_plugin.h>
#include <pluginlib/class_list_macros.h>
#include <iostream>
#include <std_msgs/Char.h>

namespace mavplugin {

class KeyboardCommandPlugin : public MavRosPlugin {
public:
    KeyboardCommandPlugin() :
        nh("~keyboard_command"),
        uas(nullptr)
    { };

    void initialize(UAS &uas_)
    {
        uas = &uas_;
        keyboard_sub = nh.subscribe("keyboard_sub", 10, &KeyboardCommandPlugin::keyboard_cb, this);
    };

    const message_map get_rx_handlers() {
        return {/* RX disabled */ };
    }

private:
    ros::NodeHandle nh;
    UAS *uas;
    ros::Subscriber keyboard_sub;

    void send_to_pixhawk(char cmd)
    {
        mavlink_message_t msg;
        mavlink_msg_key_command_pack_chan(UAS_PACK_CHAN(uas), &msg, cmd);
        UAS_FCU(uas)->send_message(&msg);
    }

    void keyboard_cb(const std_msgs::Char::ConstPtr &req)
    {
        std::cout << "Got Char : " << req->data <<  std::endl;
        send_to_pixhawk(req->data);
    }
};
};

PLUGINLIB_EXPORT_CLASS(mavplugin::KeyboardCommandPlugin, mavplugin::MavRosPlugin)

2. Edit "mavros_plugins.xml" in (workspace/src/mavros/mavros_extras). Add following lines.

   <class name="keyboard_command" type="mavplugin::KeyboardCommandPlugin" base_class_type="mavplugin::MavRosPlugin">
      <description>Accepts Keyboard Command </description>
   </class>

3. Edit "CMakeLists.txt" in (workspace/src/mavros/mavros_extras). Add following one line in "add_library".

   add_library( 
   ...
   src/plugins/keyboard_command.cpp 
   )

4. Inside "common.xml" in (workspace/src/mavlink/message_definitions/v1.0), add your mavlink message as following.

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

PX4 LEVEL :

1. Inside "common.xml" in (Firmware/mavlink/include/mavlink/v2.0/message_definitions), add your mavlink message as following. (Just like MAVROS LEVEL step 4)

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

2. Remove "common", "standard" directories in (Firmware/mavlink/include/mavlink/v2.0).

   rm -r common
   rm -r standard

3. Git clone "mavlink_generator" at your home directory (or 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 some little window that is titled "MAVLink Generator".

   At XML, "Browse" to (/Firmware/mavlink/include/mavlink/v2.0/message_definitions/standard.xml).
   At Out, "Browse" to (/Firmware/mavlink/include/mavlink/v2.0/).
   Select Language "C"
   Select Protocol "2.0"
   Check Validate

   Then, press Generate!! You will see "common", and "standard" directories made in (/Firmware/mavlink/include/mavlink/v2.0/)

4. Make your own uORB message file "key_command.msg" in (Firmware/msg). My "key_command.msg" has only char cmd in the code. Then, in "CMakeLists.txt" in (Firmware/msg), include

   set(
   ...
    key_command.msg
    )

5. Edit "mavlink_receiver.h" in (Firmware/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;
   }

6. Edit "mavlink_receiver.cpp" in (Firmware/src/modules/mavlink). This is where Px4 receives Mavlink message sent from ROS, and publish it as uORB topic.

...
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
...
    _key_command_pub(nullptr)
...
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);
    }
}

7. Make your own uORB topic subscriber just like any example subscriber module. In my case, I made my own module in (/Firmware/src/inrol/key_receiver). In this directory, I have two files "CMakeLists.txt", "key_receiver.cpp". Each one looks like following.

-CMakeListst.txt

px4_add_module(
    MODULE inrol__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(1)
    {
        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;
}

8. Add your module in "nuttx_px4fmu-v2_default.cmake" in (Firmware/cmake/configs).

   set(config_module_list
   ...
   inrol/key_receiver
   )

NOW!! You are ready to build all your work!!

Build at ROS :

1. At your workspace "catkin build".

2. 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 at PX4 :

1. Building your firmware

   
   cd Firmware
   make px4fmu-v2_default

2. Uploading your firmware on your Pixhawk (Also in "Firmware" directory).

make px4fmu-v2_default upload

NOW, let's test if your Mavros message is sent to Px4!

**Running at ROS** :
1. At first terminal,
`roslaunch mavros px4.launch`

2. At second terminal,
`rostopic pub -r 10 /mavros/keyboard_command/keyboard_sub std_msgs/Char 97`
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".

**Running at PX4** :
1. Go into your Pixhawk's nutshell through UDP. Write your IP at xxx.xx.xxx.xxx.

cd Firmware/Tools ./mavlink_shell.py xxx.xx.xxx.xxx:14557 --baudrate 57600

2. Then, after few seconds, press "Enter" several times. Then, you will see something like 
`nsh>`
in your terminal.
If then, type "key_receiver", which is to run your subscriber module!
`nsh> key_receiver`

Check if it succesfully receives 'a' from your ROS topic.

[visrinivasan]

Hi JoonmoAhn,

I am trying to publish a custom MAVLink message to PX4 and have followed the same steps. I saw your post here and could see that all of my steps are the same. I saw that you could solve your issue.

The only observation between your issue in the link I mentioned and the Manual you posted is generating Mavlink headers in v2.0 instead of v1.0 in the Firmware. On the Mavlink folder in ros workspace, its still v1.0.

Correct me if this is the only change and did this lead to publishing your Mavlink message?

Thanks.

-VS

[JoonmoAhn]

@visrinivasan Did you follow the steps that I wrote in my previous post?? Several changes were made from that post and my manual. The big changes are

1. Mavlink ID was changed from 150 to 229 (or could be any other number that is not used by other mavlink messages)

2. Even though you are using ROS Indigo, you have to install mavlink in Kinetic version.

3. Even though it uses v1.0 in Mavros, Firmware uses v2.0 for some reason. I tried changing v1.0 files in Firmware, but no errors were made while building firmware, which means Firmware doesn't use v1.0. So, you have to edit your mavlink message in v2.0 for Firmware.

4. I included my mavlink message in Firmware "common.xml" file instead of making in other "custom_message.xml" file. Then, I had to remove original "common", and "standard" directories. And, mavgenerate new "common" and "standard" directories by "standard.xml". If you only mavgenerate "standard.xml" it automatically makes "common" directory and corresponding mavlink header files also.

After these changes, I could solve the problem. Hope this helps you a little.

[visrinivasan]

@JoonmoAhn thanks for the update. I had everything in place except for using Firmware v2.0.

Got it working now. :)

[visrinivasan]

@JoonmoAhn now that I am receiving my message on nsh, my next step is to visualize this MAVLink message on QGC. Have you tried doing that? I am having some issue trying to connect to QGC and Mavros at the same time. The connection flow I am looking for is: Mavros<-->UDP<-->FCU<-->Telemetry<-->QGC

[pratyusv]

@visrinivasan @JoonmoAhn In the function send_to_pixhawk() the message type is mavlink_message_t. But I want this message to be sent to custom mavlink message that I have added in the common.xml. How can I achieve this.

[visrinivasan]

@pratyusv did you use the template and follow similar to what @JoonmoAhn has provided above? I am still not sure if I understand your question properly. What is your message ID for your custom message that you added to common.xml (CAUTION: ensure that the ID has not been used by other MAVLINK messages)

[basharnavaz]

@JoonmoAhn I am continuing on a project from a PhD student. His Ground station workspace doesnt seem to have the Mavlink folder which is the last step as outlined. He seems to have had the PixHawk system with Mavros working fine till now. How does one go about this in the case that Mavlink doesnt seem to be there? I just started on this project a week back so I am still catching up on Mavros.

Cheers

[hamishwillee]

@JoonmoAhn This document looks excellent. There has been a request to include it in the PX4 Devguide - https://github.com/PX4/Devguide/issues/806 and update it to ROS melodic.

1. Would you be OK with that? (i.e. to contribute it under the CC license we use for our docs?)
2. If so, would you be able to create a PR that adds it into this folder: https://github.com/PX4/Devguide/tree/master/en/ros - I can do this, but better for you to get credit!
3. If you want you could update to ROS melodic on Ubuntu 18.04 - but if not we can take care of that here.

[JoonmoAhn]

@hamishwillee Thank you for your compliment. It would be my honor to include my documentation into the PX4 Devguide! However, I haven't been using PX4 for long time, so I can't modify the documentation into up-to-date version of PX4 and ROS. If you are okay, it will be really thankful if you could upload the documentation and modify it into up-to-date version.

[hamishwillee]

Thanks! I've created a PR to pull it in https://github.com/PX4/Devguide/pull/810

Hopefully it will get updated first :-)