From 5e6f683f6e55965a930b87cb0fccf3168a535c91 Mon Sep 17 00:00:00 2001 From: Zhiyuan Li Date: Fri, 8 Sep 2017 17:42:06 -0700 Subject: [PATCH] Local position topic and bug fixes: closes #121, closes #125, closes #126, closes #133 --- ReadMe.md | 47 +-- dji_sdk/CMakeLists.txt | 1 + dji_sdk/include/dji_sdk/dji_sdk_node.h | 20 + dji_sdk/msg/MissionHotpointTask.msg | 10 +- dji_sdk/msg/MissionWaypoint.msg | 4 +- dji_sdk/msg/MissionWaypointAction.msg | 7 +- dji_sdk/src/modules/dji_sdk_node.cpp | 20 +- .../modules/dji_sdk_node_mission_services.cpp | 31 +- .../src/modules/dji_sdk_node_publisher.cpp | 42 ++- dji_sdk/src/modules/dji_sdk_node_services.cpp | 23 ++ dji_sdk/srv/SetLocalPosRef.srv | 6 + dji_sdk_demo/CMakeLists.txt | 8 + .../demo_local_position_control.h | 67 ++++ dji_sdk_demo/src/demo_flight_control.cpp | 25 +- .../src/demo_local_position_control.cpp | 346 ++++++++++++++++++ 15 files changed, 612 insertions(+), 45 deletions(-) create mode 100644 dji_sdk/srv/SetLocalPosRef.srv create mode 100644 dji_sdk_demo/include/dji_sdk_demo/demo_local_position_control.h create mode 100644 dji_sdk_demo/src/demo_local_position_control.cpp diff --git a/ReadMe.md b/ReadMe.md index 30da5d89..2f0e636c 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -1,6 +1,8 @@ -# DJI Onboard SDK ROS 3.3.1 +# DJI Onboard SDK ROS 3.3 -**Version 3.3.1 was developed to support N3/A3 FW 1.7.5 and above, and is backword compatible with M100 FW 1.3.1.** +## Latest Update + +The latest update on Sep 8 2017 brought back the local position topic in Cartesian frame (ENU) and addressed several issues reported on github. The `dji_sdk/SetLocalPosRef` service and the `dji_sdk/local_position` topic were added. ## Quick Start Guide @@ -10,47 +12,50 @@ The ROS package is a full rewrite of DJI Onboard SDK ROS, with changes in APIs, ROS Wiki can be found [here](http://wiki.ros.org/dji_sdk). Please be sure to read the [release notes](https://developer.dji.com/onboard-sdk/documentation/appendix/releaseNotes.html). +DJI Onboard SDK ROS 3.3 was developed to support N3/A3 FW 1.7.5 and above, and is backword compatible with M100 FW 1.3.1. + ## Firmware Compatibility -| Aircraft/FC | Firmware Package Version | Flight Controller Version | OSDK Version Support | +| Aircraft/FC | Firmware Package Version | Flight Controller Version | OSDK-ROS Branch | |--------------- |--------------------------|----------------------------|---------------------- | -| **A3/A3 Pro** | **1.7.1.5** | **3.2.36.8** | **OSDK 3.3.1 (Current)** | -| | 1.7.0.5 | 3.2.15.50 | OSDK 3.2 | -| | 1.7.0.0 | 3.2.15.37 | OSDK 3.2 | +| **A3/A3 Pro** | **1.7.1.5** | **3.2.36.8** | **OSDK-ROS 3.3 (Current)** | +| | 1.7.0.5 | 3.2.15.50 | OSDK-ROS 3.2 | +| | 1.7.0.0 | 3.2.15.37 | OSDK-ROS 3.2 | | | | | | -| **N3** | **1.7.1.5** | **3.2.36.8** | **OSDK 3.3.1 (Current)** | -| | 1.7.0.0 | 3.2.15.37 | OSDK 3.2 | +| **N3** | **1.7.1.5** | **3.2.36.8** | **OSDK-ROS 3.3 (Current)** | +| | 1.7.0.0 | 3.2.15.37 | OSDK-ROS 3.2 | | | | | | -| **M600/M600 Pro** | Coming soon! | Coming soon! | **OSDK 3.3.1 (Current)** | -| | 1.0.1.20 | 3.2.15.62 | OSDK 3.2 | -| | 1.0.0.80 | 3.2.15.00 | OSDK 3.2 | +| **M600/M600 Pro** | 1.0.1.60 | 3.2.41.5 | **OSDK-ROS 3.3 (Current)** | +| | 1.0.1.20 | 3.2.15.62 | OSDK-ROS 3.2 | +| | 1.0.0.80 | 3.2.15.00 | OSDK-ROS 3.2 | | | | | | -| **M100** | 1.3.1.0 | 3.1.10.0 | **OSDK 3.3.1 (Current),** | -| | | | and OSDK 3.2 | +| **M100** | 1.3.1.0 | 3.1.10.0 | **OSDK-ROS 3.3 (Current),** | +| | | | and OSDK-ROS 3.2 | ## Notes on differences between M100 and A3/N3 setup -Backward compatibility for M100 was brought in version 3.3.1. However, due to the limitations of the flight controller of M100, some new features such as hardware sync, MFIO, on demand telemetry data (subscription) are only supported by A3/N3, and some settings for M100 are different from those for A3/N3. +Onboard SDK ROS is backward compatible with M100. However, due to the limitations of the flight controller of M100, some new features such as hardware sync, MFIO, on demand telemetry data (subscription) are only supported by A3/N3, and some settings for M100 are different from those for A3/N3. -1. The DJI Assistant 2 for M100 and for A3/N3 are slighly different. Please download DJI Assistant 2 from the corresponding product webpage. +1. The DJI Assistant 2 for M100 and for A3/N3 are slighly **different**. Please download DJI Assistant 2 from the corresponding product webpage. -2. The DJI SDK ROS package requires different baud rate for M100 and A3/N3. For M100, set the baud rate to 230400 in DJI Assistant 2's "SDK" tab, and the sdk.launch file; while for A3/N3, use 921600. +2. The DJI SDK ROS package requires **different baud rate** for M100 and A3/N3. For M100, set the baud rate to 230400 in DJI Assistant 2's "SDK" tab, and the sdk.launch file; while for A3/N3, use 921600. -3. For M100, on the right side of the "SDK setting" tab of DJI Assistant 2, set the Data Type of ACC and GYRO to "Raw Data", and ALTI to "Data Fusion". The reason is that the raw data of acc and gyro are part of the /dji_sdk/imu message. +3. For M100, on the right side of the "SDK setting" tab of DJI Assistant 2, set the Data Type of ACC and GYRO to "Raw Data", and ALTI to "Data Fusion". The reason is that the raw data of acc and gyro are part of the `/dji_sdk/imu` message. -4. The flight_status enums for M100 and A3/N3 are different. See dji\_sdk.h for details and demo_flight_control for examples. +4. The `flight_status` enums for M100 and A3/N3 are different. See `dji_sdk.h` for details and `demo_flight_control` for examples. -5. Some topics are only available on A3/N3: display_mode, angular_velocity_fused, acceleration_ground_fused, trigger_time. +5. Some topics are only available on A3/N3: `display_mode`, `angular_velocity_fused`, `acceleration_ground_fused`, `trigger_time`. 6. The imu topic is published at 400Hz on A3/N3, and at 100Hz on M100. -7. Some services are only available on A3/N3: mfio_config, mfio_set_value, set_hardsyc +7. Some services are only available on A3/N3: `mfio_config`, `mfio_set_value`, `set_hardsyc` ## Support You can get support from DJI and the community with the following methods: -- Github Issues or [gitter.im](https://gitter.im/dji-sdk/Onboard-SDK) +- Report issue on github +- Email to dev@dji.com - [**DJI Forum**](http://forum.dev.dji.com/en) diff --git a/dji_sdk/CMakeLists.txt b/dji_sdk/CMakeLists.txt index 43655391..8c594543 100644 --- a/dji_sdk/CMakeLists.txt +++ b/dji_sdk/CMakeLists.txt @@ -67,6 +67,7 @@ add_service_files( CameraAction.srv DroneTaskControl.srv SDKControlAuthority.srv + SetLocalPosRef.srv MFIOConfig.srv MFIOSetValue.srv DroneArmControl.srv diff --git a/dji_sdk/include/dji_sdk/dji_sdk_node.h b/dji_sdk/include/dji_sdk/dji_sdk_node.h index 5344e7c5..ed1b08e5 100644 --- a/dji_sdk/include/dji_sdk/dji_sdk_node.h +++ b/dji_sdk/include/dji_sdk/dji_sdk_node.h @@ -59,6 +59,7 @@ #include #include #include +#include #include #include //! SDK library @@ -97,6 +98,7 @@ class DJISDKNode bool initFlightControl(ros::NodeHandle& nh); bool initSubscriber(ros::NodeHandle& nh); bool initPublisher(ros::NodeHandle& nh); + bool initActions(ros::NodeHandle& nh); bool initDataSubscribeFromFC(); void cleanUpSubscribeFromFC(); bool validateSerialDevice(LinuxSerialDevice* serialDevice); @@ -134,6 +136,10 @@ class DJISDKNode bool sdkCtrlAuthorityCallback( dji_sdk::SDKControlAuthority::Request& request, dji_sdk::SDKControlAuthority::Response& response); + + bool setLocalPosRefCallback( + dji_sdk::SetLocalPosRef::Request& request, + dji_sdk::SetLocalPosRef::Response& response); //! control service callbacks bool droneArmCallback(dji_sdk::DroneArmControl::Request& request, dji_sdk::DroneArmControl::Response& response); @@ -253,6 +259,8 @@ class DJISDKNode ros::ServiceServer set_hardsync_server; //! Query FW version of FC ros::ServiceServer query_version_server; + //! Set Local position reference + ros::ServiceServer local_pos_ref_server; //! flight control subscribers ros::Subscriber flight_control_sub; @@ -280,6 +288,8 @@ class DJISDKNode ros::Publisher gimbal_angle_publisher; ros::Publisher displaymode_publisher; ros::Publisher rc_publisher; + //! Local Position Publisher (Publishes local position in ENU frame) + ros::Publisher local_position_publisher; //! constant const int WAIT_TIMEOUT = 10; @@ -321,9 +331,19 @@ class DJISDKNode bool align_time_with_FC; + bool local_pos_ref_set; + void alignRosTimeWithFlightController(ros::Time now_time, uint32_t tick); void setUpM100DefaultFreq(uint8_t freq[16]); void setUpA3N3DefaultFreq(uint8_t freq[16]); + void gpsConvertENU(double &ENU_x, double &ENU_y, + double gps_t_lon, double gps_t_lat, + double gps_r_lon, double gps_r_lat); + + + double local_pos_ref_latitude, local_pos_ref_longitude, local_pos_ref_altitude; + double current_gps_latitude, current_gps_longitude, current_gps_altitude; + int current_gps_health; }; #endif // DJI_SDK_NODE_MAIN_H diff --git a/dji_sdk/msg/MissionHotpointTask.msg b/dji_sdk/msg/MissionHotpointTask.msg index 41ca31b4..73cae59b 100644 --- a/dji_sdk/msg/MissionHotpointTask.msg +++ b/dji_sdk/msg/MissionHotpointTask.msg @@ -1,8 +1,8 @@ -float64 latitude -float64 longitude -float64 altitude -float64 radius -float32 angular_speed +float64 latitude # degree +float64 longitude # degree +float64 altitude # meter +float64 radius # meter +float32 angular_speed #deg/s uint8 is_clockwise uint8 start_point uint8 yaw_mode diff --git a/dji_sdk/msg/MissionWaypoint.msg b/dji_sdk/msg/MissionWaypoint.msg index af75b15f..00941b8e 100644 --- a/dji_sdk/msg/MissionWaypoint.msg +++ b/dji_sdk/msg/MissionWaypoint.msg @@ -1,5 +1,5 @@ -float64 latitude # radian -float64 longitude # radian +float64 latitude # degree +float64 longitude # degree float32 altitude # relative altitude from takeoff point float32 damping_distance # Bend length (effective coordinated turn mode only) int16 target_yaw # Yaw (degree) diff --git a/dji_sdk/msg/MissionWaypointAction.msg b/dji_sdk/msg/MissionWaypointAction.msg index 8a4bc7ad..ebfdf2ae 100644 --- a/dji_sdk/msg/MissionWaypointAction.msg +++ b/dji_sdk/msg/MissionWaypointAction.msg @@ -1,5 +1,6 @@ -# first 4 bit: Total number of actions -# last 4 bit: Total running times +# action_repeat +# lower 4 bit: Total number of actions +# hight 4 bit: Total running times uint8 action_repeat uint8[16] command_list -int16[16] command_parameter +uint16[16] command_parameter diff --git a/dji_sdk/src/modules/dji_sdk_node.cpp b/dji_sdk/src/modules/dji_sdk_node.cpp index 34516300..87ec7253 100644 --- a/dji_sdk/src/modules/dji_sdk_node.cpp +++ b/dji_sdk/src/modules/dji_sdk_node.cpp @@ -29,6 +29,10 @@ DJISDKNode::DJISDKNode(ros::NodeHandle& nh, ros::NodeHandle& nh_private) nh_private.param("align_time", align_time_with_FC, true); nh_private.param("use_broadcast", user_select_BC, false); + //! Default values for local Position + local_pos_ref_latitude = local_pos_ref_longitude = local_pos_ref_altitude = 0; + local_pos_ref_set = false; + // @todo need some error handling for init functions //! @note parsing launch file to get environment parameters if (!initVehicle(nh_private)) @@ -147,6 +151,7 @@ bool DJISDKNode::initServices(ros::NodeHandle& nh) { mission_status_server = nh.advertiseService("dji_sdk/mission_status", &DJISDKNode::missionStatusCallback, this); send_to_mobile_server = nh.advertiseService("dji_sdk/send_data_to_mobile", &DJISDKNode::sendToMobileCallback, this); query_version_server = nh.advertiseService("dji_sdk/query_drone_version", &DJISDKNode::queryVersionCallback, this); + local_pos_ref_server = nh.advertiseService("dji_sdk/set_local_pos_ref", &DJISDKNode::setLocalPosRefCallback, this); // A3/N3 only if(!isM100()) @@ -270,6 +275,9 @@ DJISDKNode::initPublisher(ros::NodeHandle& nh) gimbal_angle_publisher = nh.advertise("dji_sdk/gimbal_angle", 10); + local_position_publisher = + nh.advertise("dji_sdk/local_position", 10); + if (telemetry_from_fc == USE_BROADCAST) { ACK::ErrorCode broadcast_set_freq_ack; @@ -308,7 +316,7 @@ DJISDKNode::initPublisher(ros::NodeHandle& nh) } else { - ROS_INFO("align_time_with_FC set to false. We will time stamp messages based on flight controller time!"); + ROS_INFO("align_time_with_FC set to true. We will time stamp messages based on flight controller time!"); } // Extra topics that is only available from subscription @@ -517,3 +525,13 @@ DJISDKNode::setUpA3N3DefaultFreq(uint8_t freq[16]) freq[12] = DataBroadcast::FREQ_10HZ; freq[13] = DataBroadcast::FREQ_10HZ; } + +void DJISDKNode::gpsConvertENU(double &ENU_x, double &ENU_y, + double gps_t_lon, double gps_t_lat, + double gps_r_lon, double gps_r_lat) +{ + double d_lon = gps_t_lon - gps_r_lon; + double d_lat = gps_t_lat - gps_r_lat; + ENU_y = DEG2RAD(d_lat) * C_EARTH; + ENU_x = DEG2RAD(d_lon) * C_EARTH * cos(DEG2RAD(gps_t_lat)); +}; diff --git a/dji_sdk/src/modules/dji_sdk_node_mission_services.cpp b/dji_sdk/src/modules/dji_sdk_node_mission_services.cpp index a13e8b00..29f9e72c 100644 --- a/dji_sdk/src/modules/dji_sdk_node_mission_services.cpp +++ b/dji_sdk/src/modules/dji_sdk_node_mission_services.cpp @@ -74,7 +74,7 @@ DJISDKNode::missionWpUploadCallback( int i = 0; for (auto waypoint : request.waypoint_task.mission_waypoint) { - wpData.latitude = waypoint.latitude * C_PI / 180; + wpData.latitude = waypoint.latitude * C_PI / 180; wpData.longitude = waypoint.longitude * C_PI / 180; wpData.altitude = waypoint.altitude; wpData.damping = waypoint.damping_distance; @@ -251,6 +251,7 @@ DJISDKNode::missionWpGetInfoCallback( else { ROS_ERROR("no waypoint mission initiated "); + return false; } response.waypoint_task.mission_waypoint.resize(info.indexNumber); @@ -263,6 +264,30 @@ DJISDKNode::missionWpGetInfoCallback( response.waypoint_task.action_on_rc_lost = info.RCLostAction; response.waypoint_task.gimbal_pitch_mode = info.gimbalPitch; + for (int i=0; i< info.indexNumber; i++) + { + DJI::OSDK::WayPointSettings wpData; + wpData = vehicle->missionManager->wpMission->getIndex(i, 10).data; + response.waypoint_task.mission_waypoint[i].latitude = wpData.latitude * 180.0 / C_PI; + response.waypoint_task.mission_waypoint[i].longitude = wpData.longitude * 180.0 / C_PI; + response.waypoint_task.mission_waypoint[i].altitude = wpData.altitude; + response.waypoint_task.mission_waypoint[i].damping_distance = wpData.damping; + response.waypoint_task.mission_waypoint[i].target_yaw = wpData.yaw; + response.waypoint_task.mission_waypoint[i].target_gimbal_pitch = wpData.gimbalPitch; + response.waypoint_task.mission_waypoint[i].turn_mode = wpData.turnMode; + response.waypoint_task.mission_waypoint[i].has_action = wpData.hasAction; + response.waypoint_task.mission_waypoint[i].action_time_limit = wpData.actionTimeLimit; + response.waypoint_task.mission_waypoint[i].waypoint_action.action_repeat = wpData.actionNumber + (wpData.actionRepeat << 4); + + std::copy(std::begin(wpData.commandList), + std::end(wpData.commandList), + response.waypoint_task.mission_waypoint[i].waypoint_action.command_list.begin()); + + std::copy(std::begin(wpData.commandParameter), + std::end(wpData.commandParameter), + response.waypoint_task.mission_waypoint[i].waypoint_action.command_parameter.begin()); + + } return true; } @@ -364,8 +389,8 @@ DJISDKNode::missionHpGetInfoCallback( ROS_ERROR("no hotpoint mission initiated "); } - response.hotpoint_task.latitude = info.latitude; - response.hotpoint_task.longitude = info.longitude; + response.hotpoint_task.latitude = info.latitude * 180.0 / C_PI; + response.hotpoint_task.longitude = info.longitude * 180.0 / C_PI; response.hotpoint_task.altitude = info.height; response.hotpoint_task.radius = info.radius; response.hotpoint_task.angular_speed = info.yawRate; diff --git a/dji_sdk/src/modules/dji_sdk_node_publisher.cpp b/dji_sdk/src/modules/dji_sdk_node_publisher.cpp index 5e5de9ba..6f1f2a45 100644 --- a/dji_sdk/src/modules/dji_sdk_node_publisher.cpp +++ b/dji_sdk/src/modules/dji_sdk_node_publisher.cpp @@ -11,7 +11,6 @@ #include #include - #include #define _TICK2ROSTIME(tick) (ros::Duration((double)(tick) / 1000.0)) @@ -117,8 +116,29 @@ DJISDKNode::dataBroadcastCallback() gps_pos.latitude = global_pos.latitude * 180 / C_PI; gps_pos.longitude = global_pos.longitude * 180 / C_PI; gps_pos.altitude = global_pos.altitude; + this->current_gps_latitude = gps_pos.latitude; + this->current_gps_longitude = gps_pos.longitude; + this->current_gps_altitude = gps_pos.altitude; + this->current_gps_health = global_pos.health; gps_position_publisher.publish(gps_pos); + if(local_pos_ref_set) + { + geometry_msgs::PointStamped local_pos; + local_pos.header.frame_id = "/local"; + local_pos.header.stamp = gps_pos.header.stamp; + gpsConvertENU(local_pos.point.x, local_pos.point.y, gps_pos.longitude, + gps_pos.latitude, this->local_pos_ref_longitude, this->local_pos_ref_latitude); + local_pos.point.z = gps_pos.altitude - this->local_pos_ref_altitude; + /*! + * note: We are now following REP 103 to use ENU for + * short-range Cartesian representations. Local position is published + * in ENU Frame + */ + + this->local_position_publisher.publish(local_pos); + } + std_msgs::Float32 agl_height; agl_height.data = global_pos.height; height_publisher.publish(agl_height); @@ -271,8 +291,27 @@ DJISDKNode::publish50HzData(Vehicle* vehicle, RecvContainer recvFrame, gps_pos.latitude = fused_gps.latitude * 180.0 / C_PI; //degree gps_pos.longitude = fused_gps.longitude * 180.0 / C_PI; //degree gps_pos.altitude = fused_gps.altitude; //meter + p->current_gps_latitude = gps_pos.latitude; + p->current_gps_longitude = gps_pos.longitude; + p->current_gps_altitude = gps_pos.altitude; p->gps_position_publisher.publish(gps_pos); + if(p->local_pos_ref_set) + { + geometry_msgs::PointStamped local_pos; + local_pos.header.frame_id = "/local"; + local_pos.header.stamp = gps_pos.header.stamp; + p->gpsConvertENU(local_pos.point.x, local_pos.point.y, gps_pos.longitude, + gps_pos.latitude, p->local_pos_ref_longitude, p->local_pos_ref_latitude); + local_pos.point.z = gps_pos.altitude - p->local_pos_ref_altitude; + /*! + * note: We are now following REP 103 to use ENU for + * short-range Cartesian representations. Local position is published + * in ENU Frame + */ + p->local_position_publisher.publish(local_pos); + } + Telemetry::TypeMap::type fused_height = vehicle->subscribe->getValue(); std_msgs::Float32 height; @@ -307,6 +346,7 @@ DJISDKNode::publish50HzData(Vehicle* vehicle, RecvContainer recvFrame, vehicle->subscribe->getValue(); std_msgs::UInt8 msg_gps_ctrl_level; msg_gps_ctrl_level.data = gps_ctrl_level; + p->current_gps_health = gps_ctrl_level; p->gps_health_publisher.publish(msg_gps_ctrl_level); Telemetry::TypeMap::type gimbal_angle = diff --git a/dji_sdk/src/modules/dji_sdk_node_services.cpp b/dji_sdk/src/modules/dji_sdk_node_services.cpp index 78ec588a..47a5071b 100644 --- a/dji_sdk/src/modules/dji_sdk_node_services.cpp +++ b/dji_sdk/src/modules/dji_sdk_node_services.cpp @@ -121,6 +121,29 @@ DJISDKNode::sdkCtrlAuthorityCallback( return true; } +bool +DJISDKNode::setLocalPosRefCallback(dji_sdk::SetLocalPosRef::Request &request, + dji_sdk::SetLocalPosRef::Response &response) { + printf("Currrent GPS health is %d \n",current_gps_health ); + if (current_gps_health > 3) + { + local_pos_ref_latitude = current_gps_latitude; + local_pos_ref_longitude = current_gps_longitude; + local_pos_ref_altitude = current_gps_altitude; + ROS_INFO("Local Position reference has been set."); + ROS_INFO("MONITOR GPS HEALTH WHEN USING THIS TOPIC"); + local_pos_ref_set = true; + return true; + } + else + { + ROS_INFO("Not enough GPS Satellites. "); + ROS_INFO("Cannot set Local Position reference"); + local_pos_ref_set = false; + return false; + } +} + bool DJISDKNode::droneTaskCallback(dji_sdk::DroneTaskControl::Request& request, dji_sdk::DroneTaskControl::Response& response) diff --git a/dji_sdk/srv/SetLocalPosRef.srv b/dji_sdk/srv/SetLocalPosRef.srv new file mode 100644 index 00000000..06b35c67 --- /dev/null +++ b/dji_sdk/srv/SetLocalPosRef.srv @@ -0,0 +1,6 @@ +#set local position +# true: set Local position reference + +#request +--- +bool result diff --git a/dji_sdk_demo/CMakeLists.txt b/dji_sdk_demo/CMakeLists.txt index b1aef9e9..c34c06e4 100644 --- a/dji_sdk_demo/CMakeLists.txt +++ b/dji_sdk_demo/CMakeLists.txt @@ -184,6 +184,14 @@ target_link_libraries(demo_mobile_comm ${DJIOSDK_LIBRARIES} ) add_dependencies(demo_mobile_comm dji_sdk_generate_messages_cpp) + +add_executable(demo_local_position_control + src/demo_local_position_control.cpp) +target_link_libraries(demo_local_position_control + ${catkin_LIBRARIES} + ${DJIOSDK_LIBRARIES} + ) +add_dependencies(demo_local_position_control dji_sdk_generate_messages_cpp) ############# ## Install ## ############# diff --git a/dji_sdk_demo/include/dji_sdk_demo/demo_local_position_control.h b/dji_sdk_demo/include/dji_sdk_demo/demo_local_position_control.h new file mode 100644 index 00000000..2312ec7d --- /dev/null +++ b/dji_sdk_demo/include/dji_sdk_demo/demo_local_position_control.h @@ -0,0 +1,67 @@ +/** @file demo_local_position_control.h + * @version 3.3 + * @date September, 2017 + * + * @brief + * demo sample of how to use local position control APIs + * + * @copyright 2017 DJI. All rights reserved. + * + */ + +#ifndef PROJECT_DEMO_LOCAL_POSITION_CONTROL_H +#define PROJECT_DEMO_LOCAL_POSITION_CONTROL_H + +#endif //PROJECT_DEMO_LOCAL_POSITION_CONTROL_H + +#include +#include +#include +#include +#include +#include +//DJI SDK includes +#include +#include +#include + + +bool set_local_position(); + +float target_offset_x; +float target_offset_y; +float target_offset_z; +float target_yaw; +int target_set_state = 0; + +void setTarget(float x, float y, float z, float yaw) +{ + target_offset_x = x; + target_offset_y = y; + target_offset_z = z; + target_yaw = yaw; +} + +void local_position_callback(const geometry_msgs::PointStamped::ConstPtr& msg); + +void display_mode_callback(const std_msgs::UInt8::ConstPtr& msg); + +void flight_status_callback(const std_msgs::UInt8::ConstPtr& msg); + +void gps_position_callback(const sensor_msgs::NavSatFix::ConstPtr& msg); + +void gps_health_callback(const std_msgs::UInt8::ConstPtr& msg); + +bool takeoff_land(int task); + +bool obtain_control(); + +bool is_M100(); + +bool monitoredTakeoff(); + +bool M100monitoredTakeoff(); + +void local_position_ctrl(double &xCmd, double &yCmd, double &zCmd); + + diff --git a/dji_sdk_demo/src/demo_flight_control.cpp b/dji_sdk_demo/src/demo_flight_control.cpp index f0d5923d..b1b80858 100644 --- a/dji_sdk_demo/src/demo_flight_control.cpp +++ b/dji_sdk_demo/src/demo_flight_control.cpp @@ -20,7 +20,7 @@ ros::ServiceClient drone_task_service; ros::ServiceClient query_version_service; ros::Publisher ctrlPosYawPub; -ros::Publisher ctrlVelYawRatePub; +ros::Publisher ctrlBrakePub; // global variables for subscribed topics uint8_t flight_status = 255; @@ -44,7 +44,12 @@ int main(int argc, char** argv) // Publish the control signal ctrlPosYawPub = nh.advertise("/dji_sdk/flight_control_setpoint_ENUposition_yaw", 10); - ctrlVelYawRatePub = nh.advertise("dji_sdk/flight_control_setpoint_generic", 10); + + // We could use dji_sdk/flight_control_setpoint_ENUvelocity_yawrate here, but + // we use dji_sdk/flight_control_setpoint_generic to demonstrate how to set the flag + // properly in function Mission::step() + 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"); @@ -158,28 +163,30 @@ void Mission::step() else if(break_counter > 0) { sensor_msgs::Joy controlVelYawRate; + uint8_t flag = (DJISDK::VERTICAL_VELOCITY | + DJISDK::HORIZONTAL_VELOCITY | + DJISDK::YAW_RATE | + DJISDK::HORIZONTAL_GROUND | + DJISDK::STABLE_ENABLE); controlVelYawRate.axes.push_back(0); controlVelYawRate.axes.push_back(0); controlVelYawRate.axes.push_back(0); controlVelYawRate.axes.push_back(0); - ctrlVelYawRatePub.publish(controlVelYawRate); + controlVelYawRate.axes.push_back(flag); + + ctrlBrakePub.publish(controlVelYawRate); break_counter++; return; } else //break_counter = 0, not in break stage { sensor_msgs::Joy controlPosYaw; - uint8_t flag = (DJISDK::VERTICAL_VELOCITY | - DJISDK::HORIZONTAL_VELOCITY | - DJISDK::YAW_RATE | - DJISDK::HORIZONTAL_GROUND | - DJISDK::STABLE_ENABLE); + controlPosYaw.axes.push_back(xCmd); controlPosYaw.axes.push_back(yCmd); controlPosYaw.axes.push_back(zCmd); controlPosYaw.axes.push_back(yawDesiredRad); - controlPosYaw.axes.push_back(flag); ctrlPosYawPub.publish(controlPosYaw); } diff --git a/dji_sdk_demo/src/demo_local_position_control.cpp b/dji_sdk_demo/src/demo_local_position_control.cpp new file mode 100644 index 00000000..88bb38a1 --- /dev/null +++ b/dji_sdk_demo/src/demo_local_position_control.cpp @@ -0,0 +1,346 @@ +/** @file demo_flight_control.cpp + * @version 3.3 + * @date September, 2017 + * + * @brief + * demo sample of how to use Local position control + * + * @copyright 2017 DJI. All rights reserved. + * + */ + +#include "dji_sdk_demo/demo_local_position_control.h" +#include "dji_sdk/dji_sdk.h" + +ros::ServiceClient set_local_pos_reference; +ros::ServiceClient sdk_ctrl_authority_service; +ros::ServiceClient drone_task_service; +ros::ServiceClient query_version_service; + +ros::Publisher ctrlPosYawPub; + +// global variables for subscribed topics +uint8_t flight_status = 255; +uint8_t display_mode = 255; +uint8_t current_gps_health = 0; +int num_targets = 0; +geometry_msgs::PointStamped local_position; +sensor_msgs::NavSatFix current_gps_position; + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "demo_local_position_control_node"); + ros::NodeHandle nh; + // Subscribe to messages from dji_sdk_node + 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); + ros::Subscriber localPosition = nh.subscribe("dji_sdk/local_position", 10, &local_position_callback); + ros::Subscriber gpsSub = nh.subscribe("dji_sdk/gps_position", 10, &gps_position_callback); + ros::Subscriber gpsHealth = nh.subscribe("dji_sdk/gps_health", 10, &gps_health_callback); + + // Publish the control signal + ctrlPosYawPub = nh.advertise("/dji_sdk/flight_control_setpoint_ENUposition_yaw", 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"); + set_local_pos_reference = nh.serviceClient ("dji_sdk/set_local_pos_ref"); + + bool obtain_control_result = obtain_control(); + bool takeoff_result; + set_local_position(); + if(is_M100()) + { + ROS_INFO("M100 taking off!"); + takeoff_result = M100monitoredTakeoff(); + } + else + { + ROS_INFO("A3/N3 taking off!"); + takeoff_result = monitoredTakeoff(); + } + + if(takeoff_result) + { + //! Enter total number of Targets + num_targets = 2; + //! Start Mission by setting Target state to 1 + target_set_state = 1; + } + + ros::spin(); + return 0; +} + +/*! + * This function is called when local position data is available. + * In the example below, we make use of two arbitrary targets as + * an example for local position control. + * + */ +void local_position_callback(const geometry_msgs::PointStamped::ConstPtr& msg) { + static ros::Time start_time = ros::Time::now(); + ros::Duration elapsed_time = ros::Time::now() - start_time; + local_position = *msg; + double xCmd, yCmd, zCmd; + sensor_msgs::Joy controlPosYaw; + + // Down sampled to 50Hz loop + if (elapsed_time > ros::Duration(0.02)) { + start_time = ros::Time::now(); + if (target_set_state == 1) { + //! First arbitrary target + if (current_gps_health > 3) { + setTarget(10, 15, 25, 2); + local_position_ctrl(xCmd, yCmd, zCmd); + } + else + { + ROS_INFO("Cannot execute Local Position Control"); + ROS_INFO("Not enough GPS Satellites"); + //! Set Target set state to 0 in order to stop Local position control mission + target_set_state = 0; + } + } + + if (target_set_state == 2) { + //! Second arbitrary target + if (current_gps_health > 3) { + setTarget(-10, 5, 5, 2); + local_position_ctrl(xCmd, yCmd, zCmd); + } + else + { + ROS_INFO("Cannot execute Local Position Control"); + ROS_INFO("Not enough GPS Satellites"); + //! Set Target set state to 0 in order to stop Local position control mission + target_set_state = 0; + } + } + } +} + +/*! + * This function calculates the difference between target and current local position + * and sends the commands to the Position and Yaw control topic. + * + */ +void local_position_ctrl(double &xCmd, double &yCmd, double &zCmd) +{ + xCmd = target_offset_x - local_position.point.x; + yCmd = target_offset_y - local_position.point.y; + zCmd = target_offset_z; + + sensor_msgs::Joy controlPosYaw; + controlPosYaw.axes.push_back(xCmd); + controlPosYaw.axes.push_back(yCmd); + controlPosYaw.axes.push_back(zCmd); + controlPosYaw.axes.push_back(target_yaw); + ctrlPosYawPub.publish(controlPosYaw); + + // 0.1m or 10cms is the minimum error to reach target in x y and z axes. + // This error threshold will have to change depending on aircraft/payload/wind conditions. + if (((std::abs(xCmd)) < 0.1) && ((std::abs(yCmd)) < 0.1) && + (local_position.point.z > (target_offset_z - 0.1)) && (local_position.point.z < (target_offset_z + 0.1))) { + if(target_set_state <= num_targets) { + ROS_INFO("%d of %d target(s) complete", target_set_state, num_targets); + target_set_state++; + } + else + { + target_set_state = 0; + } + } +} + +bool takeoff_land(int task) +{ + dji_sdk::DroneTaskControl droneTaskControl; + + droneTaskControl.request.task = task; + + drone_task_service.call(droneTaskControl); + + if(!droneTaskControl.response.result) + { + ROS_ERROR("takeoff_land fail"); + return false; + } + + return true; +} + +bool obtain_control() +{ + dji_sdk::SDKControlAuthority authority; + authority.request.control_enable=1; + sdk_ctrl_authority_service.call(authority); + + if(!authority.response.result) + { + ROS_ERROR("obtain control failed!"); + return false; + } + + return true; +} + +bool is_M100() +{ + dji_sdk::QueryDroneVersion query; + query_version_service.call(query); + + if(query.response.version == DJISDK::DroneFirmwareVersion::M100_31) + { + return true; + } + + return false; +} + + +void gps_position_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) { + current_gps_position = *msg; +} + +void gps_health_callback(const std_msgs::UInt8::ConstPtr& msg) { + current_gps_health = msg->data; +} + +void flight_status_callback(const std_msgs::UInt8::ConstPtr& msg) +{ + flight_status = msg->data; +} + +void display_mode_callback(const std_msgs::UInt8::ConstPtr& msg) +{ + display_mode = msg->data; +} + + +/*! + * This function demos how to use the flight_status + * and the more detailed display_mode (only for A3/N3) + * to monitor the take off process with some error + * handling. Note M100 flight status is different + * from A3/N3 flight status. + */ +bool +monitoredTakeoff() +{ + ros::Time start_time = ros::Time::now(); + + if(!takeoff_land(dji_sdk::DroneTaskControl::Request::TASK_TAKEOFF)) { + return false; + } + + ros::Duration(0.01).sleep(); + ros::spinOnce(); + + // Step 1.1: Spin the motor + while (flight_status != DJISDK::FlightStatus::STATUS_ON_GROUND && + display_mode != DJISDK::DisplayMode::MODE_ENGINE_START && + ros::Time::now() - start_time < ros::Duration(5)) { + ros::Duration(0.01).sleep(); + ros::spinOnce(); + } + + if(ros::Time::now() - start_time > ros::Duration(5)) { + ROS_ERROR("Takeoff failed. Motors are not spinnning."); + return false; + } + else { + start_time = ros::Time::now(); + ROS_INFO("Motor Spinning ..."); + ros::spinOnce(); + } + + + // Step 1.2: Get in to the air + while (flight_status != DJISDK::FlightStatus::STATUS_IN_AIR && + (display_mode != DJISDK::DisplayMode::MODE_ASSISTED_TAKEOFF || display_mode != DJISDK::DisplayMode::MODE_AUTO_TAKEOFF) && + ros::Time::now() - start_time < ros::Duration(20)) { + ros::Duration(0.01).sleep(); + ros::spinOnce(); + } + + if(ros::Time::now() - start_time > ros::Duration(20)) { + ROS_ERROR("Takeoff failed. Aircraft is still on the ground, but the motors are spinning."); + return false; + } + else { + start_time = ros::Time::now(); + ROS_INFO("Ascending..."); + ros::spinOnce(); + } + + // Final check: Finished takeoff + while ( (display_mode == DJISDK::DisplayMode::MODE_ASSISTED_TAKEOFF || display_mode == DJISDK::DisplayMode::MODE_AUTO_TAKEOFF) && + ros::Time::now() - start_time < ros::Duration(20)) { + ros::Duration(0.01).sleep(); + ros::spinOnce(); + } + + if ( display_mode != DJISDK::DisplayMode::MODE_P_GPS || display_mode != DJISDK::DisplayMode::MODE_ATTITUDE) + { + ROS_INFO("Successful takeoff!"); + start_time = ros::Time::now(); + } + else + { + ROS_ERROR("Takeoff finished, but the aircraft is in an unexpected mode. Please connect DJI GO."); + return false; + } + + return true; +} + + +/*! + * This function demos how to use M100 flight_status + * to monitor the take off process with some error + * handling. Note M100 flight status is different + * from A3/N3 flight status. + */ +bool +M100monitoredTakeoff() +{ + ros::Time start_time = ros::Time::now(); + + float home_altitude = current_gps_position.altitude; + if(!takeoff_land(dji_sdk::DroneTaskControl::Request::TASK_TAKEOFF)) + { + return false; + } + + ros::Duration(0.01).sleep(); + ros::spinOnce(); + + // Step 1: If M100 is not in the air after 10 seconds, fail. + while (ros::Time::now() - start_time < ros::Duration(10)) + { + ros::Duration(0.01).sleep(); + ros::spinOnce(); + } + + if(flight_status != DJISDK::M100FlightStatus::M100_STATUS_IN_AIR || + current_gps_position.altitude - home_altitude < 1.0) + { + ROS_ERROR("Takeoff failed."); + return false; + } + else + { + start_time = ros::Time::now(); + ROS_INFO("Successful takeoff!"); + ros::spinOnce(); + } + return true; +} + +bool set_local_position() +{ + dji_sdk::SetLocalPosRef localPosReferenceSetter; + set_local_pos_reference.call(localPosReferenceSetter); +}