Mavros: 向 QGroundControl 发送 MAVLink 消息

创建于 2016-07-14  ·  14评论  ·  资料来源: mavlink/mavros

你好！

我有一个飞机模拟器，它通过 ROS 主题发送一些飞行信息。 我想使用 MAVLink 将其发送到 QGroundControl。

为了开始测试它是如何工作的，我尝试发送一条带有假电池状态的消息 (mavros_msgs/BatteryStatus.msg)：

1 - 首先，我使用 gcs_url = udp://:[email protected] :14551 运行 mavros_node。
2 - 我运行我的节点，它应该发送电池消息：

#include <ros/ros.h>
#include <mavros_msgs/BatteryStatus.h>

int main(int argc, char *argv[])

{
    ros::init(argc, argv, "battery_node");
    ros::NodeHandle n;
    ros::Publisher battery_pub = n.advertise<mavros_msgs::BatteryStatus>("/mavros/battery", 1000);
    ros::Rate loop_rate(10);

    while (ros::ok())
    {
        mavros_msgs::BatteryStatus baterry_msg;

        baterry_msg.voltage = 5;
        baterry_msg.current = 0.5;
        baterry_msg.remaining = 0.36;

        battery_pub.publish(baterry_msg);

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

3 - 我将 QGroundControl 连接到 UDP：127.0.0.1:14551

...什么也没发生=/

我确信 mavros 和 QGC 之间的连接有效，因为如果我使用 MAVProxy 通过 127.0.0.1:14540 运行模拟，使用 mavros 作为“桥梁”，飞机会被识别。

我究竟做错了什么？

我使用的是 Ubuntu 14、ROS Indigo 和 QGC 2.9.7。

非常感谢您提前，
巴勃罗

question
资料来源
picture pablogarciaaunon

最有用的评论

编写自定义gcs_bridge也可能更好，因为您有MAVConnInterface类，您可以在其中发送消息：

auto gcs_link = MAVConnInterface::open_url("udp-b://@");

mavlink::common::msg::HEARTBEAT hb{};
// fill hb

gcs_link->send_message_ignore_drop(hb);
picture vooon 于 2016-12-03
👍2

所有14条评论

sys_status ：如您所见，mavros 是 _publisher_，而不是订阅者。

vooon picture vooon 于 2016-07-14

你好，

谢谢您的回答。 如果我理解的好，这个消息不能发送，只能在ROS中接收和显示，对吗？

我可以使用 _mavlink/to_ 来发送这些消息吗？
我怎么能做到？

再次感谢！

pablogarciaaunon picture pablogarciaaunon 于 2016-07-15

mavlink/to发送消息给FCU，不GCS。

但是您可以使用gcs_bridge节点和mavlink/from主题。

vooon picture vooon 于 2016-07-15

你好，我们又见面了，

好的，我更改了代码，以便它发送一个简单的心跳消息：

#include <ros/ros.h>
#include <mavros_msgs/Mavlink.h>

int main(int argc, char **argv){

    ros::init(argc, argv, "hertbeat_publisher");
    ros::NodeHandle n;

    ros::Publisher example_pub = n.advertise<mavros_msgs::Mavlink>("/mavlink/from", 1000);
    ros::Rate loop_rate(1); // 1 Hz

    mavros_msgs::Mavlink mavlink_msg;

    mavlink_msg.len = 2;
    mavlink_msg.sysid = 1;
    mavlink_msg.compid = 1;
    mavlink_msg.msgid = 0;

    std::vector<long unsigned int> payload64(7, 0);

    mavlink_msg.payload64 = payload64;

    while (ros::ok())
    {
        mavlink_msg.header.stamp = ros::Time::now();
        if (mavlink_msg.seq < 255){
            mavlink_msg.seq++;
        }
        else {mavlink_msg.seq = 0;}

        // publish message
        example_pub.publish(mavlink_msg);

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

1 - 我使用 fcu_url=udp://:14540@ 和 gcs_url = udp://:[email protected] :14551 运行 mavros_node，得到：

[ INFO] [1468659493.280504313]: FCU URL: udp://:14540@
[ INFO] [1468659493.289025098]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1468659493.289457240]: GCS URL: udp://:[email protected]:14551
[ INFO] [1468659493.289691565]: udp1: Bind address: 0.0.0.0:14570
[ INFO] [1468659493.289824558]: udp1: Remote address: 127.0.0.1:14551
...
[ INFO] [1468659494.906066742]: MAVROS started. MY ID 1.240, TARGET ID 1.1

2 - 我用 gcs_url:='udp://@127.0.0.1' 运行 gcs_bridge，得到：

[ INFO] [1468659758.392659411]: udp0: Bind address: 0.0.0.0:14555
[ INFO] [1468659758.392800665]: udp0: Remote address: 127.0.0.1:14550

3 - 我运行上述节点，以 1 Hz 的频率在 /mavlink/from 中发布。
4 - 我使用系统 ID 1 连接 QGroundControl 使用 UDP 127.0.0.1:14550

...再次 MAVLink 检查器是空的。

一开始，它不会发生任何事情，但在某些时候，QGroundControl 会抛出此错误：

存在 MAVLink 版本或波特率不匹配。 请检查 QGroundControl 和您的飞控的波特率是否相同。

我认为 QGC 在此错误之前的某个时间点以某种方式更改了 UDP 端口。

我究竟做错了什么？

谢谢！

pablogarciaaunon picture pablogarciaaunon 于 2016-07-16

a) 不要使用gcs_url的mavros！ 你应该连接到gcs_bridge ！
b) 用于填充 Mavlink.msg 的错误代码。 应该是这样的：

mavlink::common::msg::HEARTBEAT hb{};
...

mavlink::mavlink_message_t msg;
mavlink::MsgMap map(msg);

hb.serialize(map);
mavlink::finalize_message_buffer(&msg, sysid, compid, ...);

mavros_msgs::Mavlink ml;
mavros_msgs::mavlink::convert(msg, ml);

看：

vooon picture vooon 于 2016-07-16
👍1

你好！

谢谢！ 我终于让它工作了。 我使用了不同的类，因为我使用的是 Indigo。 我把代码留在这里，以防其他人想知道如何准确地做到这一点。 我发送一个心跳和一个高度消息并在 QGroundControl 中接收它们：

1- 使用 fcu_url=udp://:14540@ 和 gcs_url= udp://:[email protected] :14551 启动 mavros 节点。
2- 使用 gcs_url:=udp://@127.0.0.1:14560 启动 gcs_bridge。
3- 启动这个节点：

#include <ros/ros.h>
#include <mavconn/interface.h>
#include <mavros_msgs/Mavlink.h>
#include <mavros_msgs/mavlink_convert.h>

// General parameters
uint8_t system_id = 1;
uint8_t component_id = 1;
uint8_t type = 1;
uint8_t autopilot = 0;
uint8_t base_mode = 128;
uint32_t custom_mode = 0;
uint8_t system_status = 0;

// Parameters of the altitude message
float alt_monotonic = 10.0;
float alt_amsl = 100;
float alt_local = 10.0;
float alt_relative = 10.0;
float alt_terrain = 10;
float bottom_clearance = 0;

int main(int argc, char **argv){

    // Create heartbeat message
    mavlink_message_t msg_heartbeat;
    mavlink_msg_heartbeat_pack(system_id, component_id, &msg_heartbeat, type, autopilot, base_mode, custom_mode, system_status);

    mavros_msgs::Mavlink ml_heartbeat;
    mavros_msgs::mavlink::convert(msg_heartbeat, ml_heartbeat);

    // Create altitude message
    mavlink_message_t msg_altitude;
    mavros_msgs::Mavlink ml_altitude;

    ros::init(argc, argv, "telemetry_publisher");
    ros::NodeHandle n;

    ros::Publisher example_pub = n.advertise<mavros_msgs::Mavlink>("/mavlink/from", 1000);
    ros::Rate loop_rate(1); // 1 Hz

    while (ros::ok())
    {
        uint64_t time_stamp = ros::Time::now().nsec;

        // Finish altitude message
        mavlink_msg_altitude_pack(system_id, component_id, &msg_altitude, time_stamp, alt_monotonic, alt_amsl, alt_local, alt_relative, alt_terrain, bottom_clearance);
        mavros_msgs::mavlink::convert(msg_altitude, ml_altitude);

        // Publish messages
        example_pub.publish(ml_heartbeat);
        example_pub.publish(ml_altitude);

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

4- 启动 QGroundControl 并在 127.0.0.1:14560 配置连接

如果您打开 MavLink Inspector，您应该会看到大约 1Hz 的心跳和高度信息。

pablogarciaaunon picture pablogarciaaunon 于 2016-07-18

我尝试这样做，但出现错误，我真的不明白为什么，有人可以帮助我吗？
#include 时必须包含 mavlink_msg_heartbeat_pack 函数但它没有...
可能是我在 cmake 中错了？

#include "ros/ros.h"
#include <mavros_msgs/mavlink_convert.h>
#include <mavconn/interface.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<mavros_msgs::Mavlink>("/mavlink/from", 1000);
    ros::Rate loop_rate(10);

    uint8_t system_id = 1;
    uint8_t component_id = 1;
    uint8_t type = 1;
    uint8_t autopilot = 0;
    uint8_t base_mode = 128;
    uint32_t custom_mode = 0;
    uint8_t system_status = 0;
    mavlink_message_t msg;
    mavlink_msg_heartbeat_pack(system_id, component_id, &msg, type, autopilot, base_mode, custom_mode, system_status);
  return 0;
}

cmake_minimum_required(VERSION 2.8.3)
project(talker)

find_package(catkin REQUIRED COMPONENTS mavros mavros_msgs mavlink)
catkin_package()
include(EnableCXX11)
include(MavrosMavlink)
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${mavlink_INCLUDE_DIRS}
)

add_executable(talker talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})

错误：'mavlink_message_t' 未在此范围内声明
mavlink_message_t 味精；
错误：“mavlink_msg_heartbeat_pack”未在此范围内声明
mavlink_msg_heartbeat_pack(system_id, component_id, &msg, type, autopilot, base_mode, custom_mode, system_status);

shawinmihail picture shawinmihail 于 2016-11-29

@shawinmihail什么版本的 mavros？ 0.18+ 将只包含 mavlink_helpers.h 和 C++11 lib。

vooon picture vooon 于 2016-11-29

我找到了解决我的问题的方法。 可能 mavros v0.18+ 仅使用 mavlink v2.0 标头。 要创建任何消息，我们应该使用 MsgMap.serialize() 而不是 msg_name_pack()

    mavlink::mavlink_message_t msg;
    mavlink::MsgMap map(msg);
    mavlink::groupcontrol::msg::GROUP_COEFFS_PART1 msgPart1 //my custom msg;

    msgPart1.kc = kc;
    msgPart1.ks = ks;
        //........................... - fill msg

         msgPart1.serialize(map);
    map.reset();
    auto mi = msgPart1.get_message_info();
    mavlink::mavlink_finalize_message(&msg, system_id, component_id, mi.min_length, mi.length, mi.crc_extra);

    mavros_msgs::Mavlink mavrosMsg;
    mavros_msgs::mavlink::convert(msg, mavrosMsg);

也许它可以帮助某人。 如果你知道更好的方法，请写出来。
谢谢提示。

shawinmihail picture shawinmihail 于 2016-12-02

Message::serialize()做MsgMap::reset() ，不要直接调用！ 它可能会破坏消息。

vooon picture vooon 于 2016-12-03

编写自定义gcs_bridge也可能更好，因为您有MAVConnInterface类，您可以在其中发送消息：

auto gcs_link = MAVConnInterface::open_url("udp-b://@");

mavlink::common::msg::HEARTBEAT hb{};
// fill hb

gcs_link->send_message_ignore_drop(hb);
vooon picture vooon 于 2016-12-03
👍2

@vooon ，感谢您的评论。 我发现我的解决方案不能正确使用 mavlink v1.0 协议。 现在我制作并测试新的，它有效！

#include "ros/ros.h"
#include <mavros_msgs/mavlink_convert.h>
#include <mavconn/interface.h>
#include <iostream>
#include <mavlink/v2.0/mavlink_helpers.h>


float kc = 0.0f; /*< gravity coeff */
float ks = 0.0f; /*< repulsive coeff */
float ka = 0.0f; /*< speed alignment coeff */
float kp = 0.0f; /*< chasing coeff */
float km = 0.0f; /*< waypoint gravity coeff */
float kh = 0.0f; /*< rotate speed coeff */
float vmax = 0.0f; /*< max autopilot velocity */
float rn = 0.0f; /*< search radius of groupmate  */
float rs = 0.0f; /*< repulsiveOn radius */
float rm = 0.0f; /*< getting waypoint radius */
float rmb = 0.0f; /*< change gravity law radius */

void packMavlinkMessage(const mavlink::Message& mavMsg, mavros_msgs::Mavlink &rosMsg)
{
    mavlink::mavlink_message_t msg;
    mavlink::MsgMap map(msg);
    mavMsg.serialize(map);
    auto mi = mavMsg.get_message_info();

    mavlink::mavlink_status_t *status = mavlink::mavlink_get_channel_status(mavlink::MAVLINK_COMM_0);
    status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
    mavlink::mavlink_finalize_message_buffer(&msg, 1, 1, status, mi.min_length, mi.length, mi.crc_extra);

    mavros_msgs::mavlink::convert(msg, rosMsg);
}

void mavlinkCallback(const mavros_msgs::Mavlink& msg)
{
    mavlink::mavlink_message_t mavMsg;
    mavros_msgs::mavlink::convert(msg, mavMsg);

    if(mavMsg.msgid == mavlink::groupcontrol::msg::SET_GROUP_PARAMS::MSG_ID)
    {
        ROS_WARN("SET GROUP_PARAMS");
        mavlink::MsgMap map(mavMsg);
        mavlink::groupcontrol::msg::SET_GROUP_PARAMS setParamsMsg;
        setParamsMsg.deserialize(map);

        std::cout << setParamsMsg.kc;
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<mavros_msgs::Mavlink>("/mavlink/from", 1000);
    ros::Subscriber sub = n.subscribe("mavlink/to", 1000, mavlinkCallback);
    ros::Rate loop_rate(10);

    ROS_INFO("%s", "Talker started");

    mavlink::groupcontrol::msg::GROUP_PARAMS msgPart1;
    msgPart1.kc = kc;
    msgPart1.ks = ks;
    msgPart1.ka = ka;
    msgPart1.kp = kp;
    msgPart1.km = km;
    msgPart1.kh = kh;
    msgPart1.vmax = vmax;
    msgPart1.rn = rn;
    msgPart1.rs = rs;
    msgPart1.rm = rm;
    msgPart1.rmb = rmb;

    while (ros::ok())
    {
        mavros_msgs::Mavlink mavrosMsg;
        packMavlinkMessage(msgPart1, mavrosMsg);
        pub.publish(mavrosMsg);
        ros::spinOnce();
        loop_rate.sleep();
    }


  return 0;
}
shawinmihail picture shawinmihail 于 2016-12-05

可能有人可以帮我写
我如何从 python 中做同样的事情，例如
创建 mavlink msg 并将其转换为 mavros msg？

shawinmihail picture shawinmihail 于 2016-12-09

编写自定义gcs_bridge也可能更好，因为您有MAVConnInterface类，您可以在其中发送消息：

auto gcs_link = MAVConnInterface::open_url("udp-b://@");

mavlink::common::msg::HEARTBEAT hb{};
// fill hb

gcs_link->send_message_ignore_drop(hb);

这种方法可以简单地发送自定义msg，但是有什么风险吗？ @vooon

GaoGeolone picture GaoGeolone 于 2018-11-12
此页面是否有帮助？
0 / 5 - 0 等级

相关问题

zhahaoyu picture zhahaoyu  ·  12评论
mohand150 picture mohand150  ·  5评论
watakandai picture watakandai  ·  8评论
eleboss picture eleboss  ·  3评论
jannsta1 picture jannsta1  ·  7评论