Creation of enabled_callback service

libuvc_camera/CMakeLists.txt

@@ -2,13 +2,29 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(libuvc_camera)
 # Load catkin and all dependencies required for this package
-find_package(catkin REQUIRED COMPONENTS roscpp camera_info_manager dynamic_reconfigure image_transport nodelet sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    camera_info_manager
+    dynamic_reconfigure
+    image_transport
+    nodelet
+    sensor_msgs
+    std_msgs
+    std_srvs
+    message_generation)
 
 generate_dynamic_reconfigure_options(cfg/UVCCamera.cfg)
 
-find_package(libuvc REQUIRED)
+find_package(libuvc REQUIRED COMPONENTS)
+
 message(STATUS "libuvc ${libuvc_VERSION_MAJOR}.${libuvc_VERSION_MINOR}.${libuvc_VERSION_PATCH}")
 
+generate_messages(
+    DEPENDENCIES
+    std_msgs
+    std_srvs
+)
+
 catkin_package(
   CATKIN_DEPENDS
     roscpp

libuvc_camera/include/libuvc_camera/camera_driver.h

@@ -8,6 +8,7 @@
 #include <dynamic_reconfigure/server.h>
 #include <camera_info_manager/camera_info_manager.h>
 #include <boost/thread/mutex.hpp>
+#include <std_srvs/SetBool.h>
 
 #include <libuvc_camera/UVCCameraConfig.h>
 
@@ -21,6 +22,10 @@ class CameraDriver {
   bool Start();
   void Stop();
 
+  ros::ServiceServer enableCamera;
+
+  bool enabled_callback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
+
 private:
   enum State {
     kInitial = 0,

libuvc_camera/package.xml

@@ -35,24 +35,28 @@
   <!-- Use build_depend for packages you need at compile time: -->
   <!--   <build_depend>message_generation</build_depend> -->
   <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
   <build_depend>camera_info_manager</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>libuvc</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
   <!-- Use buildtool_depend for build tool packages: -->
   <!--   <buildtool_depend>catkin</buildtool_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <!-- Use run_depend for packages you need at runtime: -->
   <!--   <run_depend>message_runtime</run_depend> -->
   <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
   <run_depend>camera_info_manager</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>libuvc</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_srvs</run_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
 

libuvc_camera/src/camera_driver.cpp

@@ -53,6 +53,7 @@ CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh)
     config_server_(mutex_, priv_nh_),
     config_changed_(false),
     cinfo_manager_(nh) {
+    enableCamera = nh.advertiseService("/camera_bottom_front_driver/enabled", &CameraDriver::enabled_callback, this);
   cam_pub_ = it_.advertiseCamera("image_raw", 1, false);
 }
 
@@ -99,6 +100,19 @@ void CameraDriver::Stop() {
   state_ = kInitial;
 }
 
+bool CameraDriver::enabled_callback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) {
+    if((state_ == kRunning) && (req.data == false)) {
+        Stop();
+        res.message = "Camera stopped";
+    } else if ((state_ == kInitial) && (req.data == true)) {
+        Start();
+        res.message = "Camera started";
+    }
+    res.success = true;
+    return true;
+
+}
+
 void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t level) {
   boost::recursive_mutex::scoped_lock lock(mutex_);
 

libuvc_camera/src/main.cpp

@@ -45,6 +45,7 @@ int main (int argc, char **argv) {
   if (!driver.Start())
     return -1;
 
+  //We do not start the driver right away
   ros::spin();
 
   driver.Stop();

libuvc_camera/src/nodelet.cpp

@@ -62,12 +62,12 @@ void CameraNodelet::onInit() {
   ros::NodeHandle priv_nh(getPrivateNodeHandle());
 
   driver_.reset(new CameraDriver(nh, priv_nh));
-  if (driver_->Start()) {
-    running_ = true;
-  } else {
-    NODELET_ERROR("Unable to open camera.");
-    driver_.reset();
-  }
+//  if (driver_->Start()) {
+//    running_ = true;
+//  } else {
+//    NODELET_ERROR("Unable to open camera.");
+//    driver_.reset();
+//  }
 }
 
 };