This is a major change.

1) SDK main function reverted to default
2) Now using SDK blocking main function
3) ROS update now executes in its own thread
4) Signal handling added for proper shutdown
5) Some bug fixes

ARDroneLib/Soft/Lib/ardrone_tool/AT/ardrone_at_mutex.c

@@ -306,7 +306,6 @@ void ardrone_at_set_flat_trim(void)
 {
   if (!at_init)
      return;
-
   vp_os_mutex_lock(&at_mutex);
 	ATcodec_Queue_Message_valist( ids.AT_MSG_ATCMD_FTRIM_EXE,
                                  ++nb_sequence );

ARDroneLib/Soft/Lib/ardrone_tool/ardrone_tool.c

@@ -297,7 +297,7 @@ C_RESULT ardrone_tool_shutdown()
 }
 
 #include <locale.h>
-int ardrone_tool_main(int argc, char **argv, int run_loop)
+int ardrone_tool_main(int argc, char **argv)
 {
   C_RESULT res;
   const char* old_locale;
@@ -404,15 +404,11 @@ int ardrone_tool_main(int argc, char **argv, int run_loop)
 		appname = &argv[0][lastSlashPos+1];
 		ardrone_gen_appid (appname, __SDK_VERSION__, app_id, app_name, sizeof (app_name));
 		res = ardrone_tool_init(wifi_ardrone_ip, strlen(wifi_ardrone_ip), NULL, appname, NULL, NULL, NULL, MAX_FLIGHT_STORING_SIZE, NULL);
-        if (run_loop == 1)
-        {
-        while( SUCCEED(res) && ardrone_tool_exit() == FALSE )
-        {
-            res = ardrone_tool_update();
-        }
-
-        res = ardrone_tool_shutdown();
-        }
+      while( SUCCEED(res) && ardrone_tool_exit() == FALSE )
+      {
+        res = ardrone_tool_update();
+      }
+      res = ardrone_tool_shutdown();
     }
 
   if( old_locale != NULL )

ARDroneLib/Soft/Lib/ardrone_tool/ardrone_tool.h

@@ -52,7 +52,7 @@ extern void ardrone_tool_display_cmd_line_custom( void ) WEAK;
 extern bool_t ardrone_tool_parse_cmd_line_custom( const char* cmd ) WEAK;
 
 // This is implemented by the library
-int ardrone_tool_main(int argc, char**argv, int run_loop);
+int ardrone_tool_main(int argc, char**argv);
 C_RESULT ardrone_tool_init(const char* ardrone_ip, size_t n,
 		AT_CODEC_FUNCTIONS_PTRS *ptrs, const char *appname,
 		const char *usrname, const char *rootdir, const char *flightdir,

CMakeLists.txt

@@ -7,7 +7,7 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 #  Release        : w/o debug symbols, w/ optimization
 #  RelWithDebInfo : w/ debug symbols, w/ optimization
 #  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
+#set(ROS_BUILD_TYPE Release)
 
 rosbuild_init()
 

src/ardrone_driver.cpp

@@ -1,6 +1,7 @@
 #include "ardrone_driver.h"
 #include "teleop_twist.h"
 #include "video.h"
+#include <signal.h>
 
 ////////////////////////////////////////////////////////////////////////////////
 // class ARDroneDriver
@@ -21,8 +22,8 @@ ARDroneDriver::ARDroneDriver()
 
 	//int cam_state = DEFAULT_CAM_STATE; // 0 for forward and 1 for vertical, change to enum later
     //int set_navdata_demo_value = DEFAULT_NAVDATA_DEMO;  
-	ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
-	ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &set_navdata_demo_value, NULL);
+//	ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
+//	ARDRONE_TOOL_CONFIGURATION_ADDEVENT (navdata_demo, &set_navdata_demo_value, NULL);
 
 //	ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detect_type, &detect_dtype, NULL);
 //	ARDRONE_TOOL_CONFIGURATION_ADDEVENT (detections_select_h, &detect_hori_type, NULL);
@@ -46,7 +47,7 @@ ARDroneDriver::~ARDroneDriver()
 
 void ARDroneDriver::run()
 {
-	ros::Rate loop_rate(40);
+	ros::Rate loop_rate(30);
 
 	int configWait = 150;
 	bool configDone = false;
@@ -57,7 +58,7 @@ void ARDroneDriver::run()
         //int cam_state = DEFAULT_CAM_STATE; // 0 for forward and 1 for vertical, change to enum later
         //int set_navdata_demo_value = DEFAULT_NAVDATA_DEMO;  
         
-	while (node_handle.ok())
+	while (ros::ok())//node_handle.ok())
 	{
 		// For some unknown reason, sometimes the ardrone critical configurations are not applied
 		// when the commands are being sent during SDK initialization. This is a trick to send critical 
@@ -68,6 +69,7 @@ void ARDroneDriver::run()
 			if (configWait == 0) 
 			{
 				configDone = true;
+                ardrone_at_set_flat_trim();
 				fprintf(stderr, "\nSending some critical initial configuration after some delay...\n");
                 //Ensure that the horizontal camera is running
                 ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
@@ -90,10 +92,13 @@ void ARDroneDriver::run()
 			last_frame_id = current_frame_id;
 		}
 
-		ardrone_tool_update();
+        // In SDK2 there is no way to do this if you use ardrone_main_tool()
+        // Becuase this function uses a blocking call
+		//ardrone_tool_update();
 		ros::spinOnce();
 		loop_rate.sleep();
 	}
+        printf("ROS loop terminated ... \n");
 }
 
 void ARDroneDriver::publish_video()
@@ -355,33 +360,27 @@ void ARDroneDriver::publish_navdata()
 	navdata_pub.publish(msg);
 }
 
+void controlCHandler (int signal)
+{
+    ros::shutdown();
+    should_exit = 1;    
+}
 ////////////////////////////////////////////////////////////////////////////////
 // custom_main
 ////////////////////////////////////////////////////////////////////////////////
 
 //extern "C" int custom_main(int argc, char** argv)
 int main(int argc, char** argv)
-{
-	int res = ardrone_tool_setup_com( NULL );
-
-	if( FAILED(res) )
-	{
-		printf("Wifi initialization failed. It means either:\n");
-		printf("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
-		printf("\t* wifi device is not present (on your pc or on your card)\n");
-		printf("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
-		printf("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
-		printf("\t* wifi device has no antenna\n");
-	}
-	else
-	{
-		//ardrone_tool_init(argc, argv);
-        ros::init(argc, argv, "ardrone_driver");
-        ardrone_tool_main(argc, argv, 0);
-		ARDroneDriver().run();
+{        
+        // We need to implement our own Signal handler instead of ROS to shutdown
+        // the SDK threads correctly.
+    
+        ros::init(argc, argv, "ardrone_driver", ros::init_options::NoSigintHandler);
         
-	}
+        signal (SIGABRT, &controlCHandler);
+        signal (SIGTERM, &controlCHandler);
+        signal (SIGINT, &controlCHandler);
 
-	return 0;
+        return ardrone_tool_main(argc, argv);
 }
 

src/ardrone_sdk.cpp

@@ -1,6 +1,7 @@
 #include "ardrone_sdk.h"
 #include "video.h"
 #include "teleop_twist.h"
+#include "ardrone_driver.h"
 
 navdata_demo_t navdata;
 navdata_phys_measures_t navdata_phys;
@@ -8,9 +9,24 @@ navdata_vision_detect_t navdata_detect;
 
 navdata_time_t arnavtime;
 
+ARDroneDriver* rosDriver;
+
+int32_t should_exit;
+
 extern "C" {
- 
-	C_RESULT ardrone_tool_init_custom(void) {
+
+    DEFINE_THREAD_ROUTINE(update_ros, data)
+    {
+        PRINT("Thread `update_ros` started \n ");
+        ARDroneDriver* driver = (ARDroneDriver *) data;
+        driver->run();
+        return (THREAD_RET) 0;
+    }
+    
+	 C_RESULT ardrone_tool_init_custom(void) 
+     {
+     should_exit = 0;
+     rosDriver = new ARDroneDriver();
      int _w, _h;
         
         if (IS_ARDRONE2)
@@ -32,6 +48,8 @@ extern "C" {
         }
         
         //TODO: Please FIX this to read default values from ros params and move them to ardrone driver
+        //Roadmap: We have the pointer to ARDroneDriver here, so it is doable to return back ros params
+        //using this class.
         ardrone_application_default_config.bitrate_ctrl_mode = VBC_MODE_DYNAMIC;
         ardrone_application_default_config.autonomous_flight = 0;
         ardrone_application_default_config.control_level = (0 << CONTROL_LEVEL_COMBINED_YAW);
@@ -44,6 +62,8 @@ extern "C" {
         ardrone_application_default_config.control_vz_max = 850;
         ardrone_application_default_config.control_yaw = (100.0 /180.0) * 3.1415;
         ardrone_application_default_config.euler_angle_max = (12.0 / 180.0) * 3.1415;        
+        ardrone_application_default_config.video_channel = ZAP_CHANNEL_HORI;
+        ardrone_application_default_config.navdata_demo = 0;
         
 		ardrone_tool_input_add(&teleop);
         uint8_t post_stages_index = 0;
@@ -92,13 +112,29 @@ extern "C" {
         // Using the provided threaded pipeline implementation from SDK
         START_THREAD(video_stage, params);
         video_stage_init();
+//        if (ARDRONE_VERSION() >= 2)
+//        {
+//            START_THREAD (video_recorder, NULL);
+//            video_recorder_init ();
+//            video_recorder_resume_thread();
+//        }
         // Threads do not start automatically
         video_stage_resume_thread();
-		//START_THREAD(video_update_thread, 0);
-        //START_THREAD(mani, 0);
+        ardrone_tool_set_refresh_time(30);
+		START_THREAD(update_ros, rosDriver);
 		return C_OK;
 	}
-
+    
+    C_RESULT ardrone_tool_shutdown_custom() 
+    {
+        PRINT("Shutting down ... \n ");
+        JOIN_THREAD(update_ros);
+        delete rosDriver;
+        video_stage_resume_thread();
+        ardrone_tool_input_remove(&teleop);
+        return C_OK;
+    }
+    
 	C_RESULT navdata_custom_init(void *) {
 		return C_OK;
 	}
@@ -114,9 +150,15 @@ extern "C" {
 	C_RESULT navdata_custom_release() {
 		return C_OK;
 	}
-
+    
+    bool_t ardrone_tool_exit() {
+        return (should_exit == 1);
+    }
+    
 	BEGIN_THREAD_TABLE
     THREAD_TABLE_ENTRY(video_stage, 20)
+    THREAD_TABLE_ENTRY(update_ros, 20)
+//    THREAD_TABLE_ENTRY(video_recorder, 20)
 	THREAD_TABLE_ENTRY(navdata_update, 20)
 	THREAD_TABLE_ENTRY(ATcodec_Commands_Client, 20)
 	THREAD_TABLE_ENTRY(ardrone_control, 20)

src/ardrone_sdk.h

@@ -10,7 +10,6 @@
 #define USE_LINUX
 #endif
 
-
 // TODO: Research more on this issue, move the flag to CMake
 // The FFMPEG library INT macros fix
 #if defined __cplusplus
@@ -44,5 +43,6 @@ extern navdata_phys_measures_t navdata_phys;
 extern navdata_demo_t navdata;
 extern navdata_time_t arnavtime;
 
+extern int32_t should_exit;
 
 #endif