Send a message to /mavlink/from , why can't GCS receive it

[liu-jiaming]

I generated mavlink message with C language library, how to send it to GCS(not fcu). If you send it to FCU instead, FCU can receive it normally, but I want to send it to GCS. It feels strange.

#include "../include/mavlink_v2/ardupilotmega/mavlink.h"
#include <ros/ros.h>
#include <mavros_msgs/Mavlink.h>
#include <std_msgs/Bool.h>

#define SYS_ID 1
#define COMP_ID 241

using namespace std;

template <class T>
int getArrayLen(T &array)
{
  return sizeof(array) / sizeof(array[0]);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "recordVideo");
  ros::NodeHandle n;

  iscapturing.data = false;
  //
  ros::Publisher capture_status_pub = n.advertise<mavros_msgs::Mavlink>("/mavlink/from", 1);

  //
  int pnum = (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_CHECKSUM_BYTES + 7) / 8;
  mavlink_camera_capture_status_t mavlink_camera_capture_status;
  mavlink_camera_capture_status.available_capacity = 0;
  mavlink_camera_capture_status.image_count = 0;
  mavlink_camera_capture_status.image_interval = 0;
  mavlink_camera_capture_status.image_status = 0;
  mavlink_camera_capture_status.recording_time_ms=0; //
  mavlink_camera_capture_status.time_boot_ms=0;  //
  mavlink_camera_capture_status.video_status = 0;

  mavros_msgs::Mavlink mavros_msg;
  mavros_msg.framing_status=1;
  mavros_msg.magic = mavros_msg.MAVLINK_V20;
  mavros_msg.incompat_flags = 0;
  mavros_msg.compat_flags = 0;
  mavros_msg.sysid = SYS_ID;
  mavros_msg.compid = COMP_ID;
  mavros_msg.msgid = MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS;
  for (int i = 0; i < pnum; i++)
  {
    mavros_msg.payload64.push_back(0);
  }

  ros::Rate loop_rate(10);
  int iscap = false;
  ros::Time start = ros::Time::now();
  while (ros::ok())
  {
    //cout << ros::Time::now() << endl;
    //cout << ros::Time::now() - start << endl;

    mavlink_camera_capture_status.recording_time_ms = (ros::Time::now() - start).toSec() * 1000;
    mavlink_message_t mavlink_message;
    mavlink_camera_capture_status.time_boot_ms=ros::Time::now().toSec()*1000;
    mavlink_msg_camera_capture_status_encode(SYS_ID, COMP_ID, &mavlink_message, &mavlink_camera_capture_status);
    mavros_msg.header.stamp.sec=ros::Time::now().toSec();
    mavros_msg.header.stamp.nsec=(ros::Time::now().toSec()-mavros_msg.header.stamp.sec)*1000000000;
    mavros_msg.len = mavlink_message.len;
    //mavros_msg.len =22;
    mavros_msg.seq = mavlink_message.seq;
    mavros_msg.checksum = mavlink_message.checksum;
    for (int i = 0; i < pnum; i++)
    {
      mavros_msg.payload64[i] = mavlink_message.payload64[i];
      // cout << mavlink_message.payload64[i] << endl;
    }
    capture_status_pub.publish(mavros_msg);
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

[vooon]

Because you didn't finalize your message, so checksum left clear.

https://github.com/mavlink/mavros/blob/master/mavros_msgs/include/mavros_msgs/mavlink_convert.h

But anyway, it is better to write a plugin as it'll have all required environment.

[vooon]

See also:

https://github.com/mavlink/mavros/blob/220f25ba5dd6df4261275c15009deda0e63ed450/libmavconn/include/mavconn/msgbuffer.h#L53-L67

[liu-jiaming]

I added the following code, but GCS still can't receive the message, Is there anything wrong?

inline bool convert(const mavlink_message_t &mmsg, mavros_msgs::Mavlink &rmsg, uint8_t framing_status = mavros_msgs::Mavlink::FRAMING_OK)
{
    const size_t payload64_len = (mmsg.len + 7) / 8;

    rmsg.framing_status = framing_status;

    // [[[cog:
    // for f in FIELD_NAMES:
    //     cog.outl("rmsg.%s = mmsg.%s;" % (f, f))
    // ]]]
    rmsg.magic = mmsg.magic;
    rmsg.len = mmsg.len;
    rmsg.incompat_flags = mmsg.incompat_flags;
    rmsg.compat_flags = mmsg.compat_flags;
    rmsg.seq = mmsg.seq;
    rmsg.sysid = mmsg.sysid;
    rmsg.compid = mmsg.compid;
    rmsg.msgid = mmsg.msgid;
    rmsg.checksum = mmsg.checksum;
    // [[[end]]] (checksum: 4f0a50d2fcd7eb8823aea3e0806cd698)
    rmsg.payload64 = mavros_msgs::Mavlink::_payload64_type(mmsg.payload64, mmsg.payload64 + payload64_len);

    // copy signature block only if message is signed
    if (mmsg.incompat_flags & MAVLINK_IFLAG_SIGNED)
        rmsg.signature = mavros_msgs::Mavlink::_signature_type(mmsg.signature, mmsg.signature + sizeof(mmsg.signature));
    else
        rmsg.signature.clear();

    return true;
}

int main(int argc, char **argv)
{
...
    mavros_msgs::Mavlink mmsg;
    mmsg.header.stamp.sec=ros::Time::now().toSec();
    mmsg.header.stamp.nsec=(ros::Time::now().toSec()-mavros_msg.header.stamp.sec)*1000000000;
    convert(mavlink_message,mmsg);
    capture_status_pub.publish(mmsg);
...
}

Because you didn't finalize your message, so checksum left clear.

https://github.com/mavlink/mavros/blob/master/mavros_msgs/include/mavros_msgs/mavlink_convert.h

But anyway, it is better to write a plugin as it'll have all required environment.

[liu-jiaming]

The rostopic message can be seen(rostopic echo /mavlink/from), but the GCS does not receive it.

[vooon]

You won't get message on GCS side, /mavlink/to sends data to FCU link.

mavlink_msg_t msg;

mavlink_something_encode(&msg, ...args);
mavlink_finalize_message_buffer(&msg, ...);

Mavlink m;
m.header.stamp = ros::Time::now();
mavlink::convert(msg, m);
p.publish(m);

[liu-jiaming]

You won't get message on GCS side, /mavlink/to sends data to FCU link.

mavlink_msg_t msg;

mavlink_something_encode(&msg, ...args);
mavlink_finalize_message_buffer(&msg, ...);

Mavlink m;
m.header.stamp = ros::Time::now();
mavlink::convert(msg, m);
p.publish(m);

I'm not publishing /mavlink/to , but /mavlink/from

ros::Publisher capture_status_pub = n.advertise<mavros_msgs::Mavlink>("/mavlink/from", 1);

[vooon]

Mavros itself doesn't subscribe to mavlink/from. You have to use gcs_bridge.

[liu-jiaming]

Mavros itself doesn't subscribe to mavlink/from. You have to use gcs_bridge.

Isn't it written in this website to subscribe to mavlink/from? How to use gcs_bridge?Isn't gcs_url parameter filled in apm.launch file enabled? http://wiki.ros.org/mavros

[vooon]

When you're using gcs_url with mavros_node you are enabling built-in bridge, which do not use pubsub.

[liu-jiaming]

When you're using gcs_url with mavros_node you are enabling built-in bridge, which do not use pubsub.

If gcs_url parameter is not used, how can GCS connect with mavros and how can gcs_bridge be enabled

[vooon]

You should write your own launch script which would start mavros_node and a gcs_bridge.

[liu-jiaming]

I have studied it clearly, so it is apm.launch:

<launch>
        <!-- vim: set ft=xml noet : -->
        <!-- example launch script for ArduPilot based FCU's -->

        <arg name="fcu_url" default="/dev/ttyS4:921600" />
        <arg name="gcs_url" default="" />
        <arg name="tgt_system" default="1" />
        <arg name="tgt_component" default="1" />
        <arg name="log_output" default="screen" />
        <arg name="fcu_protocol" default="v2.0" />
        <arg name="respawn_mavros" default="false" />

        <include file="$(find mavros)/launch/node.launch">
                <arg name="pluginlists_yaml" value="$(find mavros)/launch/apm_pluginlists.yaml" />
                <arg name="config_yaml" value="$(find mavros)/launch/apm_config.yaml" />

                <arg name="fcu_url" value="$(arg fcu_url)" />
                <arg name="gcs_url" value="$(arg gcs_url)" />
                <arg name="tgt_system" value="$(arg tgt_system)" />
                <arg name="tgt_component" value="$(arg tgt_component)" />
                <arg name="log_output" value="$(arg log_output)" />
                <arg name="fcu_protocol" value="$(arg fcu_protocol)" />
                <arg name="respawn_mavros" value="$(arg respawn_mavros)" />
        </include>
</launch>

gcs.launch:

<launch>

                <node pkg="mavros" type="gcs_bridge" name="mavlink_bridge">
                        <param name="gcs_url" value="udp://@192.168.10.218:14551" />
                </node>
</launch>