使用 MAVROS 库编写无人机 offboard 控制代码示例

下面是一个使用 MAVROS 库编写的简单 offboard 控制代码，可以控制无人机向前飞行 5 秒钟，然后停止。

#!/usr/bin/env python

import rospy
from mavros_msgs.msg import *
from mavros_msgs.srv import *
from geometry_msgs.msg import TwistStamped

# 定义全局变量
current_state = State()
current_pose = PoseStamped()
current_velocity = TwistStamped()

# 回调函数，获取当前状态信息
def state_cb(msg):
    global current_state
    current_state = msg

# 回调函数，获取当前位置信息
def pose_cb(msg):
    global current_pose
    current_pose = msg

# 回调函数，获取当前速度信息
def velocity_cb(msg):
    global current_velocity
    current_velocity = msg

# 主函数
if __name__ == '__main__':
    # 初始化节点
    rospy.init_node('offboard_control', anonymous=True)

    # 订阅当前状态信息
    state_sub = rospy.Subscriber('mavros/state', State, state_cb)

    # 订阅当前位置信息
    pose_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_cb)

    # 订阅当前速度信息
    velocity_sub = rospy.Subscriber('mavros/local_position/velocity', TwistStamped, velocity_cb)

    # 发布控制指令
    local_vel_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)

    # 等待连接
    while not rospy.is_shutdown() and not current_state.connected:
        rospy.sleep(0.1)

    # 设置目标速度
    vel_cmd = TwistStamped()
    vel_cmd.twist.linear.x = 1.0 # 向前飞行1m/s
    vel_cmd.header.stamp = rospy.Time.now()

    # 发送控制指令
    rate = rospy.Rate(10)
    start_time = rospy.Time.now()
    while not rospy.is_shutdown() and (rospy.Time.now() - start_time).to_sec() < 5.0:
        local_vel_pub.publish(vel_cmd)
        rate.sleep()

    # 停止飞行
    vel_cmd.twist.linear.x = 0.0 # 停止
    vel_cmd.header.stamp = rospy.Time.now()

    # 发送控制指令
    while not rospy.is_shutdown() and (rospy.Time.now() - start_time).to_sec() < 10.0:
        local_vel_pub.publish(vel_cmd)
        rate.sleep()

此代码订阅了 MAVROS 库中的三个主题：'mavros/state'，'mavros/local_position/pose'，'mavros/local_position/velocity'，分别获取当前状态信息、位置信息和速度信息。然后，它使用 MAVROS 库中的 'mavros/setpoint_velocity/cmd_vel' 主题发布速度指令。在这个例子中，它向前飞行了 5 秒，然后停止。