ROS+PX4无人机室内多点飞行代码

对于室内多点飞行，你可以使用ROS和PX4进行编程和控制。以下是一个简单的示例代码，演示如何在室内建立多个目标点，并控制无人机按顺序飞行至这些目标点。

import rospy
from mavros_msgs.msg import PositionTarget
from mavros_msgs.srv import SetMode
from geometry_msgs.msg import PoseStamped

# 目标点列表
waypoints = [
    [0, 0, 1],  # 第一个目标点的 x, y, z 坐标
    [1, 1, 1],  # 第二个目标点的 x, y, z 坐标
    [2, -1, 1], # 第三个目标点的 x, y, z 坐标
]

current_waypoint = 0  # 当前目标点索引

def target_pose_callback(msg):
    global current_waypoint

    # 当前无人机位置
    current_pose = msg.pose.position

    # 当前目标点
    target_waypoint = waypoints[current_waypoint]

    # 计算目标位置与当前位置的距离
    distance = ((target_waypoint[0] - current_pose.x) ** 2 +
                (target_waypoint[1] - current_pose.y) ** 2 +
                (target_waypoint[2] - current_pose.z) ** 2) ** 0.5

    # 如果距离小于一定阈值，则认为已到达目标点
    if distance < 0.1:
        current_waypoint += 1

        # 如果已到达最后一个目标点，则停止飞行
        if current_waypoint == len(waypoints):
            rospy.loginfo("Finished mission!")
            # 停止飞行
            set_mode_client(base_mode=0, custom_mode="AUTO.LAND")
            return

        rospy.loginfo("Reached waypoint %d", current_waypoint)

def main():
    rospy.init_node('multi_point_flight')

    # 订阅当前位置
    rospy.Subscriber('/mavros/local_position/pose', PoseStamped, target_pose_callback)

    # 发布目标位置
    target_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10)

    # 设置飞行模式服务
    set_mode_client = rospy.ServiceProxy('/mavros/set_mode', SetMode)

    # 等待服务可用
    rospy.wait_for_service('/mavros/set_mode')

    # 设置飞行模式为OFFBOARD
    set_mode_client(base_mode=0, custom_mode="OFFBOARD")

    rate = rospy.Rate(10)  # 发布频率为10Hz

    while not rospy.is_shutdown():
        # 创建 PositionTarget 消息对象
        target_msg = PositionTarget()

        # 设置目标位置
        target_msg.position.x = waypoints[current_waypoint][0]
        target_msg.position.y = waypoints[current_waypoint][1]
        target_msg.position.z = waypoints[current_waypoint][2]

        # 发布目标位置消息
        target_pub.publish(target_msg)

        rate.sleep()

if __name__ == '__main__':
    main()

这段代码使用了ROS的mavros包来与PX4进行通信，通过控制无人机的位置实现多点飞行。你可以修改waypoints列表来定义自己的目标点坐标。注意，这只是一个简单的示例代码，实际应用中可能需要更复杂的逻辑和安全措施。确保在使用无人机进行室内飞行时，遵守相关法规，并确保安全操作。