diff --git a/contrib/External_CollisionAvoidance.cmake b/contrib/External_CollisionAvoidance.cmake new file mode 100644 index 00000000..a89044cf --- /dev/null +++ b/contrib/External_CollisionAvoidance.cmake @@ -0,0 +1,24 @@ +include(ExternalProject) +message( "External project - DJI Collision Avoidance" ) + +set(VERSION "1.0.0") +set(PROJECT_NAME dji-ros-collision-avoidance) +set(PKG_NAME collision-avoidance) + +ExternalProject_Add(${PROJECT_NAME} + UPDATE_COMMAND "" + GIT_REPOSITORY https://github.com/dji-sdk/Onboard-SDK-Resources.git + GIT_TAG ${PKG_NAME}-${VERSION} + PATCH_COMMAND "" + ) + +ExternalProject_Get_Property(${PROJECT_NAME} source_dir) + +add_custom_command(TARGET ${PROJECT_NAME} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_CURRENT_BINARY_DIR}/../../../${PROJECT_NAME}) + +add_custom_command( TARGET ${PROJECT_NAME} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E tar xzf ${source_dir}/${PKG_NAME}-${VERSION}.tar.gz + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/../../../${PROJECT_NAME} + ) + diff --git a/contrib/External_GTest.cmake b/contrib/External_GTest.cmake new file mode 100644 index 00000000..2ff3f27d --- /dev/null +++ b/contrib/External_GTest.cmake @@ -0,0 +1,20 @@ +include(ExternalProject) +message( "External project - GoogleTest" ) + +if(GTEST) + if (CMAKE_SYSTEM_NAME MATCHES Linux) + find_package(Threads) + ExternalProject_Add( + googletest + # Disable update step + UPDATE_COMMAND "" + GIT_REPOSITORY https://github.com/google/googletest.git + CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs + -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs + -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS} + -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT} + -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS} + -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/../../../devel) + endif () +endif () + diff --git a/contrib/External_Pointcloud2Las.cmake b/contrib/External_Pointcloud2Las.cmake new file mode 100644 index 00000000..4c55567c --- /dev/null +++ b/contrib/External_Pointcloud2Las.cmake @@ -0,0 +1,24 @@ +include(ExternalProject) +message( "External project - DJI Pointcloud to LAS Converter" ) + +set(VERSION "1.0.0") +set(PROJECT_NAME dji-ros-pointcloud2las) +set(PKG_NAME pointcloud2las) + +ExternalProject_Add(${PROJECT_NAME} + UPDATE_COMMAND "" + GIT_REPOSITORY https://github.com/dji-sdk/Onboard-SDK-Resources.git + GIT_TAG ${PKG_NAME}-${VERSION} + PATCH_COMMAND "" + ) + +ExternalProject_Get_Property(${PROJECT_NAME} source_dir) + +add_custom_command(TARGET ${PROJECT_NAME} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E make_directory ${CMAKE_CURRENT_BINARY_DIR}/../../../${PROJECT_NAME}) + +add_custom_command( TARGET ${PROJECT_NAME} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E tar xzf ${source_dir}/${PKG_NAME}-${VERSION}.tar.gz + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/../../../${PROJECT_NAME} + ) + diff --git a/dji_sdk/CMakeLists.txt b/dji_sdk/CMakeLists.txt index addda095..41473ace 100644 --- a/dji_sdk/CMakeLists.txt +++ b/dji_sdk/CMakeLists.txt @@ -1,6 +1,12 @@ cmake_minimum_required(VERSION 2.8.3) project(dji_sdk) +option(GTEST "Build with Google Test framework" ON) + +# Add module path for .cmake conf files +set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/Onboard-SDK-ROS/contrib) +include(${CMAKE_MODULE_PATH}/External_GTest.cmake) + ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages @@ -19,7 +25,6 @@ find_package(catkin REQUIRED COMPONENTS ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) - set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed @@ -166,7 +171,7 @@ catkin_package( ## Your package locations should be listed before other locations # include_directories(include) include_directories( - include + include ${catkin_INCLUDE_DIRS} ) @@ -194,6 +199,54 @@ target_link_libraries(dji_sdk_node ${catkin_LIBRARIES} ) +############# +## Testing ## +############# +find_package(rostest REQUIRED) +find_package(GTest QUIET) + +if(GTEST_FOUND) + message("Found GTest on the system\n") +endif() + +# Create a gtest executable with target name "dji_sdk_test" +# which is not built by "make all" but only by "make tests" +# add one/multiple source files to the executable target +# register the rostest launch file "test/dji_sdk-node.test" +add_rostest_gtest(${PROJECT_NAME}_test + test/${PROJECT_NAME}-node.test + src/test/${PROJECT_NAME}_test.cpp + src/modules/dji_sdk_node_actions.cpp + src/modules/dji_sdk_node_services.cpp + src/modules/dji_sdk_node_main.cpp + src/modules/dji_sdk_node_mission.cpp) + +if(TARGET ${PROJECT_NAME}_test) + add_dependencies(${PROJECT_NAME}_test + dji_sdk_generate_messages_cpp + dji_sdk_lib) + + target_link_libraries(${PROJECT_NAME}_test + ${catkin_LIBRARIES} + dji_sdk_lib + gtest + gtest_main + pthread) +endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) + +option(USE_COLLISION_AVOIDANCE "Use DJI collision avoidance library" OFF) +if(USE_COLLISION_AVOIDANCE) + include(${CMAKE_MODULE_PATH}/External_CollisionAvoidance.cmake) +endif() + +option(USE_POINTCLOUD2LAS "Use DJI Pointcloud to LAS converter library" OFF) +if(USE_POINTCLOUD2LAS) + include(${CMAKE_MODULE_PATH}/External_Pointcloud2Las.cmake) +endif() + ############# ## Install ## ############# @@ -236,15 +289,4 @@ install(DIRECTORY launch # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_dji_sdk.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/dji_sdk/include/dji_sdk/dji_drone.h b/dji_sdk/include/dji_sdk/dji_drone.h index 4b37b99f..691f621e 100644 --- a/dji_sdk/include/dji_sdk/dji_drone.h +++ b/dji_sdk/include/dji_sdk/dji_drone.h @@ -35,14 +35,14 @@ class DJIDrone ros::ServiceClient activation_service; ros::ServiceClient attitude_control_service; - ros::ServiceClient camera_action_control_service; - ros::ServiceClient drone_task_control_service; - ros::ServiceClient gimbal_angle_control_service; - ros::ServiceClient gimbal_speed_control_service; - ros::ServiceClient global_position_control_service; - ros::ServiceClient local_position_control_service; - ros::ServiceClient sdk_permission_control_service; - ros::ServiceClient velocity_control_service; + ros::ServiceClient camera_action_control_service; + ros::ServiceClient drone_task_control_service; + ros::ServiceClient gimbal_angle_control_service; + ros::ServiceClient gimbal_speed_control_service; + ros::ServiceClient global_position_control_service; + ros::ServiceClient local_position_control_service; + ros::ServiceClient sdk_permission_control_service; + ros::ServiceClient velocity_control_service; ros::ServiceClient version_check_service; ros::ServiceClient virtual_rc_enable_control_service; @@ -66,19 +66,19 @@ class DJIDrone ros::ServiceClient mission_fm_set_target_service; //ros::ServiceClient mobile_commands_service; - ros::Subscriber acceleration_subscriber; - ros::Subscriber attitude_quaternion_subscriber; - ros::Subscriber compass_subscriber; - ros::Subscriber flight_control_info_subscriber; - ros::Subscriber flight_status_subscriber; - ros::Subscriber gimbal_subscriber; - ros::Subscriber global_position_subscriber; - ros::Subscriber local_position_subscriber; - ros::Subscriber power_status_subscriber; - ros::Subscriber rc_channels_subscriber; - ros::Subscriber velocity_subscriber; - ros::Subscriber activation_subscriber; - ros::Subscriber odometry_subscriber; + ros::Subscriber acceleration_subscriber; + ros::Subscriber attitude_quaternion_subscriber; + ros::Subscriber compass_subscriber; + ros::Subscriber flight_control_info_subscriber; + ros::Subscriber flight_status_subscriber; + ros::Subscriber gimbal_subscriber; + ros::Subscriber global_position_subscriber; + ros::Subscriber local_position_subscriber; + ros::Subscriber power_status_subscriber; + ros::Subscriber rc_channels_subscriber; + ros::Subscriber velocity_subscriber; + ros::Subscriber activation_subscriber; + ros::Subscriber odometry_subscriber; ros::Subscriber time_stamp_subscriber; ros::Subscriber mission_status_subscriber; @@ -87,20 +87,20 @@ class DJIDrone public: - dji_sdk::Acceleration acceleration; - dji_sdk::AttitudeQuaternion attitude_quaternion; - dji_sdk::Compass compass; - dji_sdk::FlightControlInfo flight_control_info; - uint8_t flight_status; - uint8_t mobile_new_data; - dji_sdk::Gimbal gimbal; - dji_sdk::GlobalPosition global_position; - dji_sdk::GlobalPosition global_position_ref; - dji_sdk::LocalPosition local_position; - dji_sdk::LocalPosition local_position_ref; - dji_sdk::PowerStatus power_status; - dji_sdk::TransparentTransmissionData mobile_data; - dji_sdk::RCChannels rc_channels; + dji_sdk::Acceleration acceleration; + dji_sdk::AttitudeQuaternion attitude_quaternion; + dji_sdk::Compass compass; + dji_sdk::FlightControlInfo flight_control_info; + uint8_t flight_status; + uint8_t mobile_new_data; + dji_sdk::Gimbal gimbal; + dji_sdk::GlobalPosition global_position; + dji_sdk::GlobalPosition global_position_ref; + dji_sdk::LocalPosition local_position; + dji_sdk::LocalPosition local_position_ref; + dji_sdk::PowerStatus power_status; + dji_sdk::TransparentTransmissionData mobile_data; + dji_sdk::RCChannels rc_channels; dji_sdk::Velocity velocity; nav_msgs::Odometry odometry; dji_sdk::TimeStamp time_stamp; @@ -257,152 +257,188 @@ typedef struct CallBackHandler CallBackHandler globalNavigationTestCallback; CallBackHandler virtualRCTestCallback; CallBackHandler gimbalControlDemoCallback; + CallBackHandler startMapLASLoggingCallback; + CallBackHandler stopMapLASLoggingCallback; + CallBackHandler startCollisionAvoidanceCallback; + CallBackHandler stopCollisionAvoidanceCallback; + void mobile_data_push_info_callback(dji_sdk::TransparentTransmissionData information) { this->mobile_data = information; mobile_new_data = 1; - int cmdID = mobile_data.data[0]; - printf("Command ID code is %d \n", cmdID); - - switch(cmdID) - { - case 2: - if (obtainControlCallback.callback) - { - obtainControlCallback.callback(this); - } - break; - - case 3: - if (releaseControlCallback.callback) - { - releaseControlCallback.callback(this); - } - break; - - case 4: - //if (obtainControlCallback.callback) - //{ - // obtainControlCallback.callback(); - //} - break; - - case 5: - if (armCallback.callback) - { - armCallback.callback(this); - } - break; - - case 6: - if (disArmCallback.callback) - { - disArmCallback.callback(this); - } - break; - - case 7: - if (takeOffCallback.callback) - { - takeOffCallback.callback(this); - } - break; - - case 8: - if (landingCallback.callback) - { - landingCallback.callback(this); - } - break; - - case 9: - if (goHomeCallback.callback) - { - goHomeCallback.callback(this); - } - break; - - case 10: - if (takePhotoCallback.callback) - { - takePhotoCallback.callback(this); - } - break; - - case 11: - if (startVideoCallback.callback) - { - startVideoCallback.callback(this); - } - break; - - case 13: - if (stopVideoCallback.callback) - { - stopVideoCallback.callback(this); - } - break; - - case 61: - if (drawCircleDemoCallback.callback) - { - drawCircleDemoCallback.callback(this); - } - break; - - - case 62: - if (drawSquareDemoCallback.callback) - { - drawSquareDemoCallback.callback(this); - } - break; - - case 63: - if (attitudeControlDemoCallback.callback) - { - attitudeControlDemoCallback.callback(this); - } - break; - - case 64: - if (gimbalControlDemoCallback.callback) - { - gimbalControlDemoCallback.callback(this); - } - break; - - case 65: - if (waypointNavigationTestCallback.callback) - { - waypointNavigationTestCallback.callback(this); - } - break; - - case 66: - if (localNavigationTestCallback.callback) - { - localNavigationTestCallback.callback(this); - } - break; - - case 67: - if (globalNavigationTestCallback.callback) - { - globalNavigationTestCallback.callback(this); - } - break; - - case 68: - if (virtualRCTestCallback.callback) - { - virtualRCTestCallback.callback(this); - } - break; - - - } + int cmdID = mobile_data.data[0]; + printf("Command ID code is %d \n", cmdID); + + switch(cmdID) + { + case 2: + if (obtainControlCallback.callback) + { + obtainControlCallback.callback(this); + } + break; + + case 3: + if (releaseControlCallback.callback) + { + releaseControlCallback.callback(this); + } + break; + + case 4: + //if (obtainControlCallback.callback) + //{ + // obtainControlCallback.callback(); + //} + break; + + case 5: + if (armCallback.callback) + { + armCallback.callback(this); + } + break; + + case 6: + if (disArmCallback.callback) + { + disArmCallback.callback(this); + } + break; + + case 7: + if (takeOffCallback.callback) + { + takeOffCallback.callback(this); + } + break; + + case 8: + if (landingCallback.callback) + { + landingCallback.callback(this); + } + break; + + case 9: + if (goHomeCallback.callback) + { + goHomeCallback.callback(this); + } + break; + + case 10: + if (takePhotoCallback.callback) + { + takePhotoCallback.callback(this); + } + break; + + case 11: + if (startVideoCallback.callback) + { + startVideoCallback.callback(this); + } + break; + + case 13: + if (stopVideoCallback.callback) + { + stopVideoCallback.callback(this); + } + break; + + case 20: + if (startMapLASLoggingCallback.callback) + { + startMapLASLoggingCallback.callback(this); + } + + break; + + case 21: + if (stopMapLASLoggingCallback.callback) + { + stopMapLASLoggingCallback.callback(this); + } + break; + + case 22: + if (startCollisionAvoidanceCallback.callback) + { + startCollisionAvoidanceCallback.callback(this); + } + + break; + + case 23: + if (stopCollisionAvoidanceCallback.callback) + { + stopCollisionAvoidanceCallback.callback(this); + } + break; + + + case 61: + if (drawCircleDemoCallback.callback) + { + drawCircleDemoCallback.callback(this); + } + break; + + + case 62: + if (drawSquareDemoCallback.callback) + { + drawSquareDemoCallback.callback(this); + } + break; + + case 63: + if (attitudeControlDemoCallback.callback) + { + attitudeControlDemoCallback.callback(this); + } + break; + + case 64: + if (gimbalControlDemoCallback.callback) + { + gimbalControlDemoCallback.callback(this); + } + break; + + case 65: + if (waypointNavigationTestCallback.callback) + { + waypointNavigationTestCallback.callback(this); + } + break; + + case 66: + if (localNavigationTestCallback.callback) + { + localNavigationTestCallback.callback(this); + } + break; + + case 67: + if (globalNavigationTestCallback.callback) + { + globalNavigationTestCallback.callback(this); + } + break; + + case 68: + if (virtualRCTestCallback.callback) + { + virtualRCTestCallback.callback(this); + } + break; + + + } } void mission_event_push_info_callback(dji_sdk::MissionPushInfo event_push_info) @@ -611,6 +647,30 @@ typedef struct CallBackHandler gimbalControlDemoCallback.userData = userData; } + void setStartMapLASLoggingMobileCallback(DJIDrone::CallBack userCallback, UserData userData) + { + startMapLASLoggingCallback.callback = userCallback; + startMapLASLoggingCallback.userData = userData; + } + + void setStopMapLASLoggingMobileCallback(DJIDrone::CallBack userCallback, UserData userData) + { + stopMapLASLoggingCallback.callback = userCallback; + stopMapLASLoggingCallback.userData = userData; + } + + void setStartCollisionAvoidanceCallback(DJIDrone::CallBack userCallback, UserData userData) + { + startCollisionAvoidanceCallback.callback = userCallback; + startCollisionAvoidanceCallback.userData = userData; + } + + void setStopCollisionAvoidanceCallback(DJIDrone::CallBack userCallback, UserData userData) + { + stopCollisionAvoidanceCallback.callback = userCallback; + stopCollisionAvoidanceCallback.userData = userData; + } + bool activate() { dji_sdk::Activation activate; diff --git a/dji_sdk/src/test/dji_sdk_test.cpp b/dji_sdk/src/test/dji_sdk_test.cpp new file mode 100644 index 00000000..3003482a --- /dev/null +++ b/dji_sdk/src/test/dji_sdk_test.cpp @@ -0,0 +1,64 @@ +#include "dji_sdk_test.h" + +void dji_sdk_node::SetUp() { + + ASSERT_TRUE(ros::master::check()) << "ROS master is not up\n"; + + rosAdapter = new DJI::onboardSDK::ROSAdapter; + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + djiSDKNode = new DJISDKNode(nh, nh_private); + + activation_publisher = nh.advertise("dji_sdk/activation", 10); + activation_subscriber = nh.subscribe("dji_sdk/activation", 10, &dji_sdk_node::activation_subscriber_callback, this); +} + +void dji_sdk_node::TearDown() { + + activation_subscriber.shutdown(); + activation_publisher.shutdown(); + + //TODO find a better way to clean +/* delete rosAdapter; + rosAdapter = NULL; + delete djiSDKNode; + djiSDKNode = NULL; +*/ +} + +TEST_F(dji_sdk_node, test01) { + + ros::Duration(1.0).sleep(); + ros::spinOnce(); + + /** + *@note Activation Error Codes + * + * Successful: 0 + * Parameter error: 1 + * Encode error: 2 + * New device: 3 + * App not connected: 4 + * No Internet: 5 + * Server refused: 6 + * Access level error: 7 + * Version error: 8 + */ + ASSERT_FALSE(activation); +} + +int main(int argc, char **argv) { + + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "dji_sdk"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + int ret = RUN_ALL_TESTS(); + + spinner.stop(); + ros::shutdown(); + + return ret; +} diff --git a/dji_sdk/src/test/dji_sdk_test.h b/dji_sdk/src/test/dji_sdk_test.h new file mode 100644 index 00000000..330fad81 --- /dev/null +++ b/dji_sdk/src/test/dji_sdk_test.h @@ -0,0 +1,26 @@ +#include "gtest/gtest.h" +#include +#include + +using namespace DJI::onboardSDK; + +DJI::onboardSDK::ROSAdapter *rosAdapter; +DJISDKNode *djiSDKNode; + +class dji_sdk_node : public testing::Test { + protected: + virtual void SetUp(); + virtual void TearDown(); + + ros::Subscriber activation_subscriber; + ros::Publisher activation_publisher; + + uint8_t activation; + + public: + void activation_subscriber_callback(std_msgs::UInt8 activation){ + this->activation = activation.data; + } + +}; + diff --git a/dji_sdk/test/dji_sdk-node.test b/dji_sdk/test/dji_sdk-node.test new file mode 100644 index 00000000..27c124a5 --- /dev/null +++ b/dji_sdk/test/dji_sdk-node.test @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/dji_sdk_demo/src/client.cpp b/dji_sdk_demo/src/client.cpp index 49e6d820..486a910b 100644 --- a/dji_sdk_demo/src/client.cpp +++ b/dji_sdk_demo/src/client.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -42,6 +43,12 @@ void GlobalNavigationTestMobileCallback(DJIDrone *drone); void WaypointNavigationTestMobileCallback(DJIDrone *drone); void VirtuaRCTestMobileCallback(DJIDrone *drone); +//! For LAS logging +void StartMapLASLoggingMobileCallback(DJIDrone *drone); +void StopMapLASLoggingMobileCallback(DJIDrone *drone); +void StartCollisionAvoidanceCallback(DJIDrone *drone); +void StopCollisionAvoidanceCallback(DJIDrone *drone); + static void Display_Main_Menu(void) { @@ -131,6 +138,11 @@ int main(int argc, char *argv[]) drone->setWaypointNavigationTestMobileCallback(WaypointNavigationTestMobileCallback, &userData); drone->setVirtuaRCTestMobileCallback(VirtuaRCTestMobileCallback, &userData); + drone->setStartMapLASLoggingMobileCallback(StartMapLASLoggingMobileCallback, &userData); + drone->setStopMapLASLoggingMobileCallback(StopMapLASLoggingMobileCallback, &userData); + drone->setStartCollisionAvoidanceCallback(StartCollisionAvoidanceCallback, &userData); + drone->setStopCollisionAvoidanceCallback(StopCollisionAvoidanceCallback, &userData); + Display_Main_Menu(); while(1) @@ -1018,3 +1030,43 @@ int main(int argc, char *argv[]) } drone->virtual_rc_disable(); } + +void StartMapLASLoggingMobileCallback(DJIDrone *drone) +{ + system("roslaunch point_cloud_las start_velodyne_and_loam.launch &"); + system("rosrun point_cloud_las write _topic:=/laser_cloud_surround _folder_path:=. &"); +} + +void StopMapLASLoggingMobileCallback(DJIDrone *drone) +{ + system("rosnode kill /write_LAS /scanRegistration /laserMapping /transformMaintenance /laserOdometry &"); +} + +void StartCollisionAvoidanceCallback(DJIDrone *drone) +{ + uint8_t freq[16]; + freq[0] = 1; // 0 - Timestamp + freq[1] = 4; // 1 - Attitude Quaterniouns + freq[2] = 1; // 2 - Acceleration + freq[3] = 4; // 3 - Velocity (Ground Frame) + freq[4] = 4; // 4 - Angular Velocity (Body Frame) + freq[5] = 3; // 5 - Position + freq[6] = 0; // 6 - Magnetometer + freq[7] = 3; // 7 - M100:RC Channels Data, A3:RTK Detailed Information + freq[8] = 0; // 8 - M100:Gimbal Data, A3: Magnetometer + freq[9] = 3; // 9 - M100:Flight Status, A3: RC Channels + freq[10] = 0; // 10 - M100:Battery Level, A3: Gimble Data + freq[11] = 2; // 11 - M100:Control Information, A3: Flight Status + + drone->set_message_frequency(freq); + usleep(1e4); + system("roslaunch dji_collision_avoidance from_DJI_ros_demo.launch &"); +} + +void StopCollisionAvoidanceCallback(DJIDrone *drone) +{ + drone->release_sdk_permission_control(); + system("rosnode kill /drone_tf_builder /dji_occupancy_grid_node /dji_collision_detection_node /collision_velodyne_nodelet_manager /manual_fly"); + usleep(1e4); + drone->request_sdk_permission_control(); +} diff --git a/dji_sdk_lib/src/DJI_Link.cpp b/dji_sdk_lib/src/DJI_Link.cpp index 293a6e09..dcbf51a1 100644 --- a/dji_sdk_lib/src/DJI_Link.cpp +++ b/dji_sdk_lib/src/DJI_Link.cpp @@ -71,7 +71,6 @@ void CoreAPI::appHandler(Header *protocolHeader) data = CMDSessionTab[protocolHeader->sessionID].userData; freeSession(&CMDSessionTab[protocolHeader->sessionID]); serialDevice->freeMemory(); - if (callBack) { //! Non-blocking callback thread