| Note: This tutorial assumes that you have completed the previous tutorials: Getting Started. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Running the flight control sample code
Description: This tutorial helps you to set up and run the flight control sample code, and explain the core functions of the sample code.Tutorial Level: INTERMEDIATE
Contents
Preparation
Before running the example code, please follow the Getting Started tutorial and have the A3/N3 hardware, dji_sdk node and the simulation up and running.
The Flight Control Sample
This sample code controls the vehicle to take off, draw an L shape (half of a square) and lands. More specifically, the programs monitors the take off of the vehicle, starting from ( x = 0, y = 0 , z = 0 , yaw = 0 ), and control it to fly to ( x = 0, y = 6 , z = 3 , yaw = 60 ) and ( x = 6, y = 0 , z = 0 , yaw = 60 ) position w.r.t. the ground frame.
Running the Code
If do not have the sample code along with the dji_sdk ROS package, download the dji_sdk_demo and place the folder within your catkin_ws/src folder. Then, in your catkin workspace root directory, run catkin_make and compile.
Assume you have followed the Getting Started tutorial and have the dji_sdk node running in one of the terminals using the launch file, open another terminal, cd to your catkin workspace, then type and enter:
$ rosrun dji_sdk_demo demo_flight_control
Note: If terminal returns Error: package 'dji_sdk_demo' not found, 'catkin_make' the workspace and 'source devel/setup.bash' the terminal. |
You can setup the DJI Simulator by right clicking the simulated drone and click setup. Enable trace will show the flight trajectory of the simulated drone. By running this example code, you will see the drone flying a L pattern.
Code Walk Through
In this section, we will explain the work flow of this sample code.
The main function
1 //
2 // Created by zyli on 5/11/17.
3 //
4
5 #include "dji_sdk_demo/demo_flight_control.h"
6 #include "dji_sdk/dji_sdk.h"
7
8 const float deg2rad = C_PI/180.0;
9 const float rad2deg = 180.0/C_PI;
10
11 ros::ServiceClient sdk_ctrl_authority_service;
12 ros::ServiceClient drone_task_service;
13
14 ros::Publisher ctrlPosYawPub;
15 ros::Publisher ctrlVelYawRatePub;
16
17 // global variables for subscribed topics
18 uint8_t flight_status = 255;
19 uint8_t display_mode = 255;
20 sensor_msgs::NavSatFix current_gps;
21 geometry_msgs::Quaternion current_atti;
22
23 Mission square_mission;
24
25 int main(int argc, char** argv)
26 {
27 ros::init(argc, argv, "demo_flight_control_node");
28 ros::NodeHandle nh;
29
30 // Subscribe to messages from dji_sdk_node
31 ros::Subscriber attitudeSub = nh.subscribe("dji_sdk/attitude", 10, &attitude_callback);
32 ros::Subscriber gpsSub = nh.subscribe("dji_sdk/gps_position", 10, &gps_callback);
33 ros::Subscriber flightStatusSub = nh.subscribe("dji_sdk/flight_status", 10, &flight_status_callback);
34 ros::Subscriber displayModeSub = nh.subscribe("dji_sdk/display_mode", 10, &display_mode_callback);
35
36 // Publish the control signal
37 ctrlPosYawPub = nh.advertise<sensor_msgs::Joy>("/dji_sdk/flight_control_setpoint_ENUposition_yaw", 10);
38 ctrlVelYawRatePub = nh.advertise<sensor_msgs::Joy>("dji_sdk/flight_control_setpoint_generic", 10);
39 // Basic services
40 sdk_ctrl_authority_service = nh.serviceClient<dji_sdk::SDKControlAuthority> ("dji_sdk/sdk_control_authority");
41 drone_task_service = nh.serviceClient<dji_sdk::DroneTaskControl>("dji_sdk/drone_task_control");
42
43 bool obtain_control_result = obtain_control();
44 bool takeoff_result = monitoredTakeoff();
45
46 if(takeoff_result)
47 {
48 square_mission.reset();
49 square_mission.start_gps_location = current_gps;
50 square_mission.setTarget(0, 6, 3, 60);
51 square_mission.state = 1;
52 ROS_INFO("##### Start route %d ....", square_mission.state);
53 }
54
55 ros::spin();
56 return 0;
57 }
Open 'dji_sdk_demo/src/demo_flight_control.cpp', the main function is as above. Let's break the code down.
7 const float deg2rad = C_PI/180.0;
8 const float rad2deg = 180.0/C_PI;
9
10 ros::ServiceClient sdk_ctrl_authority_service;
11 ros::ServiceClient drone_task_service;
12
13 ros::Publisher ctrlPosYawPub;
14 ros::Publisher ctrlVelYawRatePub;
15
16 // global variables for subscribed topics
17 uint8_t flight_status = 255;
18 uint8_t display_mode = 255;
19 sensor_msgs::NavSatFix current_gps;
20 geometry_msgs::Quaternion current_atti;
Above are the declaration of the global variables.
Initialize the node with the node name "demo_flight_control_node"; create a node handle.
30 // Subscribe to messages from dji_sdk_node
31 ros::Subscriber attitudeSub = nh.subscribe("dji_sdk/attitude", 10, &attitude_callback);
32 ros::Subscriber gpsSub = nh.subscribe("dji_sdk/gps_position", 10, &gps_callback);
33 ros::Subscriber flightStatusSub = nh.subscribe("dji_sdk/flight_status", 10, &flight_status_callback);
34 ros::Subscriber displayModeSub = nh.subscribe("dji_sdk/display_mode", 10, &display_mode_callback);
Subscribe to the topics with needed information. The received information will be saved inside the global variables for other functions to use. The subscribed topics gives us the information to close feedback loops. Assign a callback function to each subscribed topic. The buffer size of the topic is set to 10, but if you only need the latest info, it could be set to 1. For more information on flight status and display mode, see linked pages.
Initialize the publishers for the vehicle control. The ctrlPosYawPub publishes commands for position-controlling the x, y position and the yaw angle of the vehicle with respect to (w.r.t.) the ground truth coordinate. The 'ctrlVelYawRatePub publishes commands for velocity-controlling the x, y` velocity and the rate of change of the yaw angle of the vehicle w.r.t. the ground truth coordinate. This is achieved using the generic control and it flag variable.
Initialize the services we need. The sdk_ctrl_authority_service will be used to request control authority of the vehicle, so we can have obtain the vehicle's control. The drone_task_service is used later to request takeoff and landing of the vehicle.
43 bool obtain_control_result = obtain_control();
Call obtain_control() to obtain control of the vehicle, utilizing the sdk_ctrl_authority_service service.
44 bool takeoff_result = monitoredTakeoff();
Calls monitoredTakeoff() to request the vehicle to takeoff, this function also utilizes flight status and display mode to check if the vehicle is indeed hovering in air.
The Mission class of the 'square_mission' is a state machine defined in 'demo_flight_control.h' that can be found in the include folder. If we have successfully taken off, reset all the counters within the class variable with 'square_mission.reset()'. Then it takes the current_gps data as a reference start point using 'square_mission.start_gps_location = current_gps'. Set the next target global position with 'square_mission.setTarget(0, 6, 3, 60)'. The Mission class variable is initialized with state equal to 0 (state 0 does nothing), so we start the state machine by setting it to 1 with 'square_mission.state = 1'.
55 ros::spin();
Start the ros spin();
The GPS callback
This is the function that is used to continuously run and update the state machine. We utilized the fact that the GPS callback function will be continuously called as long as the GPS signal is being published by the dji_sdk node.
252 void gps_callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
253 {
254 static ros::Time start_time = ros::Time::now();
255 ros::Duration elapsed_time = ros::Time::now() - start_time;
256 current_gps = *msg;
257
258 // Down sampled to 50Hz loop
259 if(elapsed_time > ros::Duration(0.02))
260 {
261 //ROS_INFO("Elapsed time: %f; GPS: lat=%.3f, long=%.3f", elapsed_time.toSec(), msg->latitude, msg->longitude);
262 start_time = ros::Time::now();
263 switch(square_mission.state)
264 {
265 case 0:
266 break;
267
268 case 1:
269 if(!square_mission.finished)
270 {
271 square_mission.step();
272 }
273 else
274 {
275 square_mission.reset();
276 square_mission.start_gps_location = current_gps;
277 square_mission.setTarget(6, 0, 0, 0);
278 square_mission.state = 2;
279 ROS_INFO("##### Start route %d ....", square_mission.state);
280 }
281 break;
282
283 case 2:
284 if(!square_mission.finished)
285 {
286 square_mission.step();
287 }
288 else
289 {
290 ROS_INFO("##### Mission %d Finished ....", square_mission.state);
291 square_mission.state = 0;
292 }
293 break;
294 }
295 }
296 }
This callback function serves two purposes: 1. Update the 'current_gps' position. 2. update the state machine.
256 current_gps = *msg;
Update the global 'current_gps' variable every time the callback function is being triggered.
Calculate the 'elapse_time' since the last update on the 'start_time'.
Force the code inside the 'if' statement to run at most 50 Hz.
262 start_time = ros::Time::now();
Once inside the if statement, update the 'start_time' to current time.
263 switch(square_mission.state)
264 {
265 case 0:
266 break;
267
268 case 1:
269 if(!square_mission.finished)
270 {
271 square_mission.step();
272 }
273 else
274 {
275 square_mission.reset();
276 square_mission.start_gps_location = current_gps;
277 square_mission.setTarget(6, 0, 0, 0);
278 square_mission.state = 2;
279 ROS_INFO("##### Start route %d ....", square_mission.state);
280 }
281 break;
282
283 case 2:
284 if(!square_mission.finished)
285 {
286 square_mission.step();
287 }
288 else
289 {
290 ROS_INFO("##### Mission %d Finished ....", square_mission.state);
291 square_mission.state = 0;
292 }
293 break;
294 }
This is the state machine that dictate the vehicle to go to each of the desinated target positions.
The state 0 is an idle state.
268 case 1:
269 if(!square_mission.finished)
270 {
271 square_mission.step();
272 }
273 else
274 {
275 square_mission.reset();
276 square_mission.start_gps_location = current_gps;
277 square_mission.setTarget(6, 0, 0, 0);
278 square_mission.state = 2;
279 ROS_INFO("##### Start route %d ....", square_mission.state);
280 }
281 break;
We assigned the 'square_mission.state' to '1', we also set our target to (0,6,3,60). Therefore, while the current target is not reached, which means 'square_mission.finished==0', then continue with stepping toward the target using square_mission.step(). If we reached the target position in 'state 1', the set the next target, update the state.
check if the target has been reached, if not, step towart the target. Rinse and repeat.
Your Turn
Now try yourself and modify the code so the vihicle can draw a square!
