dji_sdk: dji_sdk_node.h Source File

Go to the documentation of this file.
00001 
00012 #ifndef __DJI_SDK_NODE_H__
00013 #define __DJI_SDK_NODE_H__
00014 
00015 #include <ros/ros.h>
00016 #include <nav_msgs/Odometry.h>
00017 #include <std_msgs/UInt8.h>
00018 #include <boost/bind.hpp>
00019 #include <dji_sdk/dji_sdk.h>
00020 #include <actionlib/server/simple_action_server.h>
00021 #include <dji_sdk/dji_sdk_mission.h>
00022 
00023 #define C_EARTH (double) 6378137.0
00024 #define C_PI (double) 3.141592653589793
00025 #define DEG2RAD(DEG) ((DEG)*((C_PI)/(180.0)))
00026 
00027 extern DJI::onboardSDK::ROSAdapter *rosAdapter;
00028 using namespace DJI::onboardSDK;
00029 
00030 class DJISDKNode
00031 {
00032 private:
00033 //Drone state variables:
00034     dji_sdk::Acceleration acceleration;
00035     dji_sdk::AttitudeQuaternion attitude_quaternion;
00036     dji_sdk::Compass compass;
00037     dji_sdk::FlightControlInfo flight_control_info;
00038     uint8_t flight_status;
00039     dji_sdk::Gimbal gimbal;
00040     dji_sdk::GlobalPosition global_position;
00041     dji_sdk::GlobalPosition global_position_ref;
00042     dji_sdk::LocalPosition local_position;
00043     dji_sdk::LocalPosition local_position_ref;
00044     dji_sdk::PowerStatus power_status;
00045     dji_sdk::RCChannels rc_channels;
00046     dji_sdk::Velocity velocity;
00047     nav_msgs::Odometry odometry;
00048         dji_sdk::TimeStamp time_stamp;
00049         dji_sdk::A3GPS A3_GPS;
00050         dji_sdk::A3RTK A3_RTK;
00051 
00052 
00053         bool activation_result = false;
00054     bool localposbase_use_height = true;
00055 
00056     int global_position_ref_seted = 0;
00057 
00058 //internal variables
00059     DJISDKMission* dji_sdk_mission;
00060     char app_key[65];
00061         unsigned char transparent_transmission_data[100];
00062     ActivateData user_act_data;
00063 
00064 //Publishers:
00065     ros::Publisher activation_publisher;
00066     ros::Publisher acceleration_publisher;
00067     ros::Publisher attitude_quaternion_publisher;
00068     ros::Publisher compass_publisher;
00069     ros::Publisher flight_control_info_publisher;
00070     ros::Publisher flight_status_publisher;
00071     ros::Publisher gimbal_publisher;
00072     ros::Publisher global_position_publisher;
00073     ros::Publisher local_position_publisher;
00074     ros::Publisher power_status_publisher;
00075     ros::Publisher rc_channels_publisher;
00076     ros::Publisher velocity_publisher;
00077     ros::Publisher odometry_publisher;
00078     ros::Publisher time_stamp_publisher;
00079     ros::Publisher data_received_from_remote_device_publisher;
00080 
00081     ros::Publisher A3_GPS_info_publisher;
00082     ros::Publisher A3_RTK_info_publisher;
00083 
00084     void init_publishers(ros::NodeHandle& nh)
00085     {
00086         // start ros publisher
00087         activation_publisher = nh.advertise<std_msgs::UInt8>("dji_sdk/activation", 10);
00088         acceleration_publisher = nh.advertise<dji_sdk::Acceleration>("dji_sdk/acceleration", 10);
00089         attitude_quaternion_publisher = nh.advertise<dji_sdk::AttitudeQuaternion>("dji_sdk/attitude_quaternion", 10);
00090         compass_publisher = nh.advertise<dji_sdk::Compass>("dji_sdk/compass", 10);
00091         flight_control_info_publisher = nh.advertise<dji_sdk::FlightControlInfo>("dji_sdk/flight_control_info", 10);
00092         flight_status_publisher = nh.advertise<std_msgs::UInt8>("dji_sdk/flight_status", 10);
00093         gimbal_publisher = nh.advertise<dji_sdk::Gimbal>("dji_sdk/gimbal", 10);
00094         global_position_publisher = nh.advertise<dji_sdk::GlobalPosition>("dji_sdk/global_position", 10);
00095         local_position_publisher = nh.advertise<dji_sdk::LocalPosition>("dji_sdk/local_position", 10);
00096         power_status_publisher = nh.advertise<dji_sdk::PowerStatus>("dji_sdk/power_status", 10);
00097         rc_channels_publisher = nh.advertise<dji_sdk::RCChannels>("dji_sdk/rc_channels", 10);
00098         velocity_publisher = nh.advertise<dji_sdk::Velocity>("dji_sdk/velocity", 10);
00099         odometry_publisher = nh.advertise<nav_msgs::Odometry>("dji_sdk/odometry",10);
00100         time_stamp_publisher = nh.advertise<dji_sdk::TimeStamp>("dji_sdk/time_stamp", 10);
00101         data_received_from_remote_device_publisher = nh.advertise<dji_sdk::TransparentTransmissionData>("dji_sdk/data_received_from_remote_device",10);
00102 
00103         //TODO: Identify the drone version first        
00104         A3_GPS_info_publisher = nh.advertise<dji_sdk::A3GPS>("dji_sdk/A3_GPS", 10);
00105         A3_RTK_info_publisher = nh.advertise<dji_sdk::A3RTK>("dji_sdk/A3_RTK", 10);
00106         
00107 
00108 
00109     }
00110 
00111 //Services:
00112     ros::ServiceServer activation_service;
00113     ros::ServiceServer attitude_control_service;
00114     ros::ServiceServer camera_action_control_service;
00115     ros::ServiceServer drone_task_control_service;
00116     ros::ServiceServer gimbal_angle_control_service;
00117     ros::ServiceServer gimbal_speed_control_service;
00118     ros::ServiceServer global_position_control_service;
00119     ros::ServiceServer local_position_control_service;
00120     ros::ServiceServer sdk_permission_control_service;
00121     ros::ServiceServer velocity_control_service;
00122     ros::ServiceServer version_check_service;
00123         ros::ServiceServer send_data_to_remote_device_service;
00124 
00125         ros::ServiceServer virtual_rc_enable_control_service;
00126         ros::ServiceServer virtual_rc_data_control_service;
00127         ros::ServiceServer drone_arm_control_service;
00128         ros::ServiceServer sync_flag_control_service;
00129         ros::ServiceServer message_frequency_control_service;
00130 
00131         bool activation_callback(dji_sdk::Activation::Request& request, dji_sdk::Activation::Response& response);
00132     bool attitude_control_callback(dji_sdk::AttitudeControl::Request& request, dji_sdk::AttitudeControl::Response& response);
00133     bool camera_action_control_callback(dji_sdk::CameraActionControl::Request& request, dji_sdk::CameraActionControl::Response& response);
00134     bool drone_task_control_callback(dji_sdk::DroneTaskControl::Request& request, dji_sdk::DroneTaskControl::Response& response);
00135     bool gimbal_angle_control_callback(dji_sdk::GimbalAngleControl::Request& request, dji_sdk::GimbalAngleControl::Response& response);
00136     bool gimbal_speed_control_callback(dji_sdk::GimbalSpeedControl::Request& request, dji_sdk::GimbalSpeedControl::Response& response);
00137     bool global_position_control_callback(dji_sdk::GlobalPositionControl::Request& request, dji_sdk::GlobalPositionControl::Response& response);
00138     bool local_position_control_callback(dji_sdk::LocalPositionControl::Request& request, dji_sdk::LocalPositionControl::Response& response);
00139     bool sdk_permission_control_callback(dji_sdk::SDKPermissionControl::Request& request, dji_sdk::SDKPermissionControl::Response& response);
00140     bool velocity_control_callback(dji_sdk::VelocityControl::Request& request, dji_sdk::VelocityControl::Response& response);
00141         bool virtual_rc_enable_control_callback(dji_sdk::VirtualRCEnableControl::Request& request, dji_sdk::VirtualRCEnableControl::Response& response);
00142         bool virtual_rc_data_control_callback(dji_sdk::VirtualRCDataControl::Request& request, dji_sdk::VirtualRCDataControl::Response& response);
00143         bool drone_arm_control_callback(dji_sdk::DroneArmControl::Request& request, dji_sdk::DroneArmControl::Response& response);
00144         bool sync_flag_control_callback(dji_sdk::SyncFlagControl::Request& request, dji_sdk::SyncFlagControl::Response& response);
00145         bool message_frequency_control_callback(dji_sdk::MessageFrequencyControl::Request& request, dji_sdk::MessageFrequencyControl::Response& response);
00146         bool version_check_callback(dji_sdk::VersionCheck::Request& requset, dji_sdk::VersionCheck::Response& response);
00147         bool send_data_to_remote_device_callback(dji_sdk::SendDataToRemoteDevice::Request& request, dji_sdk::SendDataToRemoteDevice::Response& response);
00148 
00149     void init_services(ros::NodeHandle& nh)
00150     {
00151                 activation_service = nh.advertiseService("dji_sdk/activation", &DJISDKNode::activation_callback, this);
00152         attitude_control_service = nh.advertiseService("dji_sdk/attitude_control", &DJISDKNode::attitude_control_callback, this);
00153         camera_action_control_service = nh.advertiseService("dji_sdk/camera_action_control",&DJISDKNode::camera_action_control_callback, this);
00154         drone_task_control_service = nh.advertiseService("dji_sdk/drone_task_control", &DJISDKNode::drone_task_control_callback, this);
00155         gimbal_angle_control_service = nh.advertiseService("dji_sdk/gimbal_angle_control", &DJISDKNode::gimbal_angle_control_callback, this);
00156         gimbal_speed_control_service = nh.advertiseService("dji_sdk/gimbal_speed_control", &DJISDKNode::gimbal_speed_control_callback, this);
00157         global_position_control_service = nh.advertiseService("dji_sdk/global_position_control", &DJISDKNode::global_position_control_callback, this);
00158         local_position_control_service = nh.advertiseService("dji_sdk/local_position_control", &DJISDKNode::local_position_control_callback, this);
00159         sdk_permission_control_service = nh.advertiseService("dji_sdk/sdk_permission_control", &DJISDKNode::sdk_permission_control_callback, this);
00160         velocity_control_service = nh.advertiseService("dji_sdk/velocity_control", &DJISDKNode::velocity_control_callback, this);
00161                 version_check_service = nh.advertiseService("dji_sdk/version_check", &DJISDKNode::version_check_callback, this);
00162                 virtual_rc_enable_control_service = nh.advertiseService("dji_sdk/virtual_rc_enable_control", &DJISDKNode::virtual_rc_enable_control_callback, this);
00163                 virtual_rc_data_control_service = nh.advertiseService("dji_sdk/virtual_rc_data_control", &DJISDKNode::virtual_rc_data_control_callback,this);
00164                 drone_arm_control_service = nh.advertiseService("dji_sdk/drone_arm_control", &DJISDKNode::drone_arm_control_callback, this);
00165                 sync_flag_control_service = nh.advertiseService("dji_sdk/sync_flag_control", &DJISDKNode::sync_flag_control_callback, this);
00166                 message_frequency_control_service = nh.advertiseService("dji_sdk/message_frequency_control", &DJISDKNode::message_frequency_control_callback, this);
00167                 send_data_to_remote_device_service = nh.advertiseService("dji_sdk/send_data_to_remote_device", &DJISDKNode::send_data_to_remote_device_callback,this);
00168     }
00169 
00170 //Actions:
00171     typedef actionlib::SimpleActionServer<dji_sdk::DroneTaskAction> DroneTaskActionServer;
00172     typedef actionlib::SimpleActionServer<dji_sdk::LocalPositionNavigationAction> LocalPositionNavigationActionServer;
00173     typedef actionlib::SimpleActionServer<dji_sdk::GlobalPositionNavigationAction> GlobalPositionNavigationActionServer;
00174     typedef actionlib::SimpleActionServer<dji_sdk::WaypointNavigationAction> WaypointNavigationActionServer;
00175 
00176     DroneTaskActionServer* drone_task_action_server;
00177     LocalPositionNavigationActionServer* local_position_navigation_action_server;
00178     GlobalPositionNavigationActionServer* global_position_navigation_action_server;
00179     WaypointNavigationActionServer* waypoint_navigation_action_server;
00180 
00181     dji_sdk::DroneTaskFeedback drone_task_feedback;
00182     dji_sdk::DroneTaskResult drone_task_result;
00183     dji_sdk::LocalPositionNavigationFeedback local_position_navigation_feedback;
00184     dji_sdk::LocalPositionNavigationResult local_position_navigation_result;
00185     dji_sdk::GlobalPositionNavigationFeedback global_position_navigation_feedback;
00186     dji_sdk::GlobalPositionNavigationResult global_position_navigation_result;
00187     dji_sdk::WaypointNavigationFeedback waypoint_navigation_feedback;
00188     dji_sdk::WaypointNavigationResult waypoint_navigation_result;
00189 
00190     bool drone_task_action_callback(const dji_sdk::DroneTaskGoalConstPtr& goal);
00191     bool local_position_navigation_action_callback(const dji_sdk::LocalPositionNavigationGoalConstPtr& goal);
00192     bool global_position_navigation_action_callback(const dji_sdk::GlobalPositionNavigationGoalConstPtr& goal);
00193     bool waypoint_navigation_action_callback(const dji_sdk::WaypointNavigationGoalConstPtr& goal);
00194 
00195     void init_actions(ros::NodeHandle& nh)
00196     {
00197         drone_task_action_server = new DroneTaskActionServer(nh,
00198             "dji_sdk/drone_task_action",
00199             boost::bind(&DJISDKNode::drone_task_action_callback, this, _1), false);
00200         drone_task_action_server->start();
00201 
00202         local_position_navigation_action_server = new LocalPositionNavigationActionServer(nh,
00203             "dji_sdk/local_position_navigation_action",
00204             boost::bind(&DJISDKNode::local_position_navigation_action_callback, this, _1), false);
00205         local_position_navigation_action_server->start();
00206 
00207         global_position_navigation_action_server = new GlobalPositionNavigationActionServer(nh,
00208             "dji_sdk/global_position_navigation_action",
00209             boost::bind(&DJISDKNode::global_position_navigation_action_callback, this, _1), false );
00210         global_position_navigation_action_server->start();
00211 
00212         waypoint_navigation_action_server = new WaypointNavigationActionServer(nh,
00213             "dji_sdk/waypoint_navigation_action",
00214             boost::bind(&DJISDKNode::waypoint_navigation_action_callback, this, _1), false);
00215         waypoint_navigation_action_server->start();
00216     }
00217 
00218 public:
00219     DJISDKNode(ros::NodeHandle& nh, ros::NodeHandle& nh_private);
00220 
00221 private:
00222     int init_parameters(ros::NodeHandle& nh_private);
00223     void broadcast_callback();
00224         void transparent_transmission_callback(unsigned char *buf, unsigned char len);
00225 
00226     bool process_waypoint(dji_sdk::Waypoint new_waypoint);
00227 
00228     void gps_convert_ned(float &ned_x, float &ned_y,
00229             double gps_t_lon, double gps_t_lat,
00230             double gps_r_lon, double gps_r_lat);
00231 
00232     dji_sdk::LocalPosition gps_convert_ned(dji_sdk::GlobalPosition loc);
00233 
00234 };
00235 
00236 #endif