Added new ROS service to directly set the camera channel.

CMakeLists.txt

@@ -19,7 +19,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 #uncomment if you have defined messages
 rosbuild_genmsg()
 #uncomment if you have defined services
-#rosbuild_gensrv()
+rosbuild_gensrv()
 
 #common commands for building c++ executables and libraries
 #rosbuild_add_library(${PROJECT_NAME} src/example.cpp)

src/ardrone_driver.cpp

@@ -28,7 +28,8 @@ ARDroneDriver::ARDroneDriver()
 #endif
 
 	toggleCam_service = node_handle.advertiseService("/ardrone/togglecam", toggleCamCallback);
-        toggleNavdataDemo_service = node_handle.advertiseService("/ardrone/togglenavdatademo", toggleNavdataDemoCallback);
+                toggleNavdataDemo_service = node_handle.advertiseService("/ardrone/togglenavdatademo", toggleNavdataDemoCallback);
+                setCamChannel_service = node_handle.advertiseService("/ardrone/setcamchannel",setCamChannelCallback );
 }
 
 ARDroneDriver::~ARDroneDriver()

src/ardrone_driver.h

@@ -30,7 +30,8 @@ private:
 
 	//ros::Subscriber toggleCam_sub;
 	ros::ServiceServer toggleCam_service;
-        ros::ServiceServer toggleNavdataDemo_service;
+                ros::ServiceServer toggleNavdataDemo_service;
+                ros::ServiceServer setCamChannel_service;
 
 	int last_frame_id;
 	int flying_state;

src/teleop_twist.cpp

@@ -18,33 +18,34 @@ bool toggleNavdataDemoCallback(std_srvs::Empty::Request& request, std_srvs::Empt
     return true;
 }
 
+//ros service callback to set the camera channel
+//TODO: add input check
+bool setCamChannelCallback(ardrone_brown::CamSelect::Request& request, ardrone_brown::CamSelect::Response& response)
+{
+    cam_state = request.channel;
+ #ifdef _USING_SDK_1_7_
+    ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
+    fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
+#else
+    ardrone_at_set_toy_configuration("video:video_channel",cam_state);
+    fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
+#endif
+    response.result = true;
+    return true;
+}
 // ros service callback function for toggling Cam
 bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
 {
-  if (cam_state == 0) // toggle to 1, the vertical camera
-    {
-      cam_state = 1;
-
 #ifdef _USING_SDK_1_7_
-      ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
+    cam_state = (cam_state + 1) % 4;
+    ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
+    fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
 #else
-      ardrone_at_set_toy_configuration("video:video_channel","1");
+    cam_state = (cam_state + 1) % 2;
+    ardrone_at_set_toy_configuration("video:video_channel",cam_state);
+    fprintf(stderr, "\nSetting camera channel to : %d.\n", cam_state);
 #endif
-
-      fprintf(stderr, "\nToggling from frontal camera to vertical camera.\n");
-    }
-  else if (cam_state == 1) // toggle to the forward camera
-    {
-      cam_state = 0;
-
-#ifdef _USING_SDK_1_7_
-      ARDRONE_TOOL_CONFIGURATION_ADDEVENT (video_channel, &cam_state, NULL);
-#else
-      ardrone_at_set_toy_configuration("video:video_channel","0");
-#endif
-
-      fprintf(stderr, "\nToggling from vertical camera to frontal camera.\n");      
-    }
+    
   return true;
 }
 

src/teleop_twist.h

@@ -5,6 +5,7 @@
 #include <geometry_msgs/Twist.h>
 #include <std_msgs/Empty.h>
 #include <std_srvs/Empty.h>
+#include <ardrone_brown/CamSelect.h>
 
 #define _USING_SDK_1_7_
 
@@ -16,6 +17,7 @@ void resetCallback(const std_msgs::Empty &msg);
 void takeoffCallback(const std_msgs::Empty &msg);
 
 //void toggleCamCallback(const std_msgs::Empty &msg);
+bool setCamChannelCallback(ardrone_brown::CamSelect::Request& request, ardrone_brown::CamSelect::Response& response);
 bool toggleCamCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
 bool toggleNavdataDemoCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
 

srv/CamSelect.srv

@@ -0,0 +1,3 @@
+uint8 channel
+---
+bool result