00001
00013 #include <dji_sdk/dji_sdk.h>
00014 #include <ros/ros.h>
00015 #include <nav_msgs/Odometry.h>
00016 #include <std_msgs/UInt8.h>
00017 #include <actionlib/client/simple_action_client.h>
00018 #include <actionlib/client/terminal_state.h>
00019 #include <string>
00020
00021
00022 class DJIDrone
00023 {
00024 private:
00025
00026 typedef actionlib::SimpleActionClient<dji_sdk::DroneTaskAction> DroneTaskActionClient;
00027 typedef actionlib::SimpleActionClient<dji_sdk::LocalPositionNavigationAction> LocalPositionNavigationActionClient;
00028 typedef actionlib::SimpleActionClient<dji_sdk::GlobalPositionNavigationAction> GlobalPositionNavigationActionClient;
00029 typedef actionlib::SimpleActionClient<dji_sdk::WaypointNavigationAction> WaypointNavigationActionClient;
00030
00031 DroneTaskActionClient drone_task_action_client;
00032 LocalPositionNavigationActionClient local_position_navigation_action_client;
00033 GlobalPositionNavigationActionClient global_position_navigation_action_client;
00034 WaypointNavigationActionClient waypoint_navigation_action_client;
00035
00036 ros::ServiceClient activation_service;
00037 ros::ServiceClient attitude_control_service;
00038 ros::ServiceClient camera_action_control_service;
00039 ros::ServiceClient drone_task_control_service;
00040 ros::ServiceClient gimbal_angle_control_service;
00041 ros::ServiceClient gimbal_speed_control_service;
00042 ros::ServiceClient global_position_control_service;
00043 ros::ServiceClient local_position_control_service;
00044 ros::ServiceClient sdk_permission_control_service;
00045 ros::ServiceClient velocity_control_service;
00046 ros::ServiceClient version_check_service;
00047
00048 ros::ServiceClient virtual_rc_enable_control_service;
00049 ros::ServiceClient virtual_rc_data_control_service;
00050 ros::ServiceClient drone_arm_control_service;
00051 ros::ServiceClient sync_flag_control_service;
00052 ros::ServiceClient message_frequency_control_service;
00053 ros::ServiceClient mission_start_service;
00054 ros::ServiceClient mission_pause_service;
00055 ros::ServiceClient mission_cancel_service;
00056 ros::ServiceClient mission_wp_upload_service;
00057 ros::ServiceClient mission_wp_download_service;
00058 ros::ServiceClient mission_wp_set_speed_service;
00059 ros::ServiceClient mission_wp_get_speed_service;
00060 ros::ServiceClient mission_hp_upload_service;
00061 ros::ServiceClient mission_hp_download_service;
00062 ros::ServiceClient mission_hp_set_speed_service;
00063 ros::ServiceClient mission_hp_set_radius_service;
00064 ros::ServiceClient mission_hp_reset_yaw_service;
00065 ros::ServiceClient mission_fm_upload_service;
00066 ros::ServiceClient mission_fm_set_target_service;
00067
00068
00069 ros::Subscriber acceleration_subscriber;
00070 ros::Subscriber attitude_quaternion_subscriber;
00071 ros::Subscriber compass_subscriber;
00072 ros::Subscriber flight_control_info_subscriber;
00073 ros::Subscriber flight_status_subscriber;
00074 ros::Subscriber gimbal_subscriber;
00075 ros::Subscriber global_position_subscriber;
00076 ros::Subscriber local_position_subscriber;
00077 ros::Subscriber power_status_subscriber;
00078 ros::Subscriber rc_channels_subscriber;
00079 ros::Subscriber velocity_subscriber;
00080 ros::Subscriber activation_subscriber;
00081 ros::Subscriber odometry_subscriber;
00082
00083 ros::Subscriber time_stamp_subscriber;
00084 ros::Subscriber mission_status_subscriber;
00085 ros::Subscriber mission_event_subscriber;
00086 ros::Subscriber mobile_data_subscriber;
00087
00088 public:
00089
00090 dji_sdk::Acceleration acceleration;
00091 dji_sdk::AttitudeQuaternion attitude_quaternion;
00092 dji_sdk::Compass compass;
00093 dji_sdk::FlightControlInfo flight_control_info;
00094 uint8_t flight_status;
00095 uint8_t mobile_new_data;
00096 dji_sdk::Gimbal gimbal;
00097 dji_sdk::GlobalPosition global_position;
00098 dji_sdk::GlobalPosition global_position_ref;
00099 dji_sdk::LocalPosition local_position;
00100 dji_sdk::LocalPosition local_position_ref;
00101 dji_sdk::PowerStatus power_status;
00102 dji_sdk::TransparentTransmissionData mobile_data;
00103 dji_sdk::RCChannels rc_channels;
00104 dji_sdk::Velocity velocity;
00105 nav_msgs::Odometry odometry;
00106 dji_sdk::TimeStamp time_stamp;
00107 bool activation = false;
00108 bool localposbase_use_height = true;
00109
00110 uint8_t mission_type;
00111
00112 uint8_t incident_type;
00113
00114 dji_sdk::MissionStatusWaypoint waypoint_mission_push_info;
00115 dji_sdk::MissionStatusHotpoint hotpoint_mission_push_info;
00116 dji_sdk::MissionStatusFollowme followme_mission_push_info;
00117 dji_sdk::MissionStatusOther other_mission_push_info;
00118
00119 dji_sdk::MissionEventWpUpload waypoint_upload_result;
00120 dji_sdk::MissionEventWpAction waypoint_action_result;
00121 dji_sdk::MissionEventWpReach waypoint_reached_result;
00122
00123 typedef void *UserData;
00124 typedef void (*CallBack)(DJIDrone *);
00125
00126 typedef struct CallBackHandler
00127 {
00128 CallBack callback;
00129 UserData userData;
00130 } CallBackHandler;
00131
00132 private:
00133 void acceleration_subscriber_callback(dji_sdk::Acceleration acceleration)
00134 {
00135 this->acceleration = acceleration;
00136 }
00137
00138 void attitude_quaternion_subscriber_callback(dji_sdk::AttitudeQuaternion attitude_quaternion)
00139 {
00140 this->attitude_quaternion = attitude_quaternion;
00141 }
00142
00143 void compass_subscriber_callback(dji_sdk::Compass compass)
00144 {
00145 this->compass = compass;
00146 }
00147
00148 void flight_control_info_subscriber_callback(dji_sdk::FlightControlInfo flight_control_info)
00149 {
00150 this->flight_control_info = flight_control_info;
00151 }
00152
00153 void flight_status_subscriber_callback(std_msgs::UInt8 flight_status)
00154 {
00155 this->flight_status = flight_status.data;
00156 }
00157
00158 void gimbal_subscriber_callback(dji_sdk::Gimbal gimbal)
00159 {
00160 this->gimbal = gimbal;
00161 }
00162
00163 void global_position_subscriber_callback(dji_sdk::GlobalPosition global_position)
00164 {
00165 this->global_position = global_position;
00166 }
00167
00168 void local_position_subscriber_callback(dji_sdk::LocalPosition local_position)
00169 {
00170 this->local_position = local_position;
00171 }
00172
00173 void power_status_subscriber_callback(dji_sdk::PowerStatus power_status)
00174 {
00175 this->power_status = power_status;
00176 }
00177
00178 void rc_channels_subscriber_callback(dji_sdk::RCChannels rc_channels)
00179 {
00180 this->rc_channels = rc_channels;
00181 }
00182
00183 void velocity_subscriber_callback(dji_sdk::Velocity velocity)
00184 {
00185 this->velocity = velocity;
00186 }
00187
00188 void activation_subscriber_callback(std_msgs::UInt8 activation)
00189 {
00190 this->activation = activation.data;
00191 }
00192
00193 void odometry_subscriber_callback(nav_msgs::Odometry odometry)
00194 {
00195 this->odometry = odometry;
00196 }
00197
00198 void time_stamp_subscriber_callback(dji_sdk::TimeStamp time_stamp)
00199 {
00200 this->time_stamp = time_stamp;
00201 }
00202
00203 void mission_status_push_info_callback(dji_sdk::MissionPushInfo status_push_info)
00204 {
00205 this->mission_type = status_push_info.type;
00206 switch(status_push_info.type)
00207 {
00208 case 1:
00209 this->waypoint_mission_push_info.mission_type = status_push_info.type;
00210 this->waypoint_mission_push_info.target_waypoint = status_push_info.data_1;
00211 this->waypoint_mission_push_info.current_status = status_push_info.data_2;
00212 this->waypoint_mission_push_info.error_code = status_push_info.data_3;
00213 break;
00214
00215 case 2:
00216 this->hotpoint_mission_push_info.mission_type = status_push_info.type;
00217 this->hotpoint_mission_push_info.mission_status= status_push_info.data_1;
00218 this->hotpoint_mission_push_info.hotpoint_radius = status_push_info.data_2 << 8 | status_push_info.data_3;
00219 this->hotpoint_mission_push_info.error_code = status_push_info.data_4;
00220 this->hotpoint_mission_push_info.hotpoint_velocity = status_push_info.data_5;
00221 break;
00222
00223 case 3:
00224 this->followme_mission_push_info.mission_type = status_push_info.type;
00225 break;
00226
00227 case 0:
00228 case 4:
00229 this->other_mission_push_info.mission_type = status_push_info.type;
00230 this->other_mission_push_info.last_mission_type = status_push_info.data_1;
00231 this->other_mission_push_info.is_broken = status_push_info.data_2;
00232 this->other_mission_push_info.error_code = status_push_info.data_3;
00233 break;
00234 default:
00235 break;
00236 }
00237
00238 }
00239
00241 CallBackHandler obtainControlCallback;
00242 CallBackHandler releaseControlCallback;
00243 CallBackHandler takeOffCallback;
00244 CallBackHandler landingCallback;
00245 CallBackHandler getSDKVersionCallback;
00246 CallBackHandler armCallback;
00247 CallBackHandler disArmCallback;
00248 CallBackHandler goHomeCallback;
00249 CallBackHandler takePhotoCallback;
00250 CallBackHandler startVideoCallback;
00251 CallBackHandler stopVideoCallback;
00252 CallBackHandler drawCircleDemoCallback;
00253 CallBackHandler drawSquareDemoCallback;
00254 CallBackHandler attitudeControlDemoCallback;
00255 CallBackHandler waypointNavigationTestCallback;
00256 CallBackHandler localNavigationTestCallback;
00257 CallBackHandler globalNavigationTestCallback;
00258 CallBackHandler virtualRCTestCallback;
00259 CallBackHandler gimbalControlDemoCallback;
00260 CallBackHandler startMapLASLoggingCallback;
00261 CallBackHandler stopMapLASLoggingCallback;
00262 CallBackHandler startCollisionAvoidanceCallback;
00263 CallBackHandler stopCollisionAvoidanceCallback;
00264
00265
00266 void mobile_data_push_info_callback(dji_sdk::TransparentTransmissionData information)
00267 {
00268 this->mobile_data = information;
00269 mobile_new_data = 1;
00270 int cmdID = mobile_data.data[0];
00271 printf("Command ID code is %d \n", cmdID);
00272
00273 switch(cmdID)
00274 {
00275 case 2:
00276 if (obtainControlCallback.callback)
00277 {
00278 obtainControlCallback.callback(this);
00279 }
00280 break;
00281
00282 case 3:
00283 if (releaseControlCallback.callback)
00284 {
00285 releaseControlCallback.callback(this);
00286 }
00287 break;
00288
00289 case 4:
00290
00291
00292
00293
00294 break;
00295
00296 case 5:
00297 if (armCallback.callback)
00298 {
00299 armCallback.callback(this);
00300 }
00301 break;
00302
00303 case 6:
00304 if (disArmCallback.callback)
00305 {
00306 disArmCallback.callback(this);
00307 }
00308 break;
00309
00310 case 7:
00311 if (takeOffCallback.callback)
00312 {
00313 takeOffCallback.callback(this);
00314 }
00315 break;
00316
00317 case 8:
00318 if (landingCallback.callback)
00319 {
00320 landingCallback.callback(this);
00321 }
00322 break;
00323
00324 case 9:
00325 if (goHomeCallback.callback)
00326 {
00327 goHomeCallback.callback(this);
00328 }
00329 break;
00330
00331 case 10:
00332 if (takePhotoCallback.callback)
00333 {
00334 takePhotoCallback.callback(this);
00335 }
00336 break;
00337
00338 case 11:
00339 if (startVideoCallback.callback)
00340 {
00341 startVideoCallback.callback(this);
00342 }
00343 break;
00344
00345 case 13:
00346 if (stopVideoCallback.callback)
00347 {
00348 stopVideoCallback.callback(this);
00349 }
00350 break;
00351
00352 case 20:
00353 if (startMapLASLoggingCallback.callback)
00354 {
00355 startMapLASLoggingCallback.callback(this);
00356 }
00357
00358 break;
00359
00360 case 21:
00361 if (stopMapLASLoggingCallback.callback)
00362 {
00363 stopMapLASLoggingCallback.callback(this);
00364 }
00365 break;
00366
00367 case 22:
00368 if (startCollisionAvoidanceCallback.callback)
00369 {
00370 startCollisionAvoidanceCallback.callback(this);
00371 }
00372
00373 break;
00374
00375 case 23:
00376 if (stopCollisionAvoidanceCallback.callback)
00377 {
00378 stopCollisionAvoidanceCallback.callback(this);
00379 }
00380 break;
00381
00382
00383 case 61:
00384 if (drawCircleDemoCallback.callback)
00385 {
00386 drawCircleDemoCallback.callback(this);
00387 }
00388 break;
00389
00390
00391 case 62:
00392 if (drawSquareDemoCallback.callback)
00393 {
00394 drawSquareDemoCallback.callback(this);
00395 }
00396 break;
00397
00398 case 63:
00399 if (attitudeControlDemoCallback.callback)
00400 {
00401 attitudeControlDemoCallback.callback(this);
00402 }
00403 break;
00404
00405 case 64:
00406 if (gimbalControlDemoCallback.callback)
00407 {
00408 gimbalControlDemoCallback.callback(this);
00409 }
00410 break;
00411
00412 case 65:
00413 if (waypointNavigationTestCallback.callback)
00414 {
00415 waypointNavigationTestCallback.callback(this);
00416 }
00417 break;
00418
00419 case 66:
00420 if (localNavigationTestCallback.callback)
00421 {
00422 localNavigationTestCallback.callback(this);
00423 }
00424 break;
00425
00426 case 67:
00427 if (globalNavigationTestCallback.callback)
00428 {
00429 globalNavigationTestCallback.callback(this);
00430 }
00431 break;
00432
00433 case 68:
00434 if (virtualRCTestCallback.callback)
00435 {
00436 virtualRCTestCallback.callback(this);
00437 }
00438 break;
00439
00440
00441 }
00442 }
00443
00444 void mission_event_push_info_callback(dji_sdk::MissionPushInfo event_push_info)
00445 {
00446 this->incident_type = event_push_info.type;
00447 switch(event_push_info.type)
00448 {
00449 case 0:
00450 this->waypoint_upload_result.incident_type = event_push_info.type;
00451 this->waypoint_upload_result.mission_valid = event_push_info.data_1;
00452 this->waypoint_upload_result.estimated_runtime = event_push_info.data_2 << 8 | event_push_info.data_3;
00453 break;
00454 case 1:
00455 this->waypoint_action_result.incident_type = event_push_info.type;
00456 this->waypoint_action_result.repeat = event_push_info.data_1;
00457 break;
00458 case 2:
00459 this->waypoint_reached_result.incident_type = event_push_info.type;
00460 this->waypoint_reached_result.waypoint_index = event_push_info.data_1;
00461 this->waypoint_reached_result.current_status = event_push_info.data_2;
00462 break;
00463 default:
00464 break;
00465 }
00466 }
00467
00468 public:
00469 DJIDrone(ros::NodeHandle& nh):
00470 drone_task_action_client(nh, "dji_sdk/drone_task_action", true),
00471 local_position_navigation_action_client(nh, "dji_sdk/local_position_navigation_action", true),
00472 global_position_navigation_action_client(nh, "dji_sdk/global_position_navigation_action", true),
00473 waypoint_navigation_action_client(nh, "dji_sdk/waypoint_navigation_action", true)
00474 {
00475 activation_service = nh.serviceClient<dji_sdk::Activation>("dji_sdk/activation");
00476 attitude_control_service = nh.serviceClient<dji_sdk::AttitudeControl>("dji_sdk/attitude_control");
00477 camera_action_control_service = nh.serviceClient<dji_sdk::CameraActionControl>("dji_sdk/camera_action_control");
00478 drone_task_control_service = nh.serviceClient<dji_sdk::DroneTaskControl>("dji_sdk/drone_task_control");
00479 gimbal_angle_control_service = nh.serviceClient<dji_sdk::GimbalAngleControl>("dji_sdk/gimbal_angle_control");
00480 gimbal_speed_control_service = nh.serviceClient<dji_sdk::GimbalSpeedControl>("dji_sdk/gimbal_speed_control");
00481 global_position_control_service = nh.serviceClient<dji_sdk::GlobalPositionControl>("dji_sdk/global_position_control");
00482 local_position_control_service = nh.serviceClient<dji_sdk::LocalPositionControl>("dji_sdk/local_position_control");
00483 sdk_permission_control_service = nh.serviceClient<dji_sdk::SDKPermissionControl>("dji_sdk/sdk_permission_control");
00484 velocity_control_service = nh.serviceClient<dji_sdk::VelocityControl>("dji_sdk/velocity_control");
00485 version_check_service = nh.serviceClient<dji_sdk::VersionCheck>("dji_sdk/version_check");
00486 virtual_rc_enable_control_service = nh.serviceClient<dji_sdk::VirtualRCEnableControl>("dji_sdk/virtual_rc_enable_control");
00487 virtual_rc_data_control_service = nh.serviceClient<dji_sdk::VirtualRCDataControl>("dji_sdk/virtual_rc_data_control");
00488 drone_arm_control_service = nh.serviceClient<dji_sdk::DroneArmControl>("dji_sdk/drone_arm_control");
00489 sync_flag_control_service = nh.serviceClient<dji_sdk::SyncFlagControl>("dji_sdk/sync_flag_control");
00490 message_frequency_control_service = nh.serviceClient<dji_sdk::MessageFrequencyControl>("dji_sdk/message_frequency_control");
00491
00492
00493 mission_start_service = nh.serviceClient<dji_sdk::MissionStart>("dji_sdk/mission_start");
00494 mission_pause_service = nh.serviceClient<dji_sdk::MissionPause>("dji_sdk/mission_pause");
00495 mission_cancel_service = nh.serviceClient<dji_sdk::MissionCancel>("dji_sdk/mission_cancel");
00496 mission_wp_upload_service = nh.serviceClient<dji_sdk::MissionWpUpload>("dji_sdk/mission_waypoint_upload");
00497 mission_wp_download_service = nh.serviceClient<dji_sdk::MissionWpDownload>("dji_sdk/mission_waypoint_download");
00498 mission_wp_set_speed_service = nh.serviceClient<dji_sdk::MissionWpSetSpeed>("dji_sdk/mission_waypoint_set_speed");
00499 mission_wp_get_speed_service = nh.serviceClient<dji_sdk::MissionWpGetSpeed>("dji_sdk/mission_waypoint_get_speed");
00500 mission_hp_upload_service = nh.serviceClient<dji_sdk::MissionHpUpload>("dji_sdk/mission_hotpoint_upload");
00501 mission_hp_download_service = nh.serviceClient<dji_sdk::MissionHpDownload>("dji_sdk/mission_hotpoint_download");
00502 mission_hp_set_speed_service = nh.serviceClient<dji_sdk::MissionHpSetSpeed>("dji_sdk/mission_hotpoint_set_speed");
00503 mission_hp_set_radius_service = nh.serviceClient<dji_sdk::MissionHpSetRadius>("dji_sdk/mission_hotpoint_set_radius");
00504 mission_hp_reset_yaw_service = nh.serviceClient<dji_sdk::MissionHpResetYaw>("dji_sdk/mission_hotpoint_reset_yaw");
00505 mission_fm_upload_service = nh.serviceClient<dji_sdk::MissionFmUpload>("dji_sdk/mission_followme_upload");
00506 mission_fm_set_target_service = nh.serviceClient<dji_sdk::MissionFmSetTarget>("dji_sdk/mission_followme_set_target");
00507
00508 acceleration_subscriber = nh.subscribe<dji_sdk::Acceleration>("dji_sdk/acceleration", 10, &DJIDrone::acceleration_subscriber_callback, this);
00509 attitude_quaternion_subscriber = nh.subscribe<dji_sdk::AttitudeQuaternion>("dji_sdk/attitude_quaternion", 10, &DJIDrone::attitude_quaternion_subscriber_callback, this);
00510 compass_subscriber = nh.subscribe<dji_sdk::Compass>("dji_sdk/compass", 10, &DJIDrone::compass_subscriber_callback, this);
00511 flight_control_info_subscriber = nh.subscribe<dji_sdk::FlightControlInfo>("dji_sdk/flight_control_info", 10, &DJIDrone::flight_control_info_subscriber_callback, this);
00512 flight_status_subscriber = nh.subscribe<std_msgs::UInt8>("dji_sdk/flight_status", 10, &DJIDrone::flight_status_subscriber_callback, this);
00513 gimbal_subscriber = nh.subscribe<dji_sdk::Gimbal>("dji_sdk/gimbal", 10, &DJIDrone::gimbal_subscriber_callback, this);
00514 global_position_subscriber = nh.subscribe<dji_sdk::GlobalPosition>("dji_sdk/global_position", 10, &DJIDrone::global_position_subscriber_callback, this);
00515 local_position_subscriber = nh.subscribe<dji_sdk::LocalPosition>("dji_sdk/local_position", 10, &DJIDrone::local_position_subscriber_callback, this);
00516 power_status_subscriber = nh.subscribe<dji_sdk::PowerStatus>("dji_sdk/power_status", 10, &DJIDrone::power_status_subscriber_callback, this);
00517 rc_channels_subscriber = nh.subscribe<dji_sdk::RCChannels>("dji_sdk/rc_channels", 10, &DJIDrone::rc_channels_subscriber_callback, this);
00518 velocity_subscriber = nh.subscribe<dji_sdk::Velocity>("dji_sdk/velocity", 10, &DJIDrone::velocity_subscriber_callback, this);
00519 activation_subscriber = nh.subscribe<std_msgs::UInt8>("dji_sdk/activation", 10, &DJIDrone::activation_subscriber_callback, this);
00520 odometry_subscriber = nh.subscribe<nav_msgs::Odometry>("dji_sdk/odometry",10, &DJIDrone::odometry_subscriber_callback, this);
00521 time_stamp_subscriber = nh.subscribe<dji_sdk::TimeStamp>("dji_sdk/time_stamp", 10, &DJIDrone::time_stamp_subscriber_callback,this);
00522 mission_status_subscriber = nh.subscribe<dji_sdk::MissionPushInfo>("dji_sdk/mission_status", 10, &DJIDrone::mission_status_push_info_callback, this);
00523 mission_event_subscriber = nh.subscribe<dji_sdk::MissionPushInfo>("dji_sdk/mission_event", 10, &DJIDrone::mission_event_push_info_callback, this);
00524 mobile_data_subscriber = nh.subscribe<dji_sdk::TransparentTransmissionData>("dji_sdk/data_received_from_remote_device", 10, &DJIDrone::mobile_data_push_info_callback, this);
00525 }
00526
00527 void setObtainControlMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00528 {
00529 obtainControlCallback.callback = userCallback;
00530 obtainControlCallback.userData = userData;
00531 }
00532
00533
00534 void setReleaseControlMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00535 {
00536 releaseControlCallback.callback = userCallback;
00537 releaseControlCallback.userData = userData;
00538 }
00539
00540 void setTakeOffMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00541 {
00542 takeOffCallback.callback = userCallback;
00543 takeOffCallback.userData = userData;
00544 }
00545
00546
00547 void setLandingMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00548 {
00549 landingCallback.callback = userCallback;
00550 landingCallback.userData = userData;
00551 }
00552
00553
00554 void setGetSDKVersionMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00555 {
00556 getSDKVersionCallback.callback = userCallback;
00557 getSDKVersionCallback.userData = userData;
00558 }
00559
00560
00561 void setArmMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00562 {
00563 armCallback.callback = userCallback;
00564 armCallback.userData = userData;
00565 }
00566
00567 void setDisarmMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00568 {
00569 disArmCallback.callback = userCallback;
00570 disArmCallback.userData = userData;
00571 }
00572
00573
00574
00575 void setGoHomeMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00576 {
00577 goHomeCallback.callback = userCallback;
00578 goHomeCallback.userData = userData;
00579 }
00580
00581
00582 void setTakePhotoMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00583 {
00584 takePhotoCallback.callback = userCallback;
00585 takePhotoCallback.userData = userData;
00586 }
00587
00588
00589 void setStartVideoMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00590 {
00591 startVideoCallback.callback = userCallback;
00592 startVideoCallback.userData = userData;
00593 }
00594
00595
00596 void setStopVideoMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00597 {
00598 stopVideoCallback.callback = userCallback;
00599 stopVideoCallback.userData = userData;
00600 }
00601
00602 void setDrawCircleDemoMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00603 {
00604 drawCircleDemoCallback.callback = userCallback;
00605 drawCircleDemoCallback.userData = userData;
00606 }
00607
00608 void setDrawSquareDemoMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00609 {
00610 drawSquareDemoCallback.callback = userCallback;
00611 drawSquareDemoCallback.userData = userData;
00612 }
00613
00614 void setAttitudeControlDemoMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00615 {
00616 attitudeControlDemoCallback.callback = userCallback;
00617 attitudeControlDemoCallback.userData = userData;
00618 }
00619
00620 void setLocalNavigationTestMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00621 {
00622 localNavigationTestCallback.callback = userCallback;
00623 localNavigationTestCallback.userData = userData;
00624 }
00625
00626 void setGlobalNavigationTestMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00627 {
00628 globalNavigationTestCallback.callback = userCallback;
00629 globalNavigationTestCallback.userData = userData;
00630 }
00631
00632 void setWaypointNavigationTestMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00633 {
00634 waypointNavigationTestCallback.callback = userCallback;
00635 waypointNavigationTestCallback.userData = userData;
00636 }
00637
00638 void setVirtuaRCTestMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00639 {
00640 virtualRCTestCallback.callback = userCallback;
00641 virtualRCTestCallback.userData = userData;
00642 }
00643
00644 void setGimbalControlDemoMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00645 {
00646 gimbalControlDemoCallback.callback = userCallback;
00647 gimbalControlDemoCallback.userData = userData;
00648 }
00649
00650 void setStartMapLASLoggingMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00651 {
00652 startMapLASLoggingCallback.callback = userCallback;
00653 startMapLASLoggingCallback.userData = userData;
00654 }
00655
00656 void setStopMapLASLoggingMobileCallback(DJIDrone::CallBack userCallback, UserData userData)
00657 {
00658 stopMapLASLoggingCallback.callback = userCallback;
00659 stopMapLASLoggingCallback.userData = userData;
00660 }
00661
00662 void setStartCollisionAvoidanceCallback(DJIDrone::CallBack userCallback, UserData userData)
00663 {
00664 startCollisionAvoidanceCallback.callback = userCallback;
00665 startCollisionAvoidanceCallback.userData = userData;
00666 }
00667
00668 void setStopCollisionAvoidanceCallback(DJIDrone::CallBack userCallback, UserData userData)
00669 {
00670 stopCollisionAvoidanceCallback.callback = userCallback;
00671 stopCollisionAvoidanceCallback.userData = userData;
00672 }
00673
00674 bool activate()
00675 {
00676 dji_sdk::Activation activate;
00677 return activation_service.call(activate) && activate.response.result;
00678 }
00679
00680 bool check_version()
00681 {
00682 dji_sdk::VersionCheck version_check;
00683 return version_check_service.call(version_check) && version_check.response.result;
00684 }
00685
00686 bool takeoff()
00687 {
00688 dji_sdk::DroneTaskControl drone_task_control;
00689 drone_task_control.request.task = 4;
00690 return drone_task_control_service.call(drone_task_control) && drone_task_control.response.result;
00691 }
00692
00693 bool landing()
00694 {
00695 dji_sdk::DroneTaskControl drone_task_control;
00696 drone_task_control.request.task = 6;
00697 return drone_task_control_service.call(drone_task_control) && drone_task_control.response.result;
00698 }
00699
00700 bool gohome()
00701 {
00702 dji_sdk::DroneTaskControl drone_task_control;
00703 drone_task_control.request.task = 1;
00704 return drone_task_control_service.call(drone_task_control) && drone_task_control.response.result;
00705 }
00706
00707 bool take_picture()
00708 {
00709 dji_sdk::CameraActionControl camera_action_control;
00710 camera_action_control.request.camera_action = 0;
00711 return camera_action_control_service.call(camera_action_control) && camera_action_control.response.result;
00712 }
00713
00714 bool start_video()
00715 {
00716 dji_sdk::CameraActionControl camera_action_control;
00717 camera_action_control.request.camera_action = 1;
00718 return camera_action_control_service.call(camera_action_control) && camera_action_control.response.result;
00719 }
00720
00721 bool stop_video()
00722 {
00723 dji_sdk::CameraActionControl camera_action_control;
00724 camera_action_control.request.camera_action = 2;
00725 return camera_action_control_service.call(camera_action_control) && camera_action_control.response.result;
00726 }
00727
00728 bool gimbal_speed_control(int roll_rate = 0, int pitch_rate = 0, int yaw_rate = 0)
00729 {
00730 dji_sdk::GimbalSpeedControl gimbal_speed_control;
00731 gimbal_speed_control.request.roll_rate = roll_rate;
00732 gimbal_speed_control.request.pitch_rate = pitch_rate;
00733 gimbal_speed_control.request.yaw_rate = yaw_rate;
00734
00735 return gimbal_speed_control_service.call(gimbal_speed_control) && gimbal_speed_control.response.result;
00736 }
00737
00738 bool gimbal_angle_control(int roll = 0, int pitch = 0, int yaw = 0, int duration = 0, bool absolute_or_incremental = 1, bool yaw_cmd_ignore = 0, bool roll_cmd_ignore = 0, bool pitch_cmd_ignore = 0)
00739 {
00740 dji_sdk::GimbalAngleControl gimbal_angle_control;
00741 gimbal_angle_control.request.roll = roll;
00742 gimbal_angle_control.request.pitch = pitch;
00743 gimbal_angle_control.request.yaw = yaw;
00744 gimbal_angle_control.request.duration = duration;
00745 gimbal_angle_control.request.absolute_or_incremental = absolute_or_incremental;
00746 gimbal_angle_control.request.yaw_cmd_ignore = yaw_cmd_ignore;
00747 gimbal_angle_control.request.roll_cmd_ignore = roll_cmd_ignore;
00748 gimbal_angle_control.request.pitch_cmd_ignore = pitch_cmd_ignore;
00749
00750 return gimbal_angle_control_service.call(gimbal_angle_control) && gimbal_angle_control.response.result;
00751 }
00752
00753 bool request_sdk_permission_control()
00754 {
00755 return sdk_permission_control(1);
00756 }
00757
00758 bool release_sdk_permission_control()
00759 {
00760 return sdk_permission_control(0);
00761 }
00762
00763 bool sdk_permission_control(unsigned char request)
00764 {
00765 dji_sdk::SDKPermissionControl sdk_permission_control;
00766 sdk_permission_control.request.control_enable = request;
00767
00768 return sdk_permission_control_service.call(sdk_permission_control) && sdk_permission_control.response.result;
00769
00770 }
00771
00772 bool attitude_control(unsigned char ctrl_flag, float x, float y, float z, float yaw)
00773 {
00774 dji_sdk::AttitudeControl attitude_control;
00775 attitude_control.request.flag = ctrl_flag;
00776 attitude_control.request.x = x;
00777 attitude_control.request.y = y;
00778 attitude_control.request.z = z;
00779 attitude_control.request.yaw = yaw;
00780
00781 return attitude_control_service.call(attitude_control) && attitude_control.response.result;
00782 }
00783
00784 bool velocity_control(int frame, float x, float y, float z, float yaw)
00785 {
00786 dji_sdk::VelocityControl velocity_control;
00787 velocity_control.request.frame = frame;
00788 velocity_control.request.vx = x;
00789 velocity_control.request.vy = y;
00790 velocity_control.request.vz = z;
00791 velocity_control.request.yawRate = yaw;
00792
00793 return velocity_control_service.call(velocity_control) && velocity_control.response.result;
00794 }
00795
00796 bool virtual_rc_enable()
00797 {
00798 dji_sdk::VirtualRCEnableControl virtual_rc_enable_control;
00799 virtual_rc_enable_control.request.enable = 1;
00800 virtual_rc_enable_control.request.if_back_to_real = 1;
00801
00802 return virtual_rc_enable_control_service.call(virtual_rc_enable_control) && virtual_rc_enable_control.response.result;
00803 }
00804
00805 bool virtual_rc_disable()
00806 {
00807 dji_sdk::VirtualRCEnableControl virtual_rc_enable_control;
00808 virtual_rc_enable_control.request.enable = 0;
00809 virtual_rc_enable_control.request.if_back_to_real = 1;
00810
00811 return virtual_rc_enable_control_service.call(virtual_rc_enable_control) && virtual_rc_enable_control.response.result;
00812 }
00813
00814 bool virtual_rc_control(uint32_t channel_data[16])
00815 {
00816 dji_sdk::VirtualRCDataControl virtual_rc_data_control;
00817
00818 for (int i = 0; i < 16; i ++)
00819 {
00820 virtual_rc_data_control.request.channel_data[i] = channel_data[i];
00821 }
00822
00823 return virtual_rc_data_control_service.call(virtual_rc_data_control) && virtual_rc_data_control.response.result;
00824 }
00825
00826 bool drone_arm()
00827 {
00828 dji_sdk::DroneArmControl drone_arm_control;
00829 drone_arm_control.request.arm = 1;
00830 return drone_arm_control_service.call(drone_arm_control) && drone_arm_control.response.result;
00831 }
00832
00833 bool drone_disarm()
00834 {
00835 dji_sdk::DroneArmControl drone_arm_control;
00836 drone_arm_control.request.arm = 0;
00837 return drone_arm_control_service.call(drone_arm_control) && drone_arm_control.response.result;
00838 }
00839
00840 bool sync_flag_control(float frequency)
00841 {
00842 dji_sdk::SyncFlagControl sync_flag_control;
00843 sync_flag_control.request.frequency = frequency;
00844 return sync_flag_control_service.call(sync_flag_control) && sync_flag_control.response.result;
00845 }
00846
00847 bool set_message_frequency(uint8_t frequency_data[16])
00848 {
00849 dji_sdk::MessageFrequencyControl message_frequency_control;
00850
00851 for (int i = 0; i < 16; i++)
00852 {
00853 message_frequency_control.request.frequency[i] = frequency_data[i];
00854 }
00855
00856 return message_frequency_control_service.call(message_frequency_control) && message_frequency_control.response.result;
00857 }
00858
00859 bool local_position_control(float x, float y, float z, float yaw)
00860 {
00861 dji_sdk::LocalPositionControl local_position_control;
00862 local_position_control.request.x = x;
00863 local_position_control.request.y = y;
00864 local_position_control.request.z = z;
00865 local_position_control.request.yaw = yaw;
00866
00867 return local_position_control_service.call(local_position_control) && local_position_control.response.result;
00868
00869 }
00870
00871 bool global_position_control(double latitude, double longitude, float altitude, float yaw)
00872 {
00873 dji_sdk::GlobalPositionControl global_position_control;
00874 global_position_control.request.latitude = latitude;
00875 global_position_control.request.longitude = longitude;
00876 global_position_control.request.altitude = altitude;
00877 global_position_control.request.yaw = yaw;
00878
00879 return global_position_control_service.call(global_position_control) && global_position_control.response.result;
00880 }
00881
00882 void local_position_navigation_cancel_current_goal()
00883 {
00884 local_position_navigation_action_client.cancelGoal();
00885 }
00886
00887 void local_position_navigation_cancel_all_goals()
00888 {
00889 local_position_navigation_action_client.cancelAllGoals();
00890 }
00891
00892 void local_position_navigation_cancel_goals_at_and_before_time(const ros::Time time)
00893 {
00894 local_position_navigation_action_client.cancelGoalsAtAndBeforeTime(time);
00895 }
00896
00897 dji_sdk::LocalPositionNavigationResultConstPtr local_position_navigation_get_result()
00898 {
00899 return local_position_navigation_action_client.getResult();
00900 }
00901
00902 actionlib::SimpleClientGoalState local_position_navigation_get_state()
00903 {
00904 return local_position_navigation_action_client.getState();
00905 }
00906
00907 bool local_position_navigation_is_server_connected()
00908 {
00909 return local_position_navigation_action_client.isServerConnected();
00910 }
00911
00912 void local_position_navigation_send_request(float x, float y, float z,
00913 LocalPositionNavigationActionClient::SimpleDoneCallback done_callback = LocalPositionNavigationActionClient::SimpleDoneCallback(),
00914 LocalPositionNavigationActionClient::SimpleActiveCallback active_callback = LocalPositionNavigationActionClient::SimpleActiveCallback(),
00915 LocalPositionNavigationActionClient::SimpleFeedbackCallback feedback_callback = LocalPositionNavigationActionClient::SimpleFeedbackCallback())
00916 {
00917 dji_sdk::LocalPositionNavigationGoal local_position_navigation_goal;
00918 local_position_navigation_goal.x = x;
00919 local_position_navigation_goal.y = y;
00920 local_position_navigation_goal.z = z;
00921 local_position_navigation_action_client.sendGoal(local_position_navigation_goal, done_callback, active_callback, feedback_callback);
00922 }
00923
00924 bool local_position_navigation_wait_for_result(const ros::Duration duration = ros::Duration(0))
00925 {
00926 return local_position_navigation_action_client.waitForResult(duration);
00927 }
00928
00929 bool local_position_navigation_stop_tracking_goal()
00930 {
00931 local_position_navigation_action_client.stopTrackingGoal();
00932 return true;
00933 }
00934
00935 bool local_position_navigation_wait_server(const ros::Duration duration = ros::Duration(0))
00936 {
00937 return local_position_navigation_action_client.waitForServer(duration);
00938 }
00939
00940 void global_position_navigation_cancel_current_goal()
00941 {
00942 global_position_navigation_action_client.cancelGoal();
00943 }
00944
00945 void global_position_navigation_cancel_all_goals()
00946 {
00947 global_position_navigation_action_client.cancelAllGoals();
00948 }
00949
00950 void global_position_navigation_cancel_goals_at_and_before_time(const ros::Time time)
00951 {
00952 global_position_navigation_action_client.cancelGoalsAtAndBeforeTime(time);
00953 }
00954
00955 dji_sdk::GlobalPositionNavigationResultConstPtr global_position_navigation_get_result()
00956 {
00957 return global_position_navigation_action_client.getResult();
00958 }
00959
00960 actionlib::SimpleClientGoalState global_position_navigation_get_state()
00961 {
00962 return global_position_navigation_action_client.getState();
00963 }
00964
00965 bool global_position_navigation_is_server_connected()
00966 {
00967 return global_position_navigation_action_client.isServerConnected();
00968 }
00969
00970 void global_position_navigation_send_request(double latitude, double longitude, float altitude,
00971 GlobalPositionNavigationActionClient::SimpleDoneCallback done_callback = GlobalPositionNavigationActionClient::SimpleDoneCallback(),
00972 GlobalPositionNavigationActionClient::SimpleActiveCallback active_callback = GlobalPositionNavigationActionClient::SimpleActiveCallback(),
00973 GlobalPositionNavigationActionClient::SimpleFeedbackCallback feedback_callback = GlobalPositionNavigationActionClient::SimpleFeedbackCallback())
00974 {
00975 dji_sdk::GlobalPositionNavigationGoal global_position_navigation_goal;
00976 global_position_navigation_goal.latitude = latitude;
00977 global_position_navigation_goal.longitude = longitude;
00978 global_position_navigation_goal.altitude = altitude;
00979 global_position_navigation_action_client.sendGoal(global_position_navigation_goal, done_callback, active_callback, feedback_callback);
00980 }
00981
00982 bool global_position_navigation_wait_for_result (const ros::Duration duration = ros::Duration(0))
00983 {
00984 return global_position_navigation_action_client.waitForResult(duration);
00985 }
00986
00987 bool global_position_navigation_stop_tracking_goal()
00988 {
00989 global_position_navigation_action_client.stopTrackingGoal();
00990 return true;
00991 }
00992
00993 bool global_position_navigation_wait_server(const ros::Duration duration = ros::Duration(0))
00994 {
00995 return global_position_navigation_action_client.waitForServer(duration);
00996 }
00997
00998
00999 void waypoint_navigation_cancel_current_goal()
01000 {
01001 waypoint_navigation_action_client.cancelGoal();
01002 }
01003
01004 void waypoint_navigation_cancel_all_goals()
01005 {
01006 waypoint_navigation_action_client.cancelAllGoals();
01007 }
01008
01009 void waypoint_navigation_cancel_goals_at_and_before_time(const ros::Time time)
01010 {
01011 waypoint_navigation_action_client.cancelGoalsAtAndBeforeTime(time);
01012 }
01013
01014 dji_sdk::WaypointNavigationResultConstPtr waypoint_navigation_get_result()
01015 {
01016 return waypoint_navigation_action_client.getResult();
01017 }
01018
01019 actionlib::SimpleClientGoalState waypoint_navigation_get_state()
01020 {
01021 return waypoint_navigation_action_client.getState();
01022 }
01023
01024 bool waypoint_navigation_is_server_connected()
01025 {
01026 return waypoint_navigation_action_client.isServerConnected();
01027 }
01028
01029 void waypoint_navigation_send_request(dji_sdk::WaypointList waypoint_data,
01030 WaypointNavigationActionClient::SimpleDoneCallback done_callback = WaypointNavigationActionClient::SimpleDoneCallback(),
01031 WaypointNavigationActionClient::SimpleActiveCallback active_callback = WaypointNavigationActionClient::SimpleActiveCallback(),
01032 WaypointNavigationActionClient::SimpleFeedbackCallback feedback_callback = WaypointNavigationActionClient::SimpleFeedbackCallback())
01033 {
01034 dji_sdk::WaypointNavigationGoal waypoint_navigation_goal;
01035 waypoint_navigation_goal.waypoint_list = waypoint_data;
01036 waypoint_navigation_action_client.sendGoal(waypoint_navigation_goal, done_callback, active_callback, feedback_callback);
01037 }
01038
01039 bool waypoint_navigation_wait_for_result(const ros::Duration duration = ros::Duration(0))
01040 {
01041 return waypoint_navigation_action_client.waitForResult(duration);
01042 }
01043
01044 bool waypoint_navigation_stop_tracking_goal()
01045 {
01046 waypoint_navigation_action_client.stopTrackingGoal();
01047 return true;
01048 }
01049
01050 bool waypoint_navigation_wait_server(const ros::Duration duration = ros::Duration(0))
01051 {
01052 return waypoint_navigation_action_client.waitForServer(duration);
01053 }
01054
01055 bool mission_start()
01056 {
01057 dji_sdk::MissionStart mission_start;
01058 return mission_start_service.call(mission_start)&&mission_start.response.result;
01059 }
01060
01061 bool mission_pause()
01062 {
01063 dji_sdk::MissionPause mission_pause;
01064 mission_pause.request.pause = 0;
01065 return mission_pause_service.call(mission_pause)&&mission_pause.response.result;
01066 }
01067
01068 bool mission_resume()
01069 {
01070 dji_sdk::MissionPause mission_pause;
01071 mission_pause.request.pause = 1;
01072 return mission_pause_service.call(mission_pause)&&mission_pause.response.result;
01073 }
01074
01075 bool mission_cancel()
01076 {
01077 dji_sdk::MissionCancel mission_cancel;
01078 return mission_cancel_service.call(mission_cancel)&&mission_cancel.response.result;
01079 }
01080
01081 bool mission_waypoint_upload(dji_sdk::MissionWaypointTask waypoint_task)
01082 {
01083 dji_sdk::MissionWpUpload mission_waypoint_task;
01084 mission_waypoint_task.request.waypoint_task = waypoint_task;
01085 return mission_wp_upload_service.call(mission_waypoint_task)&&mission_waypoint_task.response.result;
01086 }
01087
01088 dji_sdk::MissionWaypointTask mission_waypoint_download()
01089 {
01090 dji_sdk::MissionWpDownload mission_waypoint_download;
01091 mission_wp_download_service.call(mission_waypoint_download);
01092 return mission_waypoint_download.response.waypoint_task;
01093 }
01094
01095 bool mission_waypoint_set_speed(float speed)
01096 {
01097 dji_sdk::MissionWpSetSpeed mission_waypoint_set_speed;
01098 mission_waypoint_set_speed.request.speed = speed;
01099 return mission_wp_set_speed_service.call(mission_waypoint_set_speed)&&mission_waypoint_set_speed.response.result;
01100 }
01101
01102 float mission_waypoint_get_speed()
01103 {
01104 dji_sdk::MissionWpGetSpeed mission_waypoint_get_speed;
01105 mission_wp_get_speed_service.call(mission_waypoint_get_speed);
01106 return mission_waypoint_get_speed.response.speed;
01107 }
01108
01109 bool mission_hotpoint_upload(dji_sdk::MissionHotpointTask hotpoint_task)
01110 {
01111 dji_sdk::MissionHpUpload mission_hotpoint_upload;
01112 mission_hotpoint_upload.request.hotpoint_task = hotpoint_task;
01113 return mission_hp_upload_service.call(mission_hotpoint_upload)&&mission_hotpoint_upload.response.result;
01114 }
01115
01116 dji_sdk::MissionHotpointTask mission_hotpoint_download()
01117 {
01118 dji_sdk::MissionHpDownload mission_hotpoint_download;
01119 mission_hp_download_service.call(mission_hotpoint_download);
01120 return mission_hotpoint_download.response.hotpoint_task;
01121 }
01122
01123 bool mission_hotpoint_set_speed(float speed, uint8_t direction)
01124 {
01125 dji_sdk::MissionHpSetSpeed mission_hotpoint_set_speed;
01126 mission_hotpoint_set_speed.request.speed = speed;
01127 mission_hotpoint_set_speed.request.direction = direction;
01128 return mission_hp_set_speed_service.call(mission_hotpoint_set_speed)&&mission_hotpoint_set_speed.response.result;
01129 }
01130
01131 bool mission_hotpoint_set_radius(float radius)
01132 {
01133 dji_sdk::MissionHpSetRadius mission_hotpoint_set_radius;
01134 mission_hotpoint_set_radius.request.radius = radius;
01135 return mission_hp_set_radius_service.call(mission_hotpoint_set_radius)&&mission_hotpoint_set_radius.response.result;
01136 }
01137
01138 bool mission_hotpoint_reset_yaw()
01139 {
01140 dji_sdk::MissionHpResetYaw mission_hotpoint_reset_yaw;
01141 return mission_hp_reset_yaw_service.call(mission_hotpoint_reset_yaw)&&mission_hotpoint_reset_yaw.response.result;
01142 }
01143
01144 bool mission_followme_upload(dji_sdk::MissionFollowmeTask followme_task)
01145 {
01146 dji_sdk::MissionFmUpload mission_followme_task;
01147 mission_followme_task.request.followme_task = followme_task;
01148 return mission_fm_upload_service.call(mission_followme_task)&&mission_followme_task.response.result;
01149 }
01150
01151 bool mission_followme_update_target(dji_sdk::MissionFollowmeTarget followme_target)
01152 {
01153 dji_sdk::MissionFmSetTarget mission_followme_set_target;
01154 mission_followme_set_target.request.followme_target = followme_target;
01155 return mission_fm_set_target_service.call(mission_followme_set_target)&&mission_followme_set_target.response.result;
01156 }
01157
01158 };