diff --git a/libuvc_camera/CMakeLists.txt b/libuvc_camera/CMakeLists.txt index 91db504..a96e6b9 100644 --- a/libuvc_camera/CMakeLists.txt +++ b/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 diff --git a/libuvc_camera/include/libuvc_camera/camera_driver.h b/libuvc_camera/include/libuvc_camera/camera_driver.h index db825fd..1dc62bb 100644 --- a/libuvc_camera/include/libuvc_camera/camera_driver.h +++ b/libuvc_camera/include/libuvc_camera/camera_driver.h @@ -8,6 +8,7 @@ #include #include #include +#include #include @@ -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, diff --git a/libuvc_camera/package.xml b/libuvc_camera/package.xml index d9dae65..974115b 100644 --- a/libuvc_camera/package.xml +++ b/libuvc_camera/package.xml @@ -35,24 +35,28 @@ roscpp + std_msgs camera_info_manager dynamic_reconfigure image_transport libuvc nodelet sensor_msgs + std_srvs catkin roscpp + std_msgs camera_info_manager dynamic_reconfigure image_transport libuvc nodelet sensor_msgs + std_srvs diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index aa4abce..bc2af88 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/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_); diff --git a/libuvc_camera/src/main.cpp b/libuvc_camera/src/main.cpp index e596a36..ce74259 100644 --- a/libuvc_camera/src/main.cpp +++ b/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(); diff --git a/libuvc_camera/src/nodelet.cpp b/libuvc_camera/src/nodelet.cpp index 91d2f3c..6f70fc2 100644 --- a/libuvc_camera/src/nodelet.cpp +++ b/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(); +// } } };