无人机软件接口与说明(二次开发)

警告：此文档仅提供给开发人员使用，请在实机飞行前请务必先完成仿真，且必须在隔离安全网内飞行。

一、激光雷达定位位姿获取

1. 启动激光雷达节点

bash

# 启动MID360激光雷达
roslaunch tutorial_navigation mid360.launch

2. 位姿数据话题说明与位姿数据获取例程

话题名称	消息类型	作用
/mavros/vision_pose/pose	geometry_msgs/PoseStamped	雷达定位位姿数据：激光雷达定位系统输出的无人机位置和姿态信息（包含三维位置和四元数姿态）

Python版本

python

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseStamped

class PoseSubscriber:
    def __init__(self):
        # 订阅/mavros/vision_pose/pose话题
        self.pose_sub = rospy.Subscriber('/mavros/vision_pose/pose', 
                                         PoseStamped, 
                                         self.pose_callback)
    
    def pose_callback(self, msg):
        # 获取位置信息
        x = msg.pose.position.x
        y = msg.pose.position.y
        z = msg.pose.position.z
        
        # 获取姿态信息（四元数）
        qx = msg.pose.orientation.x
        qy = msg.pose.orientation.y
        qz = msg.pose.orientation.z
        qw = msg.pose.orientation.w
        
        # 打印位姿信息
        rospy.loginfo(f"Position: [x: {x:.3f}, y: {y:.3f}, z: {z:.3f}]")
        rospy.loginfo(f"Orientation: [x: {qx:.3f}, y: {qy:.3f}, z: {qz:.3f}, w: {qw:.3f}]")
        
        # 在这里添加您的位姿处理代码

if __name__ == '__main__':
    try:
        rospy.init_node('pose_subscriber_node', anonymous=True)
        pose_subscriber = PoseSubscriber()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

3. 验证数据发布

在启动激光雷达节点后，可以用以下命令验证数据是否正常发布：

bash

# 查看话题列表
rostopic list

# 查看位姿话题消息
rostopic echo /mavros/vision_pose/pose

# 查看话题发布频率
rostopic hz /mavros/vision_pose/pose

4. package.xml配置

xml

<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>

<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>

<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>

这个例程会订阅/mavros/vision_pose/pose话题，并实时打印接收到的位姿数据。您可以在回调函数中添加自己的位姿处理逻辑。

二、无人机速度控制（运动控制）

1.在控制无人机运动前，需要启动两个必要的节点：

bash

# 终端1：启动激光雷达
roslaunch tutorial_navigation mid360.launch

# 终端2：启动Mavlink通讯
roslaunch tutorial_basic mavros.launch

2.相关话题说明以及编程示例

话题名称	消息类型	作用
mavros/setpoint_velocity/cmd_vel	geometry_msgs/TwistStamped	速度控制：发送速度指令（线速度+角速度）
mavros/setpoint_position/local	geometry_msgs/PoseStamped	位置控制：发送目标位置指令
mavros/cmd/arming	mavros_msgs/CommandBool	解锁/上锁服务：控制电机启动/停止
mavros/set_mode	mavros_msgs/SetMode	模式设置服务：切换飞行模式（如OFFBOARD）

---

Python版本

python

https://docs.liuyaorobot.com/wp-login.php

---

package.xml:

xml

<build_depend>geometry_msgs</build_depend>
<build_depend>mavros_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>

<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>mavros_msgs</build_export_depend>

<exec_depend>geometry_msgs</exec_depend>
<exec_depend>mavros_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>

4.使用说明

先启动必要节点：

roslaunch tutorial_navigation mid360.launch

roslaunch tutorial_basic mavros.launch

安全提示：确保无人机周围安全，建议先在仿真环境中测试，准备好遥控器作为备用，随时准备切换回手动模式，运行相关模式和控制时需要将飞行模式切换至OffBord模式（Magpei360系列通常是遥控器右侧按钮）

三、投放装置

1.在控制投放装置前，需要先启动投放装置驱动节点：

启动命令

bash

# 启动投放装置驱动
roslaunch tutorial_catapult catapult_driver.launch

2.相关话题说明与例程

话题名称	消息类型	作用
servo/front_left/open	std_msgs/Empty	左前舵机打开：释放左前位置的物品
servo/front_right/open	std_msgs/Empty	右前舵机打开：释放右前位置的物品
servo/back_left/open	std_msgs/Empty	左后舵机打开：释放左后位置的物品
servo/back_right/open	std_msgs/Empty	右后舵机打开：释放右后位置的物品
servo/all/close	std_msgs/Empty	关闭所有舵机：复位所有舵机（可用于通用关闭）

注意：/open 用于打开舵机（投放物品），/close 用于关闭舵机（复位）。/all 可用于控制所有舵机，也可用于分布控制舵机。

---

Python版本

python

#!/usr/bin/env python3
import rospy
import time
from std_msgs.msg import Empty

class CatapultController:
    def __init__(self):
        # 初始化节点
        rospy.init_node('catapult_controller_node', anonymous=True)
        
        # 创建舵机控制发布者
        self.front_left_open_pub = rospy.Publisher("servo/front_left/open", Empty, queue_size=1)   # 左前舵机打开
        self.front_right_open_pub = rospy.Publisher("servo/front_right/open", Empty, queue_size=1) # 右前舵机打开
        self.back_left_open_pub = rospy.Publisher("servo/back_left/open", Empty, queue_size=1)     # 左后舵机打开
        self.back_right_open_pub = rospy.Publisher("servo/back_right/open", Empty, queue_size=1)   # 右后舵机打开
        self.all_servo_close_pub = rospy.Publisher("servo/all/close", Empty, queue_size=1)          # 关闭所有舵机
        
        # 等待发布者准备就绪
        rospy.sleep(1)
        
        rospy.loginfo("投放装置控制器初始化完成")
        
    def open_front_left(self):
        """打开左前舵机（投放左前物品）"""
        rospy.loginfo("打开左前舵机")
        self.front_left_open_pub.publish(Empty())
        
    def open_front_right(self):
        """打开右前舵机（投放右前物品）"""
        rospy.loginfo("打开右前舵机")
        self.front_right_open_pub.publish(Empty())
        
    def open_back_left(self):
        """打开左后舵机（投放左后物品）"""
        rospy.loginfo("打开左后舵机")
        self.back_left_open_pub.publish(Empty())
        
    def open_back_right(self):
        """打开右后舵机（投放右后物品）"""
        rospy.loginfo("打开右后舵机")
        self.back_right_open_pub.publish(Empty())
        
    def close_all_servos(self):
        """关闭所有舵机（复位）"""
        rospy.loginfo("关闭所有舵机")
        self.all_servo_close_pub.publish(Empty())
        
    def open_all_servos(self):
        """打开所有舵机（同时投放所有物品）"""
        rospy.loginfo("打开所有舵机")
        self.open_front_left()
        self.open_front_right()
        self.open_back_left()
        self.open_back_right()
        
    def sequential_release(self, interval=1.0):
        """顺序投放物品
        
        Args:
            interval: 投放间隔时间（秒）
        """
        rospy.loginfo(f"开始顺序投放物品，间隔 {interval} 秒")
        
        # 按顺序投放
        self.open_front_left()
        time.sleep(interval)
        
        self.open_front_right()
        time.sleep(interval)
        
        self.open_back_left()
        time.sleep(interval)
        
        self.open_back_right()
        time.sleep(interval)
        
        rospy.loginfo("顺序投放完成")
        
    def release_with_delay(self, positions, delay=0.5):
        """带延迟的指定位置投放
        
        Args:
            positions: 要投放的位置列表，如 ['front_left', 'front_right']
            delay: 每个投放之间的延迟（秒）
        """
        rospy.loginfo(f"开始投放指定位置: {positions}")
        
        for pos in positions:
            if pos == 'front_left':
                self.open_front_left()
            elif pos == 'front_right':
                self.open_front_right()
            elif pos == 'back_left':
                self.open_back_left()
            elif pos == 'back_right':
                self.open_back_right()
            else:
                rospy.logwarn(f"未知位置: {pos}")
                continue
                
            rospy.sleep(delay)
            
        rospy.loginfo("指定位置投放完成")
        
    def reset_and_release(self, release_func):
        """复位后执行投放
        
        Args:
            release_func: 要执行的投放函数
        """
        # 先关闭所有舵机确保复位
        self.close_all_servos()
        rospy.sleep(0.5)
        
        # 执行投放
        release_func()
        
    def run_example(self):
        """运行示例程序"""
        rospy.loginfo("===== 投放装置控制示例开始 =====")
        
        # 示例1：单个舵机控制
        rospy.loginfo("示例1：打开左前舵机")
        self.open_front_left()
        rospy.sleep(2)
        self.close_all_servos()
        rospy.sleep(1)
        
        # 示例2：同时打开多个舵机
        rospy.loginfo("示例2：同时打开前两个舵机")
        self.open_front_left()
        self.open_front_right()
        rospy.sleep(2)
        self.close_all_servos()
        rospy.sleep(1)
        
        # 示例3：顺序投放
        rospy.loginfo("示例3：顺序投放所有物品")
        self.sequential_release(interval=1.0)
        rospy.sleep(1)
        self.close_all_servos()
        rospy.sleep(1)
        
        # 示例4：指定位置投放
        rospy.loginfo("示例4：投放指定位置物品")
        self.release_with_delay(['front_left', 'back_right'], delay=1.0)
        rospy.sleep(1)
        self.close_all_servos()
        
        rospy.loginfo("===== 投放装置控制示例完成 =====")

class MissionBasedCatapult:
    """基于任务的投放控制器（结合无人机任务）"""
    
    def __init__(self):
        self.catapult = CatapultController()
        
    def release_at_waypoint(self, waypoint_num, position):
        """在特定航点时投放物品
        
        Args:
            waypoint_num: 航点编号
            position: 投放位置
        """
        rospy.loginfo(f"到达航点 {waypoint_num}，投放 {position} 位置物品")
        
        if position == 'front_left':
            self.catapult.open_front_left()
        elif position == 'front_right':
            self.catapult.open_front_right()
        elif position == 'back_left':
            self.catapult.open_back_left()
        elif position == 'back_right':
            self.catapult.open_back_right()
            
    def release_all_at_final(self):
        """在最终航点投放所有物品"""
        rospy.loginfo("到达最终航点，投放所有物品")
        self.catapult.open_all_servos()
        
    def reset_before_mission(self):
        """任务开始前复位所有舵机"""
        rospy.loginfo("任务开始前复位所有舵机")
        self.catapult.close_all_servos()

if __name__ == '__main__':
    try:
        # 创建投放装置控制器
        controller = CatapultController()
        
        # 运行示例程序
        controller.run_example()
        
        # 或者使用基于任务的控制器
        # mission_controller = MissionBasedCatapult()
        # mission_controller.reset_before_mission()
        # mission_controller.release_at_waypoint(1, 'front_left')
        # mission_controller.release_at_waypoint(2, 'front_right')
        # mission_controller.release_all_at_final()
        
    except rospy.ROSInterruptException:
        pass

---

3.package.xml:

xml

<build_depend>std_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>

<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>

<exec_depend>std_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>

4.使用说明

先启动投放装置驱动：roslaunch tutorial_catapult catapult_driver.launch

验证舵机控制：

bash

# 查看话题列表
rostopic list
# 手动测试单个舵机（front_left也可以换成别的舵机）
rostopic pub /servo/front_left/open std_msgs/Empty "{}" 
# 手动关闭所有舵机
rostopic pub /servo/all/close std_msgs/Empty "{}" 

四、下视觉摄像头调用

1.调用下视觉摄像头需要先启动命令

bash

# 启动下视觉摄像头驱动
roslaunch tutorial_vision simple_camera_driver.launch

2.相关话题说明以及调用例程

话题名称	消息类型	作用
camera/image_raw	sensor_msgs/Image	原始图像数据：下视摄像头采集的图像数据

---

Python版本

python

#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import Image
import cv2
from cv_bridge import CvBridge

class SimpleCamera:
    def __init__(self):
        rospy.init_node('simple_camera_node')
        self.bridge = CvBridge()
        # 订阅图像话题
        self.image_sub = rospy.Subscriber('camera/image_raw', Image, self.image_callback)
        
    def image_callback(self, msg):
        try:
            # 将ROS图像转换为OpenCV格式
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            
            # 显示图像
            cv2.imshow("Camera View", cv_image)
            cv2.waitKey(1)
            
        except Exception as e:
            rospy.logerr(e)

if __name__ == '__main__':
    try:
        SimpleCamera()
        rospy.spin()
    except rospy.ROSInterruptException:
        cv2.destroyAllWindows()

3.package.xml:

xml

<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>opencv2</build_depend>

4.运行方法

必须先启动启动下视觉摄像头驱动roslaunch tutorial_vision simple_camera_driver.launch

5.验证数据

bash

# 查看话题
rostopic list | grep camera

# 查看图像话题信息
rostopic info /camera/image_raw

# 查看图像数据率
rostopic hz /camera/image_raw