Mavros: Envíe mensajes MAVLink a QGroundControl

sys_status : como puede ver, mavros es _publisher_, no suscriptor.

Hola vooon,

Gracias por su respuesta. Si lo entendí bien, este mensaje no se puede enviar, y solo se recibe y se muestra en ROS, ¿verdad?

¿Puedo usar entonces _mavlink / to_ para enviar esos mensajes?
¿Cómo podría hacerlo?

¡Gracias de nuevo!

mavlink/to envía un mensaje a FCU, no a GCS.

Pero puede usar el nodo gcs_bridge y el tema mavlink/from .

Hola de nuevo,

ok, cambié el código para que envíe un simple mensaje de latido:

#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 - Ejecuto mavros_node con fcu_url = udp: //: 14540 @ y gcs_url = udp: //: [email protected] : 14551, obteniendo:

[ 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 - Ejecuto gcs_bridge con gcs_url: = 'udp: //@127.0.0.1', obteniendo:

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

3 - Ejecuto el nodo anterior, publicando en / mavlink / desde a 1 Hz.
4 - Conecto QGroundControl usando UDP 127.0.0.1:14550, usando el ID del sistema 1

... y nuevamente el inspector MAVLink está vacío.

Al principio no pasa nada, pero en algún momento QGroundControl arroja este error:

Hay una versión de MAVLink o una discordancia de velocidad en baudios. Compruebe si las velocidades en baudios de QGroundControl y su piloto automático son las mismas.

Creo que de alguna manera QGC cambia el puerto UDP en algún momento antes de este error.

¿Qué estoy haciendo mal?

¡Gracias!

a) ¡No uses gcs_url de mavros! ¡Debería conectarse a gcs_bridge !
b) Código incorrecto para completar Mavlink.msg. Debería ser algo como esto:

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

Ver:

• 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

¡Hola!

¡Gracias! Finalmente lo hice funcionar. Usé diferentes clases, ya que estoy usando Indigo. Dejo aquí el código, por si alguien más se pregunta cómo hacerlo exactamente. Envío un latido y un mensaje de altitud y los recibo en el QGroundControl:

1- Inicie el nodo mavros con fcu_url = udp: //: 14540 @ y gcs_url = udp: //: [email protected] : 14551.
2- Inicie gcs_bridge con gcs_url: = udp: //@127.0.0.1: 14560.
3- Ejecuta este nodo:

#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- Inicie QGroundControl y configure la conexión en 127.0.0.1:14560

Si abre MavLink Inspector, debería ver el latido del corazón y el mensaje de altitud a ~ 1Hz aproximadamente.

Intento hacer esto, pero obtengo un error, realmente no puedo entender por qué, ¿alguien puede ayudarme?
La función mavlink_msg_heartbeat_pack debe incluirse cuando #includepero no fue así ...
¿Puede estar equivocado en 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})

error: 'mavlink_message_t' no se declaró en este ámbito
mavlink_message_t msg;
error: 'mavlink_msg_heartbeat_pack' no se declaró en este ámbito
mavlink_msg_heartbeat_pack (system_id, component_id, & msg, tipo, piloto automático, base_mode, custom_mode, system_status);

@shawinmihail ¿qué versión de mavros? 0.18+ solo incluirá mavlink_helpers.hy C ++ 11 lib.

He encontrado una solución a mi problema. Probablemente, mavros v0.18 + solo usa encabezados de mavlink v2.0. Para crear cualquier mensaje debemos usar MsgMap.serialize () en lugar 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);

Quizás pueda ayudar a alguien. Si conoce una mejor manera, escríbalo.
Gracias por la pista.

Message::serialize() hace MsgMap::reset() , ¡no lo llames directamente! Puede romper el mensaje.

También quizás sea mejor escribir gcs_bridge , ya que allí tiene la clase MAVConnInterface , donde puede enviar su mensaje:

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

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

gcs_link->send_message_ignore_drop(hb);

@vooon , gracias por los comentarios. Encuentro que mi solución no funciona correctamente con el protocolo mavlink v1.0. Ahora hago y pruebo uno nuevo, ¡funciona!

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

puede haber alguien que pueda ayudarme y escribir
cómo puedo hacer lo mismo desde python, por ejemplo
crear mavlink msg y convertirlo a mavros msg?

También quizás sea mejor escribir gcs_bridge , ya que allí tiene la clase MAVConnInterface , donde puede enviar su mensaje:

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

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

gcs_link->send_message_ignore_drop(hb);

Este método podría simplemente enviar un mensaje personalizado, pero ¿existe algún riesgo? @vooon