mavlink serial control

serial_control.py

#!/usr/bin/env python

import rospy
from mavros_msgs.msg import Mavlink
from mavros import mavlink
from pymavlink import mavutil

rospy.init_node('serial_data')
rospy.loginfo('Node inited')


def handle_mavlink(m):
    # print repr(m.msgid)
    if m.msgid == 126:
        print "Serial control messaage", m


mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
rospy.Subscriber('mavlink/from', Mavlink, handle_mavlink)

link = mavutil.mavlink.MAVLink('', 255, 1)


COMMAND = 'commander check\n'

msg = mavutil.mavlink.MAVLink_serial_control_message(
    device=mavutil.mavlink.SERIAL_CONTROL_DEV_SHELL,
    flags=mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND | mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
          mavutil.mavlink.SERIAL_CONTROL_FLAG_MULTI,
    timeout=3,
    baudrate=0,
    count=70,  #len(COMMAND),
    data=map(ord, COMMAND.ljust(70, '\0')))
msg.pack(link)
ros_msg = mavlink.convert_to_rosmsg(msg)

mavlink_pub.publish(ros_msg)

rospy.spin()

У меня примерно такое работает:

#!/usr/bin/env python

import rospy
from mavros_msgs.msg import Mavlink
from mavros import mavlink
from pymavlink import mavutil

rospy.init_node('serial_data')
rospy.loginfo('Node inited')


def handle_mavlink(m):
    if m.msgid == 126:
        mavlink_msg = mavlink.convert_to_bytes(m)
        decoded_msg = link.decode(mavlink_msg)
        data = decoded_msg.data[:decoded_msg.count]
        buf = ''.join(str(chr(x)) for x in data)
        print 'Decoded: ', buf


mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
rospy.Subscriber('/mavlink/from', Mavlink, handle_mavlink)

link = mavutil.mavlink.MAVLink('', 255, 1)

rospy.sleep(1)
rospy.loginfo('Creating MAVLink message')

def send_cmd(cmd):
    msg = mavutil.mavlink.MAVLink_serial_control_message(
        device=mavutil.mavlink.SERIAL_CONTROL_DEV_SHELL,
        flags=mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND | mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
              mavutil.mavlink.SERIAL_CONTROL_FLAG_MULTI,
        timeout=3,
        baudrate=0,
        count=len(cmd),
        data=map(ord, cmd.ljust(70, '\0')))
    msg.pack(link)
    ros_msg = mavlink.convert_to_rosmsg(msg)
    rospy.loginfo('Sending MAVLink message')
    mavlink_pub.publish(ros_msg)

send_cmd('\n')
send_cmd('commander check\n')

rospy.spin()

Кстати, в SITLе это работать не будет, в консоли просто появляется ошибка:

ERROR [mavlink] Failed to start shell (-1)