Use dynamic reconfigure to set GUI parameters

cfg/GUIParams.cfg

@@ -16,5 +16,7 @@ gen.add("KBsensRP", double_t, 0, "Sensitivity for roll/pitch Keyboard control",
 gen.add("KBsensYaw", double_t, 0, "Sensitivity for yaw Keyboard control", 1.0, 0.0, 1.0)
 gen.add("KBsensGAZ", double_t, 0, "Sensitivity for gaz Keyboard control", 1.0, 0.0, 1.0)
 
+gen.add("DroneIP", str_t, 0, "Ip address of drone for pinging", "192.168.1.1")
+
 
 exit(gen.generate(PACKAGE, "Config", "GUIParams"))

launch/tum_ardrone.launch

@@ -1,8 +1,11 @@
 <launch>
+  <arg name="drone_ip" default="127.0.0.1"/>
+
   <node name="drone_stateestimation" pkg="tum_ardrone" type="drone_stateestimation">
   </node>
   <node name="drone_autopilot" pkg="tum_ardrone" type="drone_autopilot">
   </node>
   <node name="drone_gui" pkg="tum_ardrone" type="drone_gui">
+	<param name="DroneIP" value="$(arg drone_ip)"/>
   </node>
 </launch>

src/UINode/PingThread.cpp

@@ -30,8 +30,10 @@ PingThread::PingThread()
 {
 	line1[0] = '\0';
 	line2[0] = '\0';
-	keepRunning = true;
+	keepRunning = true;
+	started = false;
 	measure = true;
+    ip = std::string("");
 
 	p500 = 25;
 	p20000 = 50;
@@ -58,6 +60,18 @@ void PingThread::stopSystem()
 	join();
 }
 
+void PingThread::setIp(std::string newip)
+{
+    bool wasRunning = started;
+    if (wasRunning)
+        stopSystem();
+
+    ip = newip;
+
+    if (wasRunning)
+        startSystem();
+}
+
 double parsePingResult(std::string s)
 {
 	// 20008 bytes from localhost (127.0.0.1): icmp_req=1 ttl=64 time=0.075 ms
@@ -76,9 +90,9 @@ double parsePingResult(std::string s)
 void PingThread::run()
 {
 	std::cout << "Starting PING Thread" << std::endl;
-
-	sprintf(pingCommand20000,"ping -c 1 -s 20000 -w 1 192.168.1.1");
-	sprintf(pingCommand500,"ping -c 1 -s 500 -w 1 192.168.1.1");
+    started = true;
+	sprintf(pingCommand20000,"ping -c 1 -s 20000 -w 1 %s", ip.c_str());
+	sprintf(pingCommand500,"ping -c 1 -s 500 -w 1 %s", ip.c_str());
 	ros::Rate r(2.0);
 	FILE *p;
 
@@ -113,8 +127,8 @@ void PingThread::run()
 			std::cout << "new ping values: 500->" << res500 << " 20000->" << res20000 << std::endl;
 
 			// clip between 10 and 1000.
-			res500 = std::min(1000.0,std::max(10.0,res500));
-			res20000 = std::min(1000.0,std::max(10.0,res20000));
+			res500 = std::min(1000.0,std::max(1.0,res500));
+			res20000 = std::min(1000.0,std::max(1.0,res20000));
 
 			// update
 			p500 = 0.7 * p500 + 0.3 * res500;

src/UINode/PingThread.h

@@ -20,9 +20,11 @@
  */
 #ifndef __PINGTHREAD_H
 #define __PINGTHREAD_H
-
-
-#include "cvd/thread.h"
+
+
+#include "cvd/thread.h"
+#include <string>
+
 
 class tum_ardrone_gui;
 class RosThread;
@@ -36,13 +38,14 @@ class PingThread : private CVD::Thread
 
 	// keep Running
 	bool keepRunning;
-
+    bool started;
 
 	// buffers
     char pingCommand500[100];
     char pingCommand20000[100];
     char line1[200];
-    char line2[200];
+    char line2[200];
+    std::string ip;
 
 
     // running averages
@@ -58,7 +61,9 @@ class PingThread : private CVD::Thread
 	// start and stop system and respective thread.
 	// to be called externally
 	void startSystem();
-	void stopSystem();
+	void stopSystem();
+
+	void setIp(std::string);
 
 	// start and stop pinging
 	void setEnabled(bool);

src/UINode/main_GUI.cpp

@@ -17,7 +17,7 @@
  *  You should have received a copy of the GNU General Public License
  *  along with tum_ardrone.  If not, see <http://www.gnu.org/licenses/>.
  */
- 
+
 #include "tum_ardrone_gui.h"
 #include "RosThread.h"
 #include "PingThread.h"
@@ -50,6 +50,12 @@ int main(int argc, char *argv[])
     p.rosThread = &t;
     w.pingThread = &p;
 
+
+    dynamic_reconfigure::Server<tum_ardrone::GUIParamsConfig> srv;
+    dynamic_reconfigure::Server<tum_ardrone::GUIParamsConfig>::CallbackType f;
+    f = boost::bind(&tum_ardrone_gui::dynConfCb, &w, _1, _2);
+    srv.setCallback(f);
+
     // start them.
     t.startSystem();
     p.startSystem();

src/UINode/tum_ardrone_gui.cpp

@@ -411,8 +411,17 @@ ControlCommand tum_ardrone_gui::calcKBControl()
 	if(isPressed[3]) c.pitch = -sensRP; // i
 	if(isPressed[4]) c.yaw = -sensYaw; // u
 	if(isPressed[5]) c.yaw = sensYaw; // o
-	if(isPressed[6]) c.gaz = sensRP; // q
-	if(isPressed[7]) c.gaz = -sensRP; // a
+	if(isPressed[6]) c.gaz = sensGaz; // q
+	if(isPressed[7]) c.gaz = -sensGaz; // a
 
 	return c;
 }
+
+
+void tum_ardrone_gui::dynConfCb(tum_ardrone::GUIParamsConfig &config, uint32_t level)
+{
+    pingThread->setIp(config.DroneIP);
+    sensRP = config.KBsensRP;
+    sensYaw = config.KBsensYaw;
+    sensGaz = config.KBsensGAZ;
+}

src/UINode/tum_ardrone_gui.h

@@ -27,6 +27,8 @@
 #include <QtGui/QWidget>
 #include "ui_tum_ardrone_gui.h"
 #include "geometry_msgs/Twist.h"
+#include <dynamic_reconfigure/server.h>
+#include "tum_ardrone/GUIParamsConfig.h"
 
 class RosThread;
 class PingThread;
@@ -96,6 +98,8 @@ private slots:
     void setStateestimationInfo(std::string s);
     void setMotorSpeeds(std::string s);
     void closeWindow();
+    void dynConfCb(tum_ardrone::GUIParamsConfig &config, uint32_t level);
+
 
     // calculates KB command, based on currently pressed keys.
     ControlCommand calcKBControl();