Changing parameters will now affect the drone, rather than doing nothing!

.gitignore

@@ -3,6 +3,10 @@
 *.lo
 *.o
 
+# Project Files
+*.sublime-workspace
+*.sublime-project
+
 # Compiled Dynamic libraries
 *.so
 
@@ -12,4 +16,15 @@
 *.a
 /nbproject/private/
 Makefile
-/build/CMakeFiles/
+/build/CMakeFiles/
+
+# Build results
+/ARDroneLib/FFMPEG/Includes
+/ARDroneLib/FFMPEG/FFMPEG
+/ARDroneLib/Soft/Build/
+/ARDroneLib/Soft/Common/generated_custom.h
+/bin
+/build/
+/msg_gen
+/srv_gen
+/src/ardrone_autonomy

ardrone.launch

@@ -0,0 +1,22 @@
+<launch>
+	<node name="ardrone_driver" pkg="ardrone_autonomy" type="ardrone_driver" output="screen" clear_params="true">
+	    <param name="outdoor" value="1" />
+	    <param name="max_bitrate" value="4000" />
+	    <param name="bitrate" value="4000" />
+	    <param name="navdata_demo" value="0" />
+	    <param name="flight_without_shell" value="1" />
+	    <param name="altitude_max" value="4000" />
+	    <param name="altitude_min" value="150" />
+	   	<param name="euler_angle_max" value="0.5" />
+	   	<param name="control_vz_max" value="2000" />
+	    <param name="control_yaw" value="350" />
+	    <param name="detect_type" value="10" />
+	    <param name="enemy_colors" value="3" />
+	    <param name="groundstripe_colors" value="8" />
+	    <param name="detections_select_h" value="32" />
+	    <param name="detections_select_v_hsync" value="128" />
+	    <param name="enemy_without_shell" value="0" />
+	    <param name="do_imu_caliberation" value="false" />
+	    <param name="tf_prefix" value="mydrone" />
+	</node>
+</launch>

src/ardrone_driver.cpp

@@ -127,6 +127,10 @@ void ARDroneDriver::run()
             if (((ros::Time::now() - startTime).toSec()) > 5.0)
             {
                 inited = true;
+
+                // Send the configuration to the drone
+                configureDrone();
+
                 vp_os_mutex_lock(&navdata_lock);
                 ROS_INFO("Successfully connected to '%s' (AR-Drone %d.0 - Firmware: %s) - Battery(\%): %d",
                          ardrone_control_config.ardrone_name,
@@ -160,6 +164,58 @@ void ARDroneDriver::run()
     printf("ROS loop terminated ... \n");
 }
 
+void ARDroneDriver::configureDrone()
+{
+    // This function will send the custom configuration to the drone
+    // It doesn't work if sent during the SDK (which runs before the configuration profiles on the drone are setup)
+    #undef ARDRONE_CONFIG_KEY_IMM_a10
+    #undef ARDRONE_CONFIG_KEY_REF_a10
+    #undef ARDRONE_CONFIG_KEY_STR_a10
+    #undef LOAD_PARAM_STR
+    #undef LOAD_PARAM_NUM
+
+    #define SEND_PARAM_NUM(NAME,CATEGORY,DEFAULT)                                                          \
+    {                                                                                                      \
+        if(ardrone_application_default_config.NAME!=DEFAULT)                                               \
+        {                                                                                                  \
+            ROS_INFO("SEND: "#CATEGORY"/"#NAME" = %f (DEFAULT = %f)", (float)ardrone_application_default_config.NAME, (float)DEFAULT);           \
+            ARDRONE_TOOL_CONFIGURATION_ADDEVENT (NAME, &ardrone_application_default_config.NAME, NULL);    \
+        }                                                                                                  \
+    }
+
+    #define SEND_PARAM_STR(NAME,CATEGORY,DEFAULT)                                                          \
+    {                                                                                                      \
+        if(0!=strcmp(ardrone_application_default_config.NAME,DEFAULT))                                     \
+        {                                                                                                  \
+            ROS_INFO("SEND: "#CATEGORY"/"#NAME" = %s (DEFAULT = %s)", ardrone_application_default_config.NAME, DEFAULT);           \
+            ARDRONE_TOOL_CONFIGURATION_ADDEVENT (NAME, ardrone_application_default_config.NAME, NULL);     \
+        }                                                                                                  \
+    }
+
+    // firstly we send the CAT_COMMON parameters, for example outdoor, as these settings can affect where the other parameters are stored
+    #define ARDRONE_CONFIG_KEY_REF_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) //do nothing for reference-only parameters
+    #define ARDRONE_CONFIG_KEY_IMM_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY==CAT_COMMON) SEND_PARAM_NUM(NAME,CATEGORY,DEFAULT) } // parameters under the custom key are for control of application/user/session, we don't want to change these!
+    #define ARDRONE_CONFIG_KEY_STR_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY==CAT_COMMON) SEND_PARAM_STR(NAME,CATEGORY,DEFAULT) }
+
+    #include <config_keys.h> // include the parameter definitions, which will be replaced by the above
+
+    #undef ARDRONE_CONFIG_KEY_IMM_a10
+    #undef ARDRONE_CONFIG_KEY_STR_a10
+
+    // then we send the rest of the parameters. The problem is if we send euler_angle_max (for example) before sending outdoor, it will get written to the wrong parameter
+    // (indoor_ not outdoor_euler_angle_max) and then will be overwritten by the default when changing state from indoor to outdoor, so we need to send common parameters first.
+    #define ARDRONE_CONFIG_KEY_IMM_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY!=CAT_COMMON) SEND_PARAM_NUM(NAME,CATEGORY,DEFAULT) } // parameters under the custom key are for control of application/user/session, we don't want to change these!
+    #define ARDRONE_CONFIG_KEY_STR_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY!=CAT_COMMON) SEND_PARAM_STR(NAME,CATEGORY,DEFAULT) }
+
+    #include <config_keys.h> // include the parameter definitions, which will be replaced by the above
+
+    #undef SEND_PARAM_NUM
+    #undef SEND_PARAM_STR
+    #undef ARDRONE_CONFIG_KEY_IMM_a10
+    #undef ARDRONE_CONFIG_KEY_REF_a10
+    #undef ARDRONE_CONFIG_KEY_STR_a10
+}
+
 void ARDroneDriver::resetCaliberation()
 {
     caliberated = false;
@@ -180,7 +236,7 @@ void ARDroneDriver::resetCaliberation()
 double ARDroneDriver::calcAverage(std::vector<double> &vec)
 {
     double ret = 0.0;
-    for (int i = 0; i < vec.size(); i++)
+    for (unsigned int i = 0; i < vec.size(); i++)
     {
         ret += vec.at(i);
     }
@@ -688,16 +744,74 @@ void controlCHandler (int signal)
 
 //extern "C" int custom_main(int argc, char** argv)
 int main(int argc, char** argv)
-{        
-        // We need to implement our own Signal handler instead of ROS to shutdown
-        // the SDK threads correctly.
+{
+    C_RESULT res = C_FAIL;
+
+    // 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);
+
+    // Now to setup the drone and communication channels
+    // We do this here because calling ardrone_tool_main uses an old
+    // function initialization and is no longer recommended by parrot
+    // I've based this section off the ControlEngine's initialization
+    // routine (distributed with ARDrone SDK 2.0 Examples) as well as
+    // the ardrone_tool_main function
 
-        ros::init(argc, argv, "ardrone_driver", ros::init_options::NoSigintHandler);
-
-        signal (SIGABRT, &controlCHandler);
-        signal (SIGTERM, &controlCHandler);
-        signal (SIGINT, &controlCHandler);
+    // Configure wifi
+    vp_com_wifi_config_t *config = (vp_com_wifi_config_t*)wifi_config();
 
-        return ardrone_tool_main(argc, argv);
+    if(config)
+    {
+        vp_os_memset( &wifi_ardrone_ip[0], 0, ARDRONE_IPADDRESS_SIZE );
+
+        printf("===================+> %s\n", config->server);
+        strncpy( &wifi_ardrone_ip[0], config->server, ARDRONE_IPADDRESS_SIZE-1);
+    }
+
+    while (-1 == getDroneVersion (".", wifi_ardrone_ip, &ardroneVersion))
+    {
+      printf ("Getting AR.Drone version ...\n");
+      vp_os_delay (250);
+    }
+
+    // Setup communication channels
+    res = ardrone_tool_setup_com( NULL );
+    if( FAILED(res) )
+    {
+        PRINT("Wifi initialization failed. It means either:\n");
+        PRINT("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
+        PRINT("\t* wifi device is not present (on your pc or on your card)\n");
+        PRINT("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
+        PRINT("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
+        PRINT("\t* wifi device has no antenna\n");
+    }
+    else
+    {
+        // setup the application and user profiles for the driver
+
+        char* appname = DRIVER_APPNAME;
+        char* usrname = DRIVER_USERNAME;
+        ardrone_gen_appid (appname, "2.0", app_id, app_name, APPLI_NAME_SIZE);
+        ardrone_gen_usrid (usrname, usr_id, usr_name, USER_NAME_SIZE);
+
+        // and finally initialize everything!
+        // this will then call our sdk, which then starts the ::run() method of this file as an ardrone client thread
+
+        res = ardrone_tool_init(wifi_ardrone_ip, strlen(wifi_ardrone_ip), NULL, app_name, usr_name, NULL, NULL, MAX_FLIGHT_STORING_SIZE, NULL);
+
+        while( SUCCEED(res) && ardrone_tool_exit() == FALSE )
+        {
+          res = ardrone_tool_update();
+        }
+        res = ardrone_tool_shutdown();
+    }
+    return SUCCEED(res) ? 0 : -1;
 }
 
+

src/ardrone_driver.h

@@ -11,10 +11,17 @@
 #include <ardrone_autonomy/Navdata.h>
 #include "ardrone_sdk.h"
 #include <vector>
+#include <utils/ardrone_gen_ids.h>
+#include <ardrone_tool/ardrone_version.h>
+#include <ardrone_tool/ardrone_tool.h>
+
 
 #define _DEG2RAD 0.01745331111
 #define _RAD2DEG 57.2957184819
 
+#define DRIVER_USERNAME "ardrone_driver"
+#define DRIVER_APPNAME "ardrone_driver"
+
 enum ROOT_FRAME
 {
     ROOT_FRAME_BASE = 0,
@@ -39,7 +46,8 @@ class ARDroneDriver
     void publish_tf();
     bool readCovParams(std::string param_name, boost::array<double, 9> &cov_array);
     double calcAverage(std::vector<double> &vec);
-    void resetCaliberation();    
+    void resetCaliberation();   
+    void configureDrone();
 
     ros::NodeHandle node_handle;
 	ros::Subscriber cmd_vel_sub;

src/ardrone_sdk.cpp

@@ -62,48 +62,79 @@ 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 = (int) rosDriver->getRosParam("~bitrate_ctrl_mode", (double) VBC_MODE_DISABLED);
+        // SET SOME NON-STANDARD DEFAULT VALUES FOR THE DRIVER
+        // THESE CAN BE OVERWRITTEN BY ROS PARAMETERS (below)
+        ardrone_application_default_config.bitrate_ctrl_mode = VBC_MODE_DISABLED;
         if (IS_ARDRONE2)
         {
-            ardrone_application_default_config.max_bitrate = (int) rosDriver->getRosParam("~max_bitrate", 4000.0);
+            ardrone_application_default_config.max_bitrate = 4000;
         }
-        // TODO: Fix CAT_COMMONS
-        ardrone_application_default_config.outdoor = (bool) rosDriver->getRosParam("~outdoor", 0.0);
-        ardrone_application_default_config.flight_without_shell = (bool) rosDriver->getRosParam("~flight_without_shell", 1.0);
-        ardrone_application_default_config.altitude_max = (int) rosDriver->getRosParam("~altitude_max", 3000.0);
-        ardrone_application_default_config.altitude_min = (int) rosDriver->getRosParam("~altitude_min", 100.0);
-        ardrone_application_default_config.enemy_colors = (int) rosDriver->getRosParam("~enemy_colors", (double) ARDRONE_DETECTION_COLOR_ORANGE_YELLOW);
-        ardrone_application_default_config.enemy_without_shell = (bool) rosDriver->getRosParam("~enemy_without_shell", (double) 0.0);
-        ardrone_application_default_config.video_on_usb = 0;
-        ardrone_application_default_config.autonomous_flight = 0;
-
-
-        ardrone_application_default_config.control_vz_max = (float) rosDriver->getRosParam("~control_vz_max", 850.0);
-        ardrone_application_default_config.control_yaw = (float) rosDriver->getRosParam("~control_yaw", (100.0 /180.0) * 3.1415);
-        ardrone_application_default_config.euler_angle_max = (float) rosDriver->getRosParam("~euler_angle_max", (12.0 / 180.0) * 3.1415);
-        ardrone_application_default_config.bitrate = (int) rosDriver->getRosParam("~bitrate", 4000.0);                
-        ardrone_application_default_config.navdata_demo = (int) rosDriver->getRosParam("~navdata_demo", (double) 1);
-        ardrone_application_default_config.detect_type = (int) rosDriver->getRosParam("~detect_type", (double) CAD_TYPE_MULTIPLE_DETECTION_MODE);
-        ardrone_application_default_config.detections_select_h = rosDriver->getRosParam("~detections_select_h", 
-                (double) TAG_TYPE_MASK(TAG_TYPE_SHELL_TAG_V2));
-        ardrone_application_default_config.detections_select_v_hsync = rosDriver->getRosParam("~detections_select_v_hsync", 
-                (double) TAG_TYPE_MASK(TAG_TYPE_BLACK_ROUNDEL));
-//        ardrone_application_default_config.detections_select_v = rosDriver->getRosParam("~detections_select_v", 
-//                (double) TAG_TYPE_MASK(TAG_TYPE_BLACK_ROUNDEL));
-
-        ardrone_application_default_config.navdata_options = NAVDATA_OPTION_FULL_MASK /*&
-        ~(NAVDATA_OPTION_MASK(NAVDATA_TRACKERS_SEND_TAG)
-        | NAVDATA_OPTION_MASK(NAVDATA_VISION_OF_TAG)
-        | NAVDATA_OPTION_MASK(NAVDATA_VISION_PERF_TAG)
-        | NAVDATA_OPTION_MASK(NAVDATA_VISION_TAG))*/;
-
+
+        ardrone_application_default_config.navdata_options = NAVDATA_OPTION_FULL_MASK;        
         ardrone_application_default_config.video_channel = ZAP_CHANNEL_HORI;
         ardrone_application_default_config.control_level = (0 << CONTROL_LEVEL_COMBINED_YAW);
         ardrone_application_default_config.flying_mode = FLYING_MODE_FREE_FLIGHT;
 
+
+        // LOAD THE CUSTOM CONFIGURATION FROM ROS PARAMETERS
+        // all possible configuration parameters are stored in config_keys.h (and documented in the manual)
+        // taking inspiration from ardrone_tool_configuration.c, we define some macros that replace these parameter definitions
+        // with a function which attempts to read corresponding ros parameters, and then if successful, sets the parameter value for the drone
+        // Note that we don't actually send these parameters to the drone, otherwise they will be overwritten when the profiles are created
+        // in a later stage of the ARDrone initialization.
+
+        #undef ARDRONE_CONFIG_KEY_IMM_a10
+        #undef ARDRONE_CONFIG_KEY_REF_a10
+        #undef ARDRONE_CONFIG_KEY_STR_a10
+        #undef LOAD_PARAM_STR
+        #undef LOAD_PARAM_NUM
+
+        #define LOAD_PARAM_NUM(NAME,C_TYPE)                                                                                     \
+            { double param;                                                                                                     \
+              if(ros::param::get("~"#NAME,param))                                                                               \
+              {                                                                                                                 \
+                ardrone_application_default_config.NAME = (C_TYPE)param;                                                        \
+                ROS_DEBUG("SET: "#NAME" = %f", (float)ardrone_application_default_config.NAME);                                 \
+              }                                                                                                                 \
+            }
+
+        #define LOAD_PARAM_STR(NAME)                                                                                            \
+            { std::string param;                                                                                                \
+              if(ros::param::get("~"#NAME,param))                                                                               \
+              {                                                                                                                 \
+                param = param.substr(0,STRING_T_SIZE-1);                                                                        \
+                strcpy(ardrone_application_default_config.NAME , param.c_str());                                                \
+                ROS_DEBUG("SET: "#NAME" = %s", ardrone_application_default_config.NAME);                                        \      
+              }                                                                                                                 \
+            }
+
+        #define ARDRONE_CONFIG_KEY_REF_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) //do nothing for reference-only parameters
+        #define ARDRONE_CONFIG_KEY_IMM_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom")) LOAD_PARAM_NUM(NAME,C_TYPE) } // parameters under the custom key are for control of application/user/session, we don't want to change these!
+        #define ARDRONE_CONFIG_KEY_STR_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom")) LOAD_PARAM_STR(NAME) }
+
+        #include <config_keys.h> // include the parameter definitions, which will be replaced by the above
+
+        #undef LOAD_PARAM_NUM
+        #undef LOAD_PARAM_STR
+        #undef ARDRONE_CONFIG_KEY_IMM_a10
+        #undef ARDRONE_CONFIG_KEY_REF_a10
+        #undef ARDRONE_CONFIG_KEY_STR_a10
+
+        // Now we delete any old configuration that we may have stored, this is so the profiles will be reinitialized to drone default before being updated with the potentially new set of custom parameters that we specify above.
+        // We have to do this because only non-default parameters are sent, so if we delete a ros_param, the local parameter will be not be changed (above), thus will remain default and thus won't be updated on the drone - a problem if old profiles exist.
+
+        char buffer[MULTICONFIG_ID_SIZE+1];
+
+        sprintf(buffer,"-%s",usr_id);
+        printf("Deleting Profile %s\n",buffer);
+        ARDRONE_TOOL_CONFIGURATION_ADDEVENT (profile_id, buffer, NULL);
+
+        sprintf(buffer,"-%s",app_id);
+        printf("Deleting Application %s\n",buffer);
+        ARDRONE_TOOL_CONFIGURATION_ADDEVENT (application_id, buffer, NULL);
+
+        // Now continue with the rest of the initialization
+
         ardrone_tool_input_add(&teleop);
         uint8_t post_stages_index = 0;
 

src/ardrone_sdk.h

@@ -25,7 +25,8 @@
 extern "C" {
 
 #include <config.h>
-
+
+#include <utils/ardrone_gen_ids.h>
 #include <ardrone_tool/ardrone_version.h>
 #include <ardrone_tool/ardrone_tool.h>
 #include <ardrone_tool/ardrone_tool_configuration.h>