Mavros: Kirim pesan MAVLink ke QGroundControl

sys_status : seperti yang Anda lihat, mavros adalah _publisher_, bukan pelanggan.

Halo voon,

Terima kasih atas jawaban Anda. Jika saya memahaminya dengan baik, pesan ini tidak dapat dikirim, dan hanya diterima dan ditampilkan di ROS, benar?

Bisakah saya menggunakan _mavlink/to_ untuk mengirim pesan-pesan itu?
Bagaimana saya bisa melakukannya?

Terima kasih lagi!

mavlink/to mengirim pesan ke FCU, bukan GCS.

Tetapi Anda dapat menggunakan simpul gcs_bridge dan topik mavlink/from .

Halo lagi,

ok, saya mengubah kode sehingga mengirimkan pesan detak jantung sederhana:

#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 - Saya menjalankan mavros_node dengan fcu_url=udp://:14540@ dan gcs_url = udp://:[email protected] :14551, mendapatkan:

[ 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 - Saya menjalankan gcs_bridge dengan gcs_url:='udp://@127.0.0.1', mendapatkan:

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

3 - Saya menjalankan simpul di atas, menerbitkan di /mavlink/from pada 1 Hz.
4 - Saya menghubungkan QGroundControl menggunakan UDP 127.0.0.1:14550, menggunakan ID Sistem 1

... dan lagi inspektur MAVLink kosong.

Pada awalnya, itu tidak terjadi apa-apa, tetapi pada titik tertentu, QGroundControl melempar kesalahan ini:

Ada Versi MAVLink atau Baud Rate Mismatch. Harap periksa apakah baud rate QGroundControl dan autopilot Anda sama.

Saya pikir entah bagaimana QGC mengubah port UDP di beberapa titik sebelum kesalahan ini.

Apa yang saya lakukan salah?

Terima kasih!

a) Jangan gunakan gcs_url dari mavros! Anda harus terhubung ke gcs_bridge !
b) Kode buruk untuk mengisi Mavlink.msg. Seharusnya seperti ini:

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

Lihat:

• https://github.com/mavlink/mavros/blob/master/libmavconn/include/mavconn/msgbuffer.h#L62
• https://github.com/mavlink/mavros/blob/master/mavros_msgs/include/mavros_msgs/mavlink_convert.h

Hai!

Terima kasih! Saya akhirnya membuatnya bekerja. Saya menggunakan kelas yang berbeda, karena saya menggunakan Indigo. Saya meninggalkan kode di sini, kalau-kalau ada orang lain yang bertanya-tanya bagaimana melakukannya dengan tepat. Saya mengirim pesan detak jantung dan ketinggian dan menerimanya di QGroundControl:

1- Luncurkan simpul mavros dengan fcu_url=udp://:14540@ dan gcs_url= udp://:[email protected] :14551.
2- Luncurkan gcs_bridge dengan gcs_url:=udp://@127.0.0.1:14560.
3- Luncurkan simpul ini:

#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- Luncurkan QGroundControl dan konfigurasikan koneksi di 127.0.0.1:14560

Jika Anda membuka MavLink Inspector, Anda akan melihat detak jantung dan pesan ketinggian sekitar ~1Hz.

saya mencoba melakukan ini, tetapi mendapatkan kesalahan, saya benar-benar tidak mengerti mengapa, adakah yang bisa membantu saya?
fungsi mavlink_msg_heartbeat_pack harus disertakan saat #includetapi itu tidak...
Mungkin saya salah dalam 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})

kesalahan: 'mavlink_message_t' tidak dideklarasikan dalam cakupan ini
pesan mavlink_message_t;
kesalahan: 'mavlink_msg_heartbeat_pack' tidak dideklarasikan dalam cakupan ini
mavlink_msg_heartbeat_pack(system_id, component_id, &msg, ketik, autopilot, base_mode, custom_mode, system_status);

@shawinmihail mavros versi berapa? 0.18+ hanya akan menyertakan mavlink_helpers.h dan C++11 lib.

Saya telah menemukan solusi dari masalah saya. Mungkin, mavros v0.18+ hanya menggunakan header mavlink v2.0. Untuk membuat pesan apa pun kita harus menggunakan MsgMap.serialize() bukan 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);

Mungkin itu bisa membantu seseorang. Jika Anda tahu cara yang lebih baik, tolong tulis.
Terima kasih untuk petunjuk.

Message::serialize() tidak MsgMap::reset() , jangan menyebutnya secara langsung! Ini mungkin merusak pesan.

Juga mungkin lebih baik untuk menulis custom gcs_bridge , karena di sana Anda memiliki kelas MAVConnInterface , tempat Anda dapat mengirim pesan:

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

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

gcs_link->send_message_ignore_drop(hb);

@vooon , terima kasih atas komentarnya. Saya menemukan bahwa solusi saya tidak bekerja dengan protokol mavlink v1.0 dengan benar. Sekarang saya membuat dan menguji yang baru, berhasil!

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

mungkin seseorang dapat membantu saya dan menulis
bagaimana saya bisa melakukan hal yang sama dari python misalnya
buat pesan mavlink dan ubah menjadi pesan mavros?

Juga mungkin lebih baik untuk menulis custom gcs_bridge , karena di sana Anda memiliki kelas MAVConnInterface , tempat Anda dapat mengirim pesan:

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

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

gcs_link->send_message_ignore_drop(hb);

Metode ini hanya dapat mengirim pesan khusus, tetapi apakah ada risiko? @vooon