Send a new message with mavros to PX4

[lhc610github]

Issue details

hi guys, I want to send a custom message to the px4 through mavros. This is the content of the whycon_connect.cpp file in the mavros_extras/src/plugins/ directory of the mavros package:

include <mavros/mavros_plugin.h>

include <eigen_conversions/eigen_msg.h>

include <geometry_msgs/PoseArray.h>

namespace mavros { namespace extra_plugins{ class WhyconConnectPlugin : public plugin::PluginBase { public: WhyconConnectPlugin() : PluginBase(), wc_nh("~whyconconnect") { }

    void initialize(UAS &uas_)
    {
            PluginBase::initialize(uas_);
            int num_target;
            wc_nh.param("num_target", num_target, 1);
            n_t = num_target;
            whycon_cn_sub = wc_nh.subscribe("/whycon/poses", 1, >&WhyconConnectPlugin::whycon_cb, this);

    }

    Subscriptions get_subscriptions()
    {
            return { /* Rx disabled */ };
    }

private: ros::NodeHandle wc_nh;

    ros::Subscriber whycon_cn_sub;
    int n_t;

    /* -*- mid-level helpers -*- */
    void whycon_cb(const geometry_msgs::PoseArray::ConstPtr &pose)
    {
            mavlink::common::msg::WHYCON_TARGET pos;
            pos.time_usec = pose->header.stamp.toNSec() / 1000;
            int send_count = 1;
                    ROS_INFO("whyc call back %d ",n_t);
            for (send_count=1;send_count<=n_t;send_count++)
            {
                    pos.pos_x = pose->poses[send_count-1].position.x;
                    pos.pos_y = pose->poses[send_count-1].position.y;
                    pos.pos_z = pose->poses[send_count-1].position.z;
                    pos.orient_x = pose->poses[send_count-1].orientation.x;
                    pos.orient_y = pose->poses[send_count-1].orientation.y;
                    pos.orient_z = pose->poses[send_count-1].orientation.z;
                    pos.orient_w = pose->poses[send_count-1].orientation.w;
                    pos.target_id = send_count;
                    ROS_INFO("whyc: %.2f %.2f %.2f %.2f %.2f %d",pos.pos_x,pos.pos_y,pos.pos_z,pos.orient_x,pos.orient_y,pos.target_id);
                    UAS_FCU(m_uas)->send_message_ignore_drop(pos);
            }
    }

}; } // namespace extra_plugins } // namespace mavros

in /mavros/launch/px4_pluginlists.yaml

plugin_blacklist:
# common
- safety_area
- hil_actuator_controls
- hil_controls
- manual_control
- param
- waypoint
# extras
- image_pub
- vibration
- distance_sensor
- cam_imu_sync
- mocap_fake_gps
- mocap_pose_estimate
- px4flow
- vision_pose_estimate
- vision_speed_estimate
plugin_whitelist: []
#- 'sys_*'

Then I added the code to convert the new mavlink message to a uORB message.

    case MAVLINK_MSG_ID_WHYCON_TARGET:
        PX4_INFO("whycon received!!!");
        handle_message_whycon_target(msg);
        break;

void
MavlinkReceiver::handle_message_whycon_target(mavlink_message_t *msg)
{
    mavlink_whycon_target_t wc_target;
    mavlink_msg_whycon_target_decode(msg, &wc_target);

    struct whycon_target_s whycon_target = {};
    whycon_target.timestamp = sync_stamp(wc_target.time_usec);
    whycon_target.timestamp_received = hrt_absolute_time();

    whycon_target.id = wc_target.target_id;
    whycon_target.x = wc_target.pos_x;
    whycon_target.y = wc_target.pos_y;
    whycon_target.z = wc_target.pos_z;
    whycon_target.q[0] = wc_target.orient_w;
    whycon_target.q[1] = wc_target.orient_x;
    whycon_target.q[2] = wc_target.orient_y;
    whycon_target.q[3] = wc_target.orient_z;
    printf("whycon_target   id :%d \n",whycon_target.id);
    printf("whycon_target q:   %.2f  %.2f  %.2f  %.2f \n",(double)whycon_target.q[0],(double)whycon_target.q[1],(double)whycon_target.q[2],(double)whycon_target.q[3]);
    printf("whycon_target pos: %.2f  %.2f  %.2f\n",(double)whycon_target.x,(double)whycon_target.y,(double)whycon_target.z);
    if (_whycon_target_pub == nullptr) {
        _whycon_target_pub = orb_advertise(ORB_ID(whycon_target), &whycon_target);

    } else {
        orb_publish(ORB_ID(whycon_target), _whycon_target_pub, &whycon_target);
    }
}

MAVROS version and platform

Mavros: ?0.18.7? ROS: ?Kinetic? Ubuntu: ?16.04?

Autopilot type and version

PX4

Version: ?3.7.1?

Node logs

mavros node works fine. but PX4 cannot receive the new message.

Diagnostics

header:
  seq: 26
  stamp:
    secs: 1490621559
    nsecs: 547175592
  frame_id: ''
status:
  -
    level: 1
    name: mavros: FCU connection
    message: not connected
    hardware_id: /dev/ttyUSB0:57600
    values:
      -
        key: Received packets:
        value: 0
      -
        key: Dropped packets:
        value: 0
      -
        key: Buffer overruns:
        value: 0
      -
        key: Parse errors:
        value: 0
      -
        key: Rx sequence number:
        value: 0
      -
        key: Tx sequence number:
        value: 81
      -
        key: Rx total bytes:
        value: 0
      -
        key: Tx total bytes:
        value: 7809
      -
        key: Rx speed:
        value: 0.000000
      -
        key: Tx speed:
        value: 434.000000
  -
    level: 2
    name: mavros: GPS
    message: No satellites
    hardware_id: /dev/ttyUSB0:57600
    values:
      -
        key: Satellites visible
        value: 0
      -
        key: Fix type
        value: 0
      -
        key: EPH (m)
        value: Unknown
      -
        key: EPV (m)
        value: Unknown
  -
    level: 2
    name: mavros: Heartbeat
    message: No events recorded.
    hardware_id: /dev/ttyUSB0:57600
    values:
      -
        key: Heartbeats since startup
        value: 0
      -
        key: Frequency (Hz)
        value: 0.000000
      -
        key: Vehicle type
        value: Generic micro air vehicle
      -
        key: Autopilot type
        value: Generic autopilot
      -
        key: Mode
        value: ''
      -
        key: System status
        value: Uninit
  -
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyUSB0:57600
    values:
      -
        key: Sensor present
        value: 0x00000000
      -
        key: Sensor enabled
        value: 0x00000000
      -
        key: Sensor helth
        value: 0x00000000
      -
        key: CPU Load (%)
        value: 0.0
      -
        key: Drop rate (%)
        value: 0.0
      -
        key: Errors comm
        value: 0
      -
        key: Errors count #1
        value: 0
      -
        key: Errors count #2
        value: 0
      -
        key: Errors count #3
        value: 0
      -
        key: Errors count #4
        value: 0
  -
    level: 2
    name: mavros: Battery
    message: No data
    hardware_id: /dev/ttyUSB0:57600
    values:
      -
        key: Voltage
        value: -1.00
      -
        key: Current
        value: 0.0
      -
        key: Remaining
        value: 0.0
  -
    level: 2
    name: mavros: Time Sync
    message: No events recorded.
    hardware_id: /dev/ttyUSB0:57600
    values:
      -
        key: Timesyncs since startup
        value: 0
      -
        key: Frequency (Hz)
        value: 0.000000
      -
        key: Last dt (ms)
        value: 0.000000
      -
        key: Mean dt (ms)
        value: 0.000000
      -
        key: Last system time (s)
        value: 0.000000000
      -
        key: Time offset (s)
        value: 0.000000000
---

Check ID

rosrun mavros checkid

OK. I got messages from 1:1.

---
Received 79 messages, from 2 addresses
sys:comp   list of messages
 28:1     1
  1:1     0, 65, 147, 70, 1, 105, 74, 141, 83, 245, 36, 30, 32

[vooon]

At begginning not important, but useful: a) use triple tilda ("``") for code block. quote not readable. b)target_idbad name, it looks similar totarget_systemandtarget_component(which used to address message to particular system or system's component). Perhapsindex` may be better. And why start from one?

What strange: diagnostics show "not connected", while checkid sees msgid = 0 (HEARTBEAT). You sure that there no "Got heartbeat" message in roslaunch log?

To check that your message are send to wire you need traffic sniffer, i recommend wireshark. You may set fcu_url:=udp://@127.0.0.1:14555 and then monitor that port and publish poses. There exists wireshark mavlink plugin (wlua), but i never used it.

Then you should check PX4 sources. There may be situation when msg header exist, but used incorrect msg crc array. Unknown and corrupted (bad crc) messages don't pass to handlers.

[lhc610github]

@vooon thank you for reply. The mavros can connect to the PX4 and work fine. PX4 receive TIMESYNC,SYSTEM_TIME and HEARTBEAT MAVLink Messages. However, PX4 cannot receive the new message WHYCON_TARGET.What I have done was: mavros:

1. add the new message in mavlink/message_definitions/v1.0/common.xml. idis 213
2. manually generate the new common floder in /devel/include/mavlink/v1.0/and /devel/include/mavlink/v2.0/
3. add the whycon_connect.cpp file in the mavros_extras/src/plugins/ directory of the mavros_extras package.
4. build the whole workspace by catkin build.

PX4:

1. git clone from https://github.com/mavlink/mavlink -branch mavlink2-cxx11
2. add the new message in mavlink/message_definitions/v1.0/common.xml
3. manually generate the common floder in /Firmware/mavlink/include/mavlink/v1.0/and /Firmware/mavlink/include/mavlink/v2.0/
4. add Add a function that handles the incoming mavlink message in mavlink_receiver.h. add a function that handles the incoming mavlink message in the MavlinkReceiver class in mavlink_receiver.hand mavlink_receiver.cpp
5. build the firmware.

I add PX4_INFO("%d ,msgi" ,msg->msgid) in MavlinkReceiver::handle_message() of mavlink_receiver.cpp. Then the test result was that I can get the 0(HEARTBEAT), 2(SYSTEM_TIME), 111(TIMESYNC). Can not receive 213(new message what I defined) when I publish poses to mavros. Is it should be situation used incorrect msg crc array?

Thanks again for recommend . but wireshark may be too large for my board(Raspberrypi3). Is there a simpler way to monitor the mavros?

[vooon]

Mavros step 2 is not needed, catkin build should generate on xml change. You use source install instructions, right? At p 3 did you add cpp to CMakeLists.txt? But since it loads, ok.

PX4 step 1 is unnecessary, firmware doesn't use C++ gen, better to use master. Also be sure that message definition is identical. It might be CRC error, but first need to check that message are sent.

You may use tcpdump on remote machine:

$ cat ~/bin/router-wireshark.sh 
#!/bin/bash
SSHHOST=root@router
BINPATH=/opt/sbin

wireshark -k -i <(ssh $SSHHOST $BINPATH/tcpdump -s 0 -U -n -w - not port 22)

Or simply set fcu_url:=udp://@<your-pc-ip> and look what UDP packets arrives to your machine.

[KJsouth]

hi,friends .At present ,i am doing the same thing as you .i want to know where to define your own msg.please tell me .thank you.

[lhc610github]

@KJNKJNKJN Mavlink: in /yourworkspace_ws/src/mavlink/message_defines/v1.0/common.xml or add a new .xml file. Ros: in /yourpackage/msg/

[TSC21]

@lhc610github is this answered to you?

[TSC21]

Closing this as there are no updates.