玩转四旋翼无人机（DJI OnBoard SDK ROS）
System Structure
- dji_sdk: the core package handling the communication with Matrice 100, which provides a header file
dji_drone.hfor future use - dji_sdk_demo: an example package of using
dji_drone.hto control the Matrice 100 - dji_sdk_web_groundstation: a WebSocket example using ROS-bridge-suite, where a webpage groundstatino is provided
- dji_sdk_manifold_read_cam: a specifed X3 video reading package for Manifold, video stream will be published out in RGB, CATKIN_IGNOREd by defualt
- dji_sdk_manifold_read_cam_nv: same as the previous one, but use hardware decoding method and in Grayscale, CATKIN_IGNOREd by defualt
- dji_sdk_doc: all documents
DJI Onboard SDK ROS Package for Video Decoding on Manifold
This package is a specified video decoding package for Manifold.
How to use
- Install the necessary library: refer here
- Delete the
CATKIN_IGNOREfile inside package andcatkin_make. rosrun dji_sdk_manifold_read_cam_nv dji_sdk_manifold_read_cam_nv- The image will be published into topic
/dji_sdk/image_raw
Note:
- This package is specially designed for Manifold.
- The RC controller must be connected to Matrice 100 in order to get the video stream.
- This package does not provice video transparent transmission. You cannot see the video on DJI Go while running this package.
- This package uses hardware decoding method, while the other one uses FFMPEG.
- The image format is in Grayscale, while the other one publishes RGB image.
DJI Onboard SDK ROS Package for Video Decoding on Manifold
This package is a specified video decoding package for Manifold.
How to use
- Install the necessary library: it should be already there, try to run the project in the
demofolder of your Manifold home directory. If not, please run theinstall_lib.sh - Delete the
CATKIN_IGNOREfile inside package andcatkin_make. rosrun dji_sdk_manifold_read_cam dji_sdk_manifold_read_cam- The image will be published into topic
/dji_sdk/image_raw
Note:
- This package is specially designed for Manifold.
- The RC controller must be connected to Matrice 100 in order to get the video stream.
- This package does not provice video transparent transmission. You cannot see the video on DJI Go while running this package.
- This package uses FFMPENG for video decoding. while the _nv one uses hardware decoding method.
- The image format is RGB, while the _nv one publishes Grayscale image.
DJI_SDK_Node是这个ROS package的核心部分，可以看出它打包了DJI_LIB，向外展示了三个模块，一是dji_sdk_node_main.cpp(Publisher), dji_sdk_node_service.cpp(Service_Server), 以及dji_sdk_node_action.cpp(ActionLib Server).为了理解这三个文件我们首先看一下，他们构成了一个类就是DJISDKNode, 所以这里先看一下他的类头文件。
//# acceleration
Header header
int32 ts
float32 ax
float32 ay
float32 az
//#//#Gimbal
Header header
int32 ts
float32 pitch
float32 yaw
float32 roll
//#Compass
Header header
int32 ts
int8 x
int8 y
int8 z
//#FlightControlInfo
int8 cur_ctrl_dev_in_navi_mode
int8 serial_req_status
//#AttitudeQuaternion
//# The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right), expressed as quaternion. Quaternion order is w, x, y, z and a zero rotation would be expressed as (1 0 0 0).
Header header
int32 ts
//# Quaternion component
float32 q0
float32 q1
float32 q2
float32 q3
//# Angular speed (rad/s)
float32 wx
float32 wy
float32 wz
//#GlobalPosition
Header header
int32 ts
//#latitude is in angle
float64 latitude
//#longitude is in angle
float64 longitude
float32 altitude
float32 height
//#reliablity [0,5]
int8 health
//#LocalPosition
//# North(x)
//# /
//# /
//# /______East(y)
//# |
//# |
//# Donw (-z)
Header header
int32 ts
float32 x
float32 y
float32 z
//#RCChannels
//# RC Map
//#
//# mode:
//#
//# +8000 <---> 0 <---> -8000
//# API <---> ATT <---> POS
//#
//# CH3(throttle) +10000 CH1(pitch) +10000
//# ^ ^
//# | | / -10000
//# CH2(yaw) | CH0(roll) | /
//# -10000 <-----------> +10000 -10000 <-----------> +10000 H(gear)
//# | | \
//# | | \ -4545
//# V V
//# -10000 -10000
//#
Header header
int32 ts
float32 roll
float32 pitch
float32 yaw
float32 throttle
float32 mode
float32 gear
//#waypoint
//#latitude is in degree
float64 latitude
//#longitude is in degree
float64 longitude
float32 altitude
//#heading is in [-180,180]
int16 heading
//#stay time is in second
uint16 staytime
//#Velocity
Header header
int32 ts
float32 vx
float32 vy
float32 vz
//# if from guidance
uint8 health_flag
uint8 feedback_sensor_id这是从文件msg中的读出的变量定义结构，其他action，srv中的变量类似如此。DJISDKNode中开始定义了一些状态变量，加速度，位姿四元数，位置和姿态等。
class DJISDKNode
{
private:
//Drone state variables:
dji_sdk::Acceleration acceleration;
dji_sdk::AttitudeQuaternion attitude_quaternion;
dji_sdk::Compass compass;
dji_sdk::FlightControlInfo flight_
control_info;
uint8_t flight_status;
dji_sdk::Gimbal gimbal;//姿态，yaw，pitch,roll
dji_sdk::GlobalPosition global_position;//维度，经度，海拔。
dji_sdk::GlobalPosition global_position_ref;
dji_sdk::LocalPosition local_position;//x,y,z
dji_sdk::LocalPosition local_position_ref;
dji_sdk::PowerStatus power_status;
dji_sdk::RCChannels rc_channels;
dji_sdk::Velocity velocity;
nav_msgs::Odometry odometry;
bool sdk_permission_opened = false;
bool activated = false;
bool localposbase_use_height = true;
int global_position_ref_seted = 0;
//internal variables
char app_key[65];
activate_data_t user_act_data;
//对应前面的状态变量各有一个发布者。
//Publishers:
ros::Publisher acceleration_publisher;
ros::Publisher attitude_quaternion_publisher;
ros::Publisher compass_publisher;
ros::Publisher flight_control_info_publisher;
ros::Publisher flight_status_publisher;
ros::Publisher gimbal_publisher;
ros::Publisher global_position_publisher;
ros::Publisher local_position_publisher;
ros::Publisher power_status_publisher;
ros::Publisher rc_channels_publisher;
ros::Publisher velocity_publisher;
ros::Publisher activation_publisher;
ros::Publisher odometry_publisher;
ros::Publisher sdk_permission_publisher;
//构造发布者
void init_publishers(ros::NodeHandle\& nh)
{
// start ros publisher
acceleration_publisher = nh.advertiseAcceleration>("dji_sdk/acceleration", 10);
//Creating a handle to publish messages to a topic is done using the ros::NodeHandle class.
attitude_quaternion_publisher = nh.advertiseAttitudeQuaternion>("dji_sdk/attitude_quaternion", 10);
compass_publisher = nh.advertiseCompass>("dji_sdk/compass", 10);
flight_control_info_publisher = nh.advertiseFlightControlInfo>("dji_sdk/flight_control_info", 10);
flight_status_publisher = nh.advertiseUInt8>("dji_sdk/flight_status", 10);
gimbal_publisher = nh.advertiseGimbal>("dji_sdk/gimbal", 10);
global_position_publisher = nh.advertiseGlobalPosition>("dji_sdk/global_position", 10);
local_position_publisher = nh.advertiseLocalPosition>("dji_sdk/local_position", 10);
power_status_publisher = nh.advertisePowerStatus>("dji_sdk/power_status", 10);
rc_channels_publisher = nh.advertiseRCChannels>("dji_sdk/rc_channels", 10);
velocity_publisher = nh.advertiseVelocity>("dji_sdk/velocity", 10);
activation_publisher = nh.advertiseUInt8>("dji_sdk/activation", 10);
odometry_publisher = nh.advertiseOdometry>("dji_sdk/odometry",10);
sdk_permission_publisher = nh.advertiseUInt8>("dji_sdk/sdk_permission", 10);
}
//Services:
ros::ServiceServer attitude_control_service;
ros::ServiceServer camera_action_control_service;
ros::ServiceServer drone_task_control_service;
ros::ServiceServer gimbal_angle_control_service;
ros::ServiceServer gimbal_speed_control_service;
ros::ServiceServer global_position_control_service;
ros::ServiceServer local_position_control_service;
ros::ServiceServer sdk_permission_control_service;
ros::ServiceServer velocity_control_service;
bool attitude_control_callback(dji_sdk::AttitudeControl::Request\& request, dji_sdk::AttitudeControl::Response\& response);
bool camera_action_control_callback(dji_sdk::CameraActionControl::Request\& request, dji_sdk::CameraActionControl::Response\& response);
bool drone_task_control_callback(dji_sdk::DroneTaskControl::Request\& request, dji_sdk::DroneTaskControl::Response\& response);
bool gimbal_angle_control_callback(dji_sdk::GimbalAngleControl::Request\& request, dji_sdk::GimbalAngleControl::Response\& response);
bool gimbal_speed_control_callback(dji_sdk::GimbalSpeedControl::Request\& request, dji_sdk::GimbalSpeedControl::Response\& response);
bool global_position_control_callback(dji_sdk::GlobalPositionControl::Request\& request, dji_sdk::GlobalPositionControl::Response\& response);
bool local_position_control_callback(dji_sdk::LocalPositionControl::Request\& request, dji_sdk::LocalPositionControl::Response\& response);
bool sdk_permission_control_callback(dji_sdk::SDKPermissionControl::Request\& request, dji_sdk::SDKPermissionControl::Response\& response);
bool velocity_control_callback(dji_sdk::VelocityControl::Request\& request, dji_sdk::VelocityControl::Response\& response);
void init_services(ros::NodeHandle\& nh)
{
attitude_control_service = nh.advertiseService("dji_sdk/attitude_control", \&DJISDKNode::attitude_control_callback, this);
camera_action_control_service = nh.advertiseService("dji_sdk/camera_action_control",\&DJISDKNode::camera_action_control_callback, this);
drone_task_control_service = nh.advertiseService("dji_sdk/drone_task_control", \&DJISDKNode::drone_task_control_callback, this);
gimbal_angle_control_service = nh.advertiseService("dji_sdk/gimbal_angle_control", \&DJISDKNode::gimbal_angle_control_callback, this);
gimbal_speed_control_service = nh.advertiseService("dji_sdk/gimbal_speed_control", \&DJISDKNode::gimbal_speed_control_callback, this);
global_position_control_service = nh.advertiseService("dji_sdk/global_position_control", \&DJISDKNode::global_position_control_callback, this);
local_position_control_service = nh.advertiseService("dji_sdk/local_position_control", \&DJISDKNode::local_position_control_callback, this);
sdk_permission_control_service = nh.advertiseService("dji_sdk/sdk_permission_control", \&DJISDKNode::sdk_permission_control_callback, this);
velocity_control_service = nh.advertiseService("dji_sdk/velocity_control", \&DJISDKNode::velocity_control_callback, this);
}
//Actions:
typedef actionlib::SimpleActionServerDroneTaskAction> DroneTaskActionServer;
typedef actionlib::SimpleActionServerLocalPositionNavigationAction> LocalPositionNavigationActionServer;
typedef actionlib::SimpleActionServerGlobalPositionNavigationAction> GlobalPositionNavigationActionServer;
typedef actionlib::SimpleActionServerWaypointNavigationAction> WaypointNavigationActionServer;
DroneTaskActionServer* drone_task_action_server;
LocalPositionNavigationActionServer* local_position_navigation_action_server;
GlobalPositionNavigationActionServer* global_position_navigation_action_server;
WaypointNavigationActionServer* waypoint_navigation_action_server;
dji_sdk::DroneTaskFeedback drone_task_feedback;
dji_sdk::DroneTaskResult drone_task_result;
dji_sdk::LocalPositionNavigationFeedback local_position_navigation_feedback;
dji_sdk::LocalPositionNavigationResult local_position_navigation_result;
dji_sdk::GlobalPositionNavigationFeedback global_position_navigation_feedback;
dji_sdk::GlobalPositionNavigationResult global_position_navigation_result;
dji_sdk::WaypointNavigationFeedback waypoint_navigation_feedback;
dji_sdk::WaypointNavigationResult waypoint_navigation_result;
bool drone_task_action_callback(const dji_sdk::DroneTaskGoalConstPtr\& goal);
bool local_position_navigation_action_callback(const dji_sdk::LocalPositionNavigationGoalConstPtr\& goal);
bool global_position_navigation_action_callback(const dji_sdk::GlobalPositionNavigationGoalConstPtr\& goal);
bool waypoint_navigation_action_callback(const dji_sdk::WaypointNavigationGoalConstPtr\& goal);
void init_actions(ros::NodeHandle\& nh)
{
drone_task_action_server = new DroneTaskActionServer(nh,
"dji_sdk/drone_task_action",
boost::bind(\&DJISDKNode::drone_task_action_callback, this, _1), false);
drone_task_action_server->start();
local_position_navigation_action_server = new LocalPositionNavigationActionServer(nh,
"dji_sdk/local_position_navigation_action",
boost::bind(\&DJISDKNode::local_position_navigation_action_callback, this, _1), false);
local_position_navigation_action_server->start();
global_position_navigation_action_server = new GlobalPositionNavigationActionServer(nh,
"dji_sdk/global_position_navigation_action",
boost::bind(\&DJISDKNode::global_position_navigation_action_callback, this, _1), false );
global_position_navigation_action_server->start();
waypoint_navigation_action_server = new WaypointNavigationActionServer(nh,
"dji_sdk/waypoint_navigation_action",
boost::bind(\&DJISDKNode::waypoint_navigation_action_callback, this, _1), false);
waypoint_navigation_action_server->start();
}
public:
DJISDKNode(ros::NodeHandle\& nh, ros::NodeHandle\& nh_private);
private:
int init_parameters_and_activate(ros::NodeHandle\& nh_private);
void broadcast_callback();
bool process_waypoint(dji_sdk::Waypoint new_waypoint);
inline void gps_convert_ned(float \&ned_x, float \&ned_y,
double gps_t_lon, double gps_t_lat,
double gps_r_lon, double gps_r_lat);
dji_sdk::LocalPosition gps_convert_ned(dji_sdk::GlobalPosition loc);
};