Diff between tum-vision/ardrone_autonomy (be96c3) and AutonomyLab/ardrone_autonomy (Fuerte)

gistfile1.diff

diff --git a/ARDroneLib/Soft/Common/autoconf.h b/ARDroneLib/Soft/Common/autoconf.h
new file mode 100644
index 0000000..da7ddb5
--- /dev/null
+++ b/ARDroneLib/Soft/Common/autoconf.h
@@ -0,0 +1,51 @@
+/*
+ * Automatically generated C config: don't edit
+ * Linux kernel version:
+ * Thu Nov  5 18:06:01 2009
+ */
+#define AUTOCONF_INCLUDED
+#define PAL_TRACE_THREAD_VAL 0
+#define PAL_ASSERT 1
+#define MODIF_VERSION_NUMBER 0
+#define PAL_BUTTON_LONG_PRESS_TIME 2000
+#define PAL_BUTTON_DRIVER 1
+#define PAL_I2C_DRIVER 1
+#define MAJOR_VERSION_NUMBER 1
+#define PAL_TRACE_SEM_VAL 0
+#define MINOR_VERSION_NUMBER 0
+#define PAL_TRACE_SYS_VAL 0
+#define PAL_SUP_NB_TIMERS 18
+#define PAL_TRACE_HWALARM_VAL 0
+#define PAL_PWM_DRIVER 1
+#define PAL_I2C_DEVICES 1
+#define EXTENDED_VERSION_INFO RC0
+#define PAL_SUP_NB_MOD 32
+#define PAL_TRACE_FLAG_VAL 0
+#define PAL_TRACE_COND_VAL 0
+#define PAL_DEBUG_LEVEL 1
+#define PAL_TRACE_GPIO_VAL 0
+#define PAL_TRACE_TIME_VAL 0
+#define PAL_TRACE_MBOX_VAL 0
+#define PAL_TRACE_UART_VAL 0
+#define PAL_GPIO_DRIVER 1
+#define PAL_P6MU_ADC_DEVICE 1
+#define PAL_P6MU_CODEC_DEVICE 1
+#define BUILD_PAL 1
+#define PAL_SUP_PRIO_P1 18
+#define PAL_SUP_PRIO_P2 20
+#define PAL_SUP_PRIO_P3 22
+#define PAL_SUP_PRIO_P4 24
+#define PAL_SUP_PRIO_P5 26
+#define PAL_UARTS_COUNT 4
+#define PAL_LINUX_NOSMP 1
+#define PAL_TRACE_ALARM_VAL 0
+#define PAL_SUP_MAX_MES 60
+#define CONFIG_PAL_USER_ASSERT 1
+#define TRUC_POURRI_YMM 1
+#define UNAME_RELEASE 2.6.28-16-generic
+#define PAL_BUTTON_MAX_HW_DRIVERS 1
+#define PAL_TRACE_MUTEX_VAL 0
+#define PAL_BUTTON_MAX_BUTTONS 32
+#define PAL_STACKSIZE 6144
+#define PAL_POSIX_MIX_PRIO 1
+#define PAL_POSIX_RT_PRIO_THRESHOLD 10
diff --git a/README.md b/README.md
index 190592a..08120ba 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,3 @@
-*Important Note*: ROS _Fuerte_ has been End-Of-Lifed on April 23 2012. This branch will not receive any more updates.
-
 # ardrone_autonomy : A ROS Driver for ARDrone 1.0 & 2.0
 
 "ardrone_autonomy" is a [ROS](http://ros.org/ "Robot Operating System") driver for [Parrot AR-Drone](http://http://ardrone.parrot.com/parrot-ar-drone/select-site) quadrocopter. This driver is based on official [AR-Drone SDK](https://projects.ardrone.org/) version 2.0 and supports both AR-Drone 1.0 and 2.0. "ardrone_autonomy" is a fork of [AR-Drone Brown](http://code.google.com/p/brown-ros-pkg/wiki/ardrone_brown) driver. This package has been developed in [Autonomy Lab](http://autonomy.cs.sfu.ca) of [Simon Fraser University](http://www.sfu.ca) by [Mani Monajjemi](http://sfu.ca/~mmonajje) ( +other [contributors](#contributors)).
diff --git a/calibrations/ardrone1_bottom/cal.yml b/calibrations/ardrone1_bottom/cal.yml
new file mode 100644
index 0000000..25e72ea
--- /dev/null
+++ b/calibrations/ardrone1_bottom/cal.yml
@@ -0,0 +1,20 @@
+image_width: 174
+image_height: 144
+camera_name: ardrone_bottom
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [197.611868, 0, 87.004133, 0, 215.807977, 70.061796, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [0.25568, -0.861356, 0.001048, -0.002966, 0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [199.946396, 0, 86.168696, 0, 0, 218.65036, 69.654309, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/calibrations/ardrone1_bottom/ost.ini b/calibrations/ardrone1_bottom/ost.ini
new file mode 100644
index 0000000..4812b43
--- /dev/null
+++ b/calibrations/ardrone1_bottom/ost.ini
@@ -0,0 +1,31 @@
+# oST version 5.0 parameters
+
+
+[image]
+
+width
+174
+
+height
+144
+
+[ardrone_bottom]
+
+camera matrix
+197.611868 0.000000 87.004133
+0.000000 215.807977 70.061796
+0.000000 0.000000 1.000000
+
+distortion
+0.255680 -0.861356 0.001048 -0.002966 0.000000
+
+rectification
+1.000000 0.000000 0.000000
+0.000000 1.000000 0.000000
+0.000000 0.000000 1.000000
+
+projection
+199.946396 0.000000 86.168696 0.000000
+0.000000 218.650360 69.654309 0.000000
+0.000000 0.000000 1.000000 0.000000
+
diff --git a/calibrations/ardrone1_front/cal.yml b/calibrations/ardrone1_front/cal.yml
new file mode 100644
index 0000000..d5e4093
--- /dev/null
+++ b/calibrations/ardrone1_front/cal.yml
@@ -0,0 +1,20 @@
+image_width: 320
+image_height: 240
+camera_name: ardrone_front
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [209.62677, 0, 161.275943, 0, 210.825901, 123.723408, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [0.160828, -0.280969, 4.4e-05, 0.001607, 0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [-245.858795, 0, 250.684758, 0, 0, -253.237381, 55.012524, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/calibrations/ardrone1_front/ost.ini b/calibrations/ardrone1_front/ost.ini
new file mode 100644
index 0000000..a332ad8
--- /dev/null
+++ b/calibrations/ardrone1_front/ost.ini
@@ -0,0 +1,31 @@
+# oST version 5.0 parameters
+
+
+[image]
+
+width
+320
+
+height
+240
+
+[ardrone_front]
+
+camera matrix
+209.626770 0.000000 161.275943
+0.000000 210.825901 123.723408
+0.000000 0.000000 1.000000
+
+distortion
+0.160828 -0.280969 0.000044 0.001607 0.000000
+
+rectification
+1.000000 0.000000 0.000000
+0.000000 1.000000 0.000000
+0.000000 0.000000 1.000000
+
+projection
+-245.858795 0.000000 250.684758 0.000000
+0.000000 -253.237381 55.012524 0.000000
+0.000000 0.000000 1.000000 0.000000
+
diff --git a/calibrations/ardrone2_bottom/cal.yml b/calibrations/ardrone2_bottom/cal.yml
new file mode 100644
index 0000000..c96f9c3
--- /dev/null
+++ b/calibrations/ardrone2_bottom/cal.yml
@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 360
+camera_name: ardrone_bottom
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [686.994766, 0, 329.323208, 0, 688.195055, 159.323007, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.006282, 0.03748, -0.003174, -0.002855, 0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [687.761902, 0, 327.39355, 0, 0, 688.746338, 157.996045, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/calibrations/ardrone2_bottom/ost.ini b/calibrations/ardrone2_bottom/ost.ini
new file mode 100644
index 0000000..c927ab0
--- /dev/null
+++ b/calibrations/ardrone2_bottom/ost.ini
@@ -0,0 +1,31 @@
+# oST version 5.0 parameters
+
+
+[image]
+
+width
+640
+
+height
+360
+
+[ardrone_bottom]
+
+camera matrix
+686.994766 0.000000 329.323208
+0.000000 688.195055 159.323007
+0.000000 0.000000 1.000000
+
+distortion
+-0.006282 0.037480 -0.003174 -0.002855 0.000000
+
+rectification
+1.000000 0.000000 0.000000
+0.000000 1.000000 0.000000
+0.000000 0.000000 1.000000
+
+projection
+687.761902 0.000000 327.393550 0.000000
+0.000000 688.746338 157.996045 0.000000
+0.000000 0.000000 1.000000 0.000000
+
diff --git a/calibrations/ardrone2_front/cal.yml b/calibrations/ardrone2_front/cal.yml
new file mode 100644
index 0000000..8850991
--- /dev/null
+++ b/calibrations/ardrone2_front/cal.yml
@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 360
+camera_name: ardrone_front
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [561.999146, 0, 307.433982, 0, 561.782697, 190.144373, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.50758, 0.24911, 0.000579, 0.000996, 0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [454.505341, 0, 302.364285, 0, 0, 528.832031, 190.983935, 0, 0, 0, 1, 0]
\ No newline at end of file
diff --git a/calibrations/ardrone2_front/ost.ini b/calibrations/ardrone2_front/ost.ini
new file mode 100644
index 0000000..edc5767
--- /dev/null
+++ b/calibrations/ardrone2_front/ost.ini
@@ -0,0 +1,31 @@
+# oST version 5.0 parameters
+
+
+[image]
+
+width
+640
+
+height
+360
+
+[ardrone_front]
+
+camera matrix
+561.999146 0.000000 307.433982
+0.000000 561.782697 190.144373
+0.000000 0.000000 1.000000
+
+distortion
+-0.507580 0.249110 0.000579 0.000996 0.000000
+
+rectification
+1.000000 0.000000 0.000000
+0.000000 1.000000 0.000000
+0.000000 0.000000 1.000000
+
+projection
+454.505341 0.000000 302.364285 0.000000
+0.000000 528.832031 190.983935 0.000000
+0.000000 0.000000 1.000000 0.000000
+
diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp
index 8bc9c45..9abf572 100644
--- a/src/ardrone_driver.cpp
+++ b/src/ardrone_driver.cpp
@@ -2,6 +2,7 @@
 #include "teleop_twist.h"
 #include "video.h"
 #include <signal.h>
+#include "ros/package.h"
 
 ////////////////////////////////////////////////////////////////////////////////
 // class ARDroneDriver
@@ -71,8 +72,8 @@ ARDroneDriver::ARDroneDriver()
     }
 
     // Camera Info Manager
-    cinfo_hori_ = new camera_info_manager::CameraInfoManager(ros::NodeHandle("ardrone/front"), "ardrone_front");
-    cinfo_vert_ = new camera_info_manager::CameraInfoManager(ros::NodeHandle("ardrone/bottom"), "ardrone_bottom");
+    cinfo_hori_ = 0; //new camera_info_manager::CameraInfoManager(ros::NodeHandle("ardrone/front"), "ardrone_front");
+    cinfo_vert_ = 0; //new camera_info_manager::CameraInfoManager(ros::NodeHandle("ardrone/bottom"), "ardrone_bottom");
 
     // TF Stuff
 
@@ -334,7 +335,7 @@ void ARDroneDriver::publish_video()
        ) return;
 
     // Camera Info (NO PIP)
-
+	if (cinfo_hori_ == 0) return;
     sensor_msgs::CameraInfo cinfo_msg_hori = cinfo_hori_->getCameraInfo();
     sensor_msgs::CameraInfo cinfo_msg_vert = cinfo_vert_->getCameraInfo();
 
@@ -365,6 +366,9 @@ void ARDroneDriver::publish_video()
         sensor_msgs::Image::_data_type::iterator _it;
 
         image_msg.header.stamp = ros::Time::now();
+        cinfo_msg_hori.header.stamp = image_msg.header.stamp;
+        cinfo_msg_vert.header.stamp = image_msg.header.stamp;
+        
         if ((cam_state == ZAP_CHANNEL_HORI) || (cam_state == ZAP_CHANNEL_LARGE_HORI_SMALL_VERT))
         {
             image_msg.header.frame_id = droneFrameFrontCam;
@@ -595,6 +599,25 @@ void ARDroneDriver::publish_video()
 
 void ARDroneDriver::publish_navdata(navdata_unpacked_t &navdata_raw, const ros::Time &navdata_receive_time)
 {
+	if(cinfo_hori_ == 0)
+	{
+		std::string path = ros::package::getPath("ardrone_autonomy")+"/calibrations/";
+		if(IS_ARDRONE2)
+		{
+			cinfo_hori_ = new camera_info_manager::CameraInfoManager(node_handle, "ardrone_front", "file://" + path + "ardrone2_front/cal.yml");
+			cinfo_vert_ = new camera_info_manager::CameraInfoManager(node_handle, "ardrone_bottom", "file://" + path + "ardrone2_bottom/cal.yml");
+		}
+		else
+		{
+			cinfo_hori_ = new camera_info_manager::CameraInfoManager(node_handle, "ardrone_front", "file://" + path + "ardrone1_front/cal.yml");
+			cinfo_vert_ = new camera_info_manager::CameraInfoManager(node_handle, "ardrone_bottom", "file://" + path + "ardrone1_bottom/cal.yml");
+		}
+
+		ROS_INFO("Loaded Camera Calibration Files");
+	}
+	// Camera Info Manager
+	
+	
     if ((do_caliberation) && (!caliberated))
     {
         acc_samples[0].push_back(navdata_raw.navdata_phys_measures.phys_accs[ACC_X]);
diff --git a/src/ardrone_sdk.cpp b/src/ardrone_sdk.cpp
index 8ef0164..e18207a 100644
--- a/src/ardrone_sdk.cpp
+++ b/src/ardrone_sdk.cpp
@@ -61,8 +61,8 @@ extern "C" {
         }
 
         ros::param::param("~looprate",looprate,50);
-        ros::param::param("~realtime_navdata",realtime_navdata,false);
-        ros::param::param("~realtime_video",realtime_video,false);
+        ros::param::param("~realtime_navdata",realtime_navdata,true);
+        ros::param::param("~realtime_video",realtime_video,true);
 
         // SET SOME NON-STANDARD DEFAULT VALUES FOR THE DRIVER
         // THESE CAN BE OVERWRITTEN BY ROS PARAMETERS (below)
@@ -71,6 +71,13 @@ extern "C" {
         {
             ardrone_application_default_config.max_bitrate = 4000;
         }
+        
+        ardrone_application_default_config.control_vz_max = 1300;
+        ardrone_application_default_config.outdoor_control_vz_max = 1300;
+        ardrone_application_default_config.indoor_control_vz_max = 1300;
+
+        ardrone_application_default_config.outdoor = 0;
+        ardrone_application_default_config.flight_without_shell = 0;
 
         ardrone_application_default_config.navdata_options = NAVDATA_OPTION_FULL_MASK;        
         ardrone_application_default_config.video_channel = ZAP_CHANNEL_HORI;