用python实现大疆无人机M600航点规划任务_waypoints.zip

# 用Python实现大疆无人机航点规划任务 [ROS](https://wiki.ros.org/)|[大疆](https://www.dji.com/cn)|[M600 Pro](https://www.dji.com/cn/matrice600-pro?site=brandsite&from=nav)|[开发者](https://developer.dji.com/cn/onboard-sdk/)|**Python**|[SDK](https://developer.dji.com/onboard-sdk/documentation/sample-doc/sample-setup.html#ros-onboard-computer) ## 一、硬、软件环境 - 无人机：大疆M600 Pro无人机 - 飞控单元：A3 Pro - 飞控接口：大疆[Onboard-SDK-ROS](https://github.com/dji-sdk/Onboard-SDK-ROS) - 开发平台：树莓派ubuntu、ubuntu电脑主机 - 代码实现语言：Python 事实上，以下机型都支持 | Aircraft/FC | Firmware Package Version | Flight Controller Version | OSDK Branch | Notes | |-------------------|--------------------------|---------------------------|----------------------------|-----------------------------------------------------------------------| | **M210/M210 RTK** | **1.1.0913** | **3.3.10.4** | **OSDK-ROS 3.7** | | | | | | | | | **M600/M600 Pro** | **1.0.1.66** | **3.2.41.13** | **OSDK-ROS 3.7** | | | | | | | | | **A3/A3 Pro** | **1.7.6.0** | **3.3.8.39** | **OSDK-ROS 3.7** | | | | | | | | | **N3** | **1.7.6.0** | **3.3.8.39** | **OSDK-ROS 3.7** | | | | | | | | | **M100** | 1.3.1.82 | **3.1.10.0** | **OSDK-ROS 3.7** | | ## 二、航点规划任务的流程图  ## 三、流程的基本步骤 1. 按照大疆官网**安装Onboard-SDK-ROS** ，[链接](https://developer.dji.com/onboard-sdk/documentation/sample-doc/sample-setup.html#ros-onboard-computer)； 2. **导入相关的包**，其中`drone_proxy`这个包定义了一系列无人机对象和方法，需要付费才能获取哦 ； ```python #!/usr/bin/python # -*- coding: utf-8 -*- import rospy from drone_proxy import * ``` 3. **激活无人机并获取SDK控制权限** ； ```python drone = Drone() ret = drone.activate() if ret.result: print "无人机激活成功!" ret = drone.obtain_authority() if ret.result: print "SDK获取无人机控制权限成功!" else: if ret.ack_data == 3 and ret.cmd_set == 1 and ret.cmd_id == 0: print "正在获取控制权限，再发一次请求..." drone.obtain_authority() else: print "获取控制权限失败!" ``` 4. **ROS节点初始化** 并订阅当前位置； ```python rospy.init_node('demo_waypoint', anonymous=True) ori_pos = rospy.client.wait_for_message("/dji_sdk/gps_position", NavSatFix, 2) ``` 5. 定义航点属性函数； ```python def waypoint_property(latitude, longitude, altitude, heading=0, hovertime=5): """航点属性赋值函数""" wp = MissionWaypoint() wp.latitude = latitude wp.longitude = longitude wp.altitude = altitude wp.damping_distance = 0 # Bend length (effective coordinated turn mode only) wp.target_yaw = heading # 目标偏航角 wp.target_gimbal_pitch = 0 #TODO wp.turn_mode = 0 # 转弯模式 0顺时针方向 1逆时针方向 # 设置悬停 wp.has_action = 1 # 有没有动作 0无 1有 wp.action_time_limit = 5000 wp.waypoint_action.action_repeat = 1 wp.waypoint_action.command_list = "\0"*16 wp.waypoint_action.command_parameter[0] = hovertime * 1000 return wp ``` 6. 定义航点任务； ```python def waypoint_mission(p): # 航点任务基本描述 bp = MissionWaypointTask() bp.velocity_range = 10 bp.idle_velocity = 5 bp.action_on_finish = 0 # 结束后的动作 0无动作 1返回原点 2自动着陆 3返回某点 4无尽模式，不退出 bp.mission_exec_times = 1 # 仅执行一次 bp.yaw_mode = 0 # 航向（机头方向） 0自动模式（指向下一个航点） 1锁定初始值 2由遥控器控制 3采用航点的偏航角 bp.trace_mode = 0 # 转弯模式为定点转弯 bp.action_on_rc_lost = 1 # RC失联后执行失联保护 #TODO bp.gimbal_pitch_mode = 0 # 添加航点 wps = [] for i in range(len(p)): heading_angle = 0 # 机头朝向 wp = waypoint_property(p[i][0], p[i][1], p[i][2], heading=heading_angle) wps.append(wp) bp.mission_waypoint = wps return bp ``` 7. 装订航线并启动任务； ```python def load_and_start(mission): # 装订航点 ret = drone.wpMission.upload(mission) if ret.result: print "航点装订成功!" ret = drone.wpMission.start() if ret.result: print "航点任务启动成功!" # 开始ROS调度 try: rospy.spin() except KeyboardInterrupt, e: pass ``` 8. 定义主函数，在主函数中定义航点，并调用函数。 ```python def main(): if isinstance(ori_pos, NavSatFix): origin_p = (ori_pos.latitude, ori_pos.longitude, ori_pos.altitude) points = [] next_p = (origin_p[0], origin_p[1], origin_p[2]+12) points.append(next_p) next_p = (origin_p[0]+0.00030, origin_p[1], origin_p[2]+12) points.append(next_p) next_p = (origin_p[0]+0.00030, origin_p[1]+0.00020, origin_p[2]+12) points.append(next_p) next_p = (origin_p[0], origin_p[1]+0.00020, origin_p[2]+12) points.append(next_p) print "总共有{}个点，各点的坐标为\n{}".format(len(points), points) task = waypoint_mission(points) load_and_start(task) if __name__ == "__main__": main() ``` **提示：** - 1、以上航点实现的是长方形飞行，如果想实现来回往返的多航点飞行，就需要规划好航点，传给 `waypoint_mission()` 函数； - 2、由于室内GPS信号弱，无人机并不会启动飞行。 ## 四、`drone_proxy`包的获取方式 扫二维码，支付宝付款成功后，加微信好友，备注**drone_proxy**  ---