Mavros: Envoyer des messages MAVLink à QGroundControl

sys_status : comme vous pouvez le voir, mavros est _éditeur_, pas abonné.

Bonjour vooon,

Merci pour votre réponse. Si j'ai bien compris, ce message ne peut pas être envoyé, et il n'est reçu et affiché que dans ROS, n'est-ce pas ?

Puis-je utiliser _mavlink/to_ pour envoyer ces messages ?
Comment pourrais-je le faire ?

Merci encore!

mavlink/to envoie un message au FCU, pas au GCS.

Mais vous pouvez utiliser le nœud gcs_bridge et le sujet mavlink/from .

Re-bonjour,

ok, j'ai modifié le code pour qu'il envoie un simple message de pulsation :

#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 - Je lance mavros_node avec fcu_url=udp://:14540@ et gcs_url = udp://:[email protected] :14551, obtenant :

[ 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 - Je lance le gcs_bridge avec gcs_url:='udp://@127.0.0.1', obtenant :

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

3 - Je lance le nœud ci-dessus, en publiant dans /mavlink/from à 1 Hz.
4 - Je connecte QGroundControl en utilisant UDP 127.0.0.1:14550, en utilisant l'ID système 1

... et encore une fois l'inspecteur MAVLink est vide.

Au début, il ne se passe rien, mais à un moment donné, QGroundControl renvoie cette erreur :

Il existe une version MAVLink ou un décalage de débit en bauds. Veuillez vérifier si les débits en bauds de QGroundControl et de votre pilote automatique sont les mêmes.

Je pense que QGC modifie le port UDP à un moment donné avant cette erreur.

Qu'est-ce que je fais mal?

Merci!

a) N'utilisez pas gcs_url de mavros ! Vous devez vous connecter à gcs_bridge !
b) Mauvais code pour remplir Mavlink.msg. Ça devrait être quelque chose comme ça :

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

Voir:

• 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

Salut!

Merci! J'ai finalement réussi à le faire fonctionner. J'ai utilisé différentes classes, puisque j'utilise Indigo. Je laisse ici le code, au cas où quelqu'un d'autre se demande comment le faire exactement. J'envoie un battement de cœur et un message d'altitude et les reçois dans le QGroundControl :

1- Lancez le nœud mavros avec fcu_url=udp://:14540@ et gcs_url= udp://:[email protected] :14551.
2- Lancez gcs_bridge avec gcs_url:=udp://@127.0.0.1:14560.
3- Lancez ce nœud :

#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- Lancez QGroundControl et configurez la connexion à 127.0.0.1:14560

Si vous ouvrez l'inspecteur MavLink, vous devriez voir le rythme cardiaque et le message d'altitude à environ 1 Hz environ.

J'essaie de le faire, mais j'obtiens une erreur, je ne comprends vraiment pas pourquoi, quelqu'un peut-il m'aider?
La fonction mavlink_msg_heartbeat_pack doit être incluse lorsque #includemais ça n'a pas marché...
Peut-être que je me suis trompé dans 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})

erreur : 'mavlink_message_t' n'a pas été déclaré dans cette étendue
mavlink_message_t msg;
erreur : 'mavlink_msg_heartbeat_pack' n'a pas été déclaré dans cette étendue
mavlink_msg_heartbeat_pack(system_id, component_id, &msg, type, pilote automatique, base_mode, custom_mode, system_status);

@shawinmihail quelle version de mavros ? 0.18+ n'inclura que mavlink_helpers.h et C++11 lib.

J'ai trouvé une solution à mon problème. Probablement, mavros v0.18+ utilise uniquement les en-têtes mavlink v2.0. Pour créer un message, nous devons utiliser MsgMap.serialize() au lieu de 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);

Peut-être que ça peut aider quelqu'un. Si vous connaissez un meilleur moyen, veuillez l'écrire.
Merci pour l'astuce.

Message::serialize() fait MsgMap::reset() , ne l'appelez pas directement ! Cela peut casser le message.

Aussi peut-être mieux d'écrire sur mesure gcs_bridge , car là vous avez la classe MAVConnInterface , où vous pouvez envoyer votre message :

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

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

gcs_link->send_message_ignore_drop(hb);

@vooon , merci pour les commentaires. Je trouve que ma solution ne fonctionne pas correctement avec le protocole mavlink v1.0. Maintenant j'en fabrique et j'en teste un nouveau, ça marche !

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

peut-être que quelqu'un peut m'aider et écrire
comment je peux faire la même chose à partir de python, par exemple
créer mavlink msg et le convertir en mavros msg ?

Aussi peut-être mieux d'écrire sur mesure gcs_bridge , car là vous avez la classe MAVConnInterface , où vous pouvez envoyer votre message :

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

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

gcs_link->send_message_ignore_drop(hb);

Cette méthode pourrait simplement envoyer des messages personnalisés, mais y a-t-il un risque ? @vooon