大疆官方提供的软件包的主要有3个部分 1. 核心的API部分dji-sdk-lib,用于串口通信,建立各种任务的线程,读取信息的线程。 2. 用于封装核心API为ROS接口的dji_sdk 3. 简单的demo,给出了dji_sdk的使用方法。
由于涉及到的内容比较多,先看一下官方提供的关系图

该节点为dji_sdk包的核心,成员变量包括无人机的状态信息,服务信息,控制信息,话题发布和订阅信息。
1)状态信息
参数列表,包括串口名称,波特率,开发者ID及密码,直接在sdk.launch中进行修改即可。
nh_private.param("serial_name", serial_device, std::string("/dev/ttyUSB0")); nh_private.param("baud_rate", baud_rate, 921600); nh_private.param("app_id", app_id, 123456); nh_private.param("app_version", app_version, 1); nh_private.param("enc_key", enc_key, std::string("abcd1234")); nh_private.param("drone_version", drone_version, std::string("M100")); // choose M100 as default nh_private.param("grity_const", grity_const, 9.801); nh_private.param("align_time", align_time_with_FC, true); nh_private.param("use_broadcast", user_select_BC, false);三个经常要使用的位置参数,经度,纬度,海拔(海拔和姿态极易混淆,注意区分)
local_pos_ref_latitude = local_pos_ref_longitude = local_pos_ref_altitude = 0;2)服务信息
drone_activation_server = nh.advertiseService("dji_sdk/activation", &DJISDKNode::droneActivationCallback, this); drone_arm_server = nh.advertiseService("dji_sdk/drone_arm_control", &DJISDKNode::droneArmCallback, this); drone_task_server = nh.advertiseService("dji_sdk/drone_task_control", &DJISDKNode::droneTaskCallback, this); sdk_ctrlAuthority_server = nh.advertiseService("dji_sdk/sdk_control_authority", &DJISDKNode::sdkCtrlAuthorityCallback, this); camera_action_server = nh.advertiseService("dji_sdk/camera_action", &DJISDKNode::cameraActionCallback, this);对应的回调函数可在dji_sdk_node_services.cpp中找到。
3)控制信息
flight_control_sub = nh.subscribe( "dji_sdk/flight_control_setpoint_generic", 10, &DJISDKNode::flightControlSetpointCallback, this);对应的回调函数可在dji_sdk_node_control中找到。
void DJISDKNode::flightControlSetpointCallback( const sensor_msgs::Joy::ConstPtr& pMsg) { float xSP = pMsg->axes[0]; float ySP = pMsg->axes[1]; float zSP = pMsg->axes[2]; float yawSP = pMsg->axes[3]; uint8_t flag = (uint8_t)(pMsg->axes[4]); flightControl(flag, xSP, ySP, zSP, yawSP); }如该函数通过订阅JOY消息来将无人机移动到指定位置,其中flag用来表示控制模式,即FAP。
4)话题发布和订阅信息
height_publisher = nh.advertise("dji_sdk/height_above_takeoff", 10); velocity_publisher = nh.advertise("dji_sdk/velocity", 10);这里发布的许多话题都是我们比较感兴趣的,如attitude(四元组),imu,gps_position(经纬度,海拔),height_above_takeoff,velocity等等。
2.dji_sdk_demo以demo_flight_control为例
首先是初始化
// Subscribe to messages from dji_sdk_node ros::Subscriber attitudeSub = nh.subscribe("dji_sdk/attitude", 10, &attitude_callback); //话题,队列长度,回调函数 ros::Subscriber gpsSub = nh.subscribe("dji_sdk/gps_position", 10, &gps_callback); ros::Subscriber flightStatusSub = nh.subscribe("dji_sdk/flight_status", 10, &flight_status_callback); ros::Subscriber displayModeSub = nh.subscribe("dji_sdk/display_mode", 10, &display_mode_callback);从dji_sdk_node处订阅消息并赋给已经定义好的全局变量。
// Publish the control signal ctrlPosYawPub = nh.advertise("/dji_sdk/flight_control_setpoint_ENUposition_yaw", 10); ctrlBrakePub = nh.advertise("dji_sdk/flight_control_setpoint_generic", 10); // Basic services sdk_ctrl_authority_service = nh.serviceClient ("dji_sdk/sdk_control_authority"); drone_task_service = nh.serviceClient("dji_sdk/drone_task_control"); query_version_service = nh.serviceClient("dji_sdk/query_drone_version");之后发布控制指令及启动基本服务。
if(takeoff_result) { square_mission.reset(); square_mission.start_gps_location = current_gps; square_mission.setTarget(0, 20, 3, 60); square_mission.state = 1; ROS_INFO("##### Start route %d ....", square_mission.state); }最后是具体任务规划的函数,事实上这里只是定义了初始状态,具体过程是通过不断调用回调函数gps_callback来完成的
void gps_callback(const sensor_msgs::NSatFix::ConstPtr& msg) { static ros::Time start_time = ros::Time::now(); ros::Duration elapsed_time = ros::Time::now() - start_time; current_gps = *msg; // Down sampled to 50Hz loop if(elapsed_time > ros::Duration(0.02)) { start_time = ros::Time::now(); switch(square_mission.state) { case 0: break; case 1: if(!square_mission.finished) { square_mission.step(); } else { square_mission.reset(); square_mission.start_gps_location = current_gps; square_mission.setTarget(20, 0, 0, 0); square_mission.state = 2; ROS_INFO("##### Start route %d ....", square_mission.state); } break; case 2: if(!square_mission.finished) { square_mission.step(); } else { square_mission.reset(); square_mission.start_gps_location = current_gps; square_mission.setTarget(0, -20, 0, 0); square_mission.state = 3; ROS_INFO("##### Start route %d ....", square_mission.state); } break; case 3: if(!square_mission.finished) { square_mission.step(); } else { square_mission.reset(); square_mission.start_gps_location = current_gps; square_mission.setTarget(-20, 0, 0, 0); square_mission.state = 4; ROS_INFO("##### Start route %d ....", square_mission.state); } break; case 4: if(!square_mission.finished) { square_mission.step(); } else { ROS_INFO("##### Mission %d Finished ....", square_mission.state); square_mission.state = 0; } break; } } }通过state来分段控制,通过finished来结束过程。
其他几个demo也是类似的。