From 617e24e5f28d7b5900e5c4d7bc9960e95d0ca45d Mon Sep 17 00:00:00 2001 From: Patrick Date: Mon, 24 Jun 2019 10:10:14 +0200 Subject: [PATCH 01/11] Adding services --- libuvc_camera/CMakeLists.txt | 26 +++++++++++++++++-- .../include/libuvc_camera/camera_driver.h | 6 +++++ libuvc_camera/package.xml | 10 +++++++ libuvc_camera/src/camera_driver.cpp | 10 +++++++ libuvc_camera/srv/DoStartCamera.srv | 3 +++ libuvc_camera/srv/DoStopCamera.srv | 3 +++ 6 files changed, 56 insertions(+), 2 deletions(-) create mode 100644 libuvc_camera/srv/DoStartCamera.srv create mode 100644 libuvc_camera/srv/DoStopCamera.srv diff --git a/libuvc_camera/CMakeLists.txt b/libuvc_camera/CMakeLists.txt index 91db504..26cfcf6 100644 --- a/libuvc_camera/CMakeLists.txt +++ b/libuvc_camera/CMakeLists.txt @@ -2,13 +2,35 @@ 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}") +add_service_files( + FILES + DoStartCamera.srv + DoStopCamera.srv +) + +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..b549851 100644 --- a/libuvc_camera/include/libuvc_camera/camera_driver.h +++ b/libuvc_camera/include/libuvc_camera/camera_driver.h @@ -10,6 +10,8 @@ #include #include +#include +#include namespace libuvc_camera { @@ -21,6 +23,10 @@ class CameraDriver { bool Start(); void Stop(); + + bool ServiceStart(libuvc_camera::DoStartCamera::Request &req, libuvc_camera::DoStartCamera::Request &res); + bool ServiceStop(libuvc_camera::DoStopCamera::Request &req, libuvc_camera::DoStopCamera::Response &res); + private: enum State { kInitial = 0, diff --git a/libuvc_camera/package.xml b/libuvc_camera/package.xml index d9dae65..ec3ce51 100644 --- a/libuvc_camera/package.xml +++ b/libuvc_camera/package.xml @@ -35,24 +35,34 @@ roscpp + std_msgs camera_info_manager dynamic_reconfigure image_transport libuvc nodelet sensor_msgs + std_srvs + actionlib + actionlib_msgs + message_generation catkin roscpp + std_msgs camera_info_manager dynamic_reconfigure image_transport libuvc nodelet sensor_msgs + std_srvs + actionlib_msgs + actionlib + message_generation diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 5b6e611..294866f 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -99,6 +99,16 @@ void CameraDriver::Stop() { state_ = kInitial; } +bool CameraDriver::ServiceStart(libuvc_camera::DoStartCamera::Request &req, libuvc_camera::DoStartCamera::Request &res) { + Start(); + return true; +} + +bool CameraDriver::ServiceStop(libuvc_camera::DoStopCamera::Request &req, libuvc_camera::DoStopCamera::Response &res) { + Stop(); + return true; +} + void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t level) { boost::recursive_mutex::scoped_lock(mutex_); diff --git a/libuvc_camera/srv/DoStartCamera.srv b/libuvc_camera/srv/DoStartCamera.srv new file mode 100644 index 0000000..963bf4c --- /dev/null +++ b/libuvc_camera/srv/DoStartCamera.srv @@ -0,0 +1,3 @@ +--- +bool success +string message diff --git a/libuvc_camera/srv/DoStopCamera.srv b/libuvc_camera/srv/DoStopCamera.srv new file mode 100644 index 0000000..963bf4c --- /dev/null +++ b/libuvc_camera/srv/DoStopCamera.srv @@ -0,0 +1,3 @@ +--- +bool success +string message From 52a2eb6ea37c68f3c873f371b1c8774bd1232673 Mon Sep 17 00:00:00 2001 From: doisyg Date: Mon, 24 Jun 2019 10:59:23 +0200 Subject: [PATCH 02/11] Advertise services --- libuvc_camera/include/libuvc_camera/camera_driver.h | 7 ++++--- libuvc_camera/src/camera_driver.cpp | 7 +++++-- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/libuvc_camera/include/libuvc_camera/camera_driver.h b/libuvc_camera/include/libuvc_camera/camera_driver.h index b549851..b182878 100644 --- a/libuvc_camera/include/libuvc_camera/camera_driver.h +++ b/libuvc_camera/include/libuvc_camera/camera_driver.h @@ -23,9 +23,10 @@ class CameraDriver { bool Start(); void Stop(); - - bool ServiceStart(libuvc_camera::DoStartCamera::Request &req, libuvc_camera::DoStartCamera::Request &res); - bool ServiceStop(libuvc_camera::DoStopCamera::Request &req, libuvc_camera::DoStopCamera::Response &res); + ros::ServiceServer startCameraService; + ros::ServiceServer stopCameraService; + bool service_start_callback(libuvc_camera::DoStartCamera::Request &req, libuvc_camera::DoStartCamera::Request &res); + bool service_stop_callback(libuvc_camera::DoStopCamera::Request &req, libuvc_camera::DoStopCamera::Response &res); private: enum State { diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 294866f..da14f6c 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -39,6 +39,7 @@ #include #include #include + #define libuvc_VERSION (libuvc_VERSION_MAJOR * 10000 \ + libuvc_VERSION_MINOR * 100 \ + libuvc_VERSION_PATCH) @@ -53,6 +54,8 @@ CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh) config_server_(mutex_, priv_nh_), config_changed_(false), cinfo_manager_(nh) { + stopCameraService = nh.advertiseService("/camera_driver/stop_camera", &CameraDriver::service_stop_callback, this); + startCameraService = nh.advertiseService("/camera_driver/start_camera", &CameraDriver::service_start_callback, this); cam_pub_ = it_.advertiseCamera("image_raw", 1, false); } @@ -99,12 +102,12 @@ void CameraDriver::Stop() { state_ = kInitial; } -bool CameraDriver::ServiceStart(libuvc_camera::DoStartCamera::Request &req, libuvc_camera::DoStartCamera::Request &res) { +bool CameraDriver::service_start_callback(libuvc_camera::DoStartCamera::Request &req, libuvc_camera::DoStartCamera::Request &res) { Start(); return true; } -bool CameraDriver::ServiceStop(libuvc_camera::DoStopCamera::Request &req, libuvc_camera::DoStopCamera::Response &res) { +bool CameraDriver::service_stop_callback(libuvc_camera::DoStopCamera::Request &req, libuvc_camera::DoStopCamera::Response &res) { Stop(); return true; } From bd395060dc375890353357c844f04b9ccdc506ef Mon Sep 17 00:00:00 2001 From: doisyg Date: Mon, 24 Jun 2019 11:35:55 +0200 Subject: [PATCH 03/11] Opti lines --- libuvc_camera/src/camera_driver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index da14f6c..4bdae9d 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -103,8 +103,7 @@ void CameraDriver::Stop() { } bool CameraDriver::service_start_callback(libuvc_camera::DoStartCamera::Request &req, libuvc_camera::DoStartCamera::Request &res) { - Start(); - return true; + return Start(); } bool CameraDriver::service_stop_callback(libuvc_camera::DoStopCamera::Request &req, libuvc_camera::DoStopCamera::Response &res) { From 7d89931362ed77ed627a3ee5c8264bd2e757aa9c Mon Sep 17 00:00:00 2001 From: Patrick Date: Wed, 26 Jun 2019 10:39:02 +0200 Subject: [PATCH 04/11] Services become std_srvs::Trigger --- libuvc_camera/CMakeLists.txt | 6 ------ libuvc_camera/include/libuvc_camera/camera_driver.h | 7 +++---- libuvc_camera/src/camera_driver.cpp | 8 +++++--- libuvc_camera/srv/DoStartCamera.srv | 3 --- libuvc_camera/srv/DoStopCamera.srv | 3 --- 5 files changed, 8 insertions(+), 19 deletions(-) delete mode 100644 libuvc_camera/srv/DoStartCamera.srv delete mode 100644 libuvc_camera/srv/DoStopCamera.srv diff --git a/libuvc_camera/CMakeLists.txt b/libuvc_camera/CMakeLists.txt index 26cfcf6..a96e6b9 100644 --- a/libuvc_camera/CMakeLists.txt +++ b/libuvc_camera/CMakeLists.txt @@ -19,12 +19,6 @@ find_package(libuvc REQUIRED COMPONENTS) message(STATUS "libuvc ${libuvc_VERSION_MAJOR}.${libuvc_VERSION_MINOR}.${libuvc_VERSION_PATCH}") -add_service_files( - FILES - DoStartCamera.srv - DoStopCamera.srv -) - generate_messages( DEPENDENCIES std_msgs diff --git a/libuvc_camera/include/libuvc_camera/camera_driver.h b/libuvc_camera/include/libuvc_camera/camera_driver.h index b182878..3d9863e 100644 --- a/libuvc_camera/include/libuvc_camera/camera_driver.h +++ b/libuvc_camera/include/libuvc_camera/camera_driver.h @@ -8,10 +8,9 @@ #include #include #include +#include #include -#include -#include namespace libuvc_camera { @@ -25,8 +24,8 @@ class CameraDriver { ros::ServiceServer startCameraService; ros::ServiceServer stopCameraService; - bool service_start_callback(libuvc_camera::DoStartCamera::Request &req, libuvc_camera::DoStartCamera::Request &res); - bool service_stop_callback(libuvc_camera::DoStopCamera::Request &req, libuvc_camera::DoStopCamera::Response &res); + bool service_start_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + bool service_stop_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); private: enum State { diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 4bdae9d..0972cd2 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -102,12 +102,14 @@ void CameraDriver::Stop() { state_ = kInitial; } -bool CameraDriver::service_start_callback(libuvc_camera::DoStartCamera::Request &req, libuvc_camera::DoStartCamera::Request &res) { - return Start(); +bool CameraDriver::service_start_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { + res.success = Start(); + return true; } -bool CameraDriver::service_stop_callback(libuvc_camera::DoStopCamera::Request &req, libuvc_camera::DoStopCamera::Response &res) { +bool CameraDriver::service_stop_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { Stop(); + res.success = true; return true; } diff --git a/libuvc_camera/srv/DoStartCamera.srv b/libuvc_camera/srv/DoStartCamera.srv deleted file mode 100644 index 963bf4c..0000000 --- a/libuvc_camera/srv/DoStartCamera.srv +++ /dev/null @@ -1,3 +0,0 @@ ---- -bool success -string message diff --git a/libuvc_camera/srv/DoStopCamera.srv b/libuvc_camera/srv/DoStopCamera.srv deleted file mode 100644 index 963bf4c..0000000 --- a/libuvc_camera/srv/DoStopCamera.srv +++ /dev/null @@ -1,3 +0,0 @@ ---- -bool success -string message From e654bc7132f3a72f4dbd15e1746480619bca2318 Mon Sep 17 00:00:00 2001 From: Patrick Date: Wed, 26 Jun 2019 11:09:20 +0200 Subject: [PATCH 05/11] Merging the 2 services into a single one --- .../include/libuvc_camera/camera_driver.h | 9 ++++----- libuvc_camera/src/camera_driver.cpp | 19 ++++++++++--------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/libuvc_camera/include/libuvc_camera/camera_driver.h b/libuvc_camera/include/libuvc_camera/camera_driver.h index 3d9863e..1dc62bb 100644 --- a/libuvc_camera/include/libuvc_camera/camera_driver.h +++ b/libuvc_camera/include/libuvc_camera/camera_driver.h @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include @@ -22,10 +22,9 @@ class CameraDriver { bool Start(); void Stop(); - ros::ServiceServer startCameraService; - ros::ServiceServer stopCameraService; - bool service_start_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); - bool service_stop_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + ros::ServiceServer enableCamera; + + bool enabled_callback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); private: enum State { diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 0972cd2..33a3992 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -54,8 +54,7 @@ CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh) config_server_(mutex_, priv_nh_), config_changed_(false), cinfo_manager_(nh) { - stopCameraService = nh.advertiseService("/camera_driver/stop_camera", &CameraDriver::service_stop_callback, this); - startCameraService = nh.advertiseService("/camera_driver/start_camera", &CameraDriver::service_start_callback, this); + enableCamera = nh.advertiseService("/bottom_cam/enabled", &CameraDriver::enabled_callback, this); cam_pub_ = it_.advertiseCamera("image_raw", 1, false); } @@ -102,15 +101,17 @@ void CameraDriver::Stop() { state_ = kInitial; } -bool CameraDriver::service_start_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { - res.success = Start(); - return true; -} - -bool CameraDriver::service_stop_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { - Stop(); +bool CameraDriver::enabled_callback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) { + if(state_ == kRunning) { + Stop(); + res.message = "Camera stopped"; + } else if (state_ == kStopped) { + Start(); + res.message = "Camera started"; + } res.success = true; return true; + } void CameraDriver::ReconfigureCallback(UVCCameraConfig &new_config, uint32_t level) { From 5da5ea693f5fbefe238db967d52de75aabe88f00 Mon Sep 17 00:00:00 2001 From: doisyg Date: Wed, 26 Jun 2019 11:25:39 +0200 Subject: [PATCH 06/11] Correction on state checking --- libuvc_camera/src/camera_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 33a3992..47b6d5a 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -105,7 +105,7 @@ bool CameraDriver::enabled_callback(std_srvs::SetBool::Request &req, std_srvs::S if(state_ == kRunning) { Stop(); res.message = "Camera stopped"; - } else if (state_ == kStopped) { + } else if (state_ == kInitial) { Start(); res.message = "Camera started"; } From c1deea3eadc6b3c3403735b819097884a677434f Mon Sep 17 00:00:00 2001 From: Patrick Date: Wed, 26 Jun 2019 13:32:52 +0200 Subject: [PATCH 07/11] New srv name and checking data value --- libuvc_camera/src/camera_driver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index 47b6d5a..e7bf6b2 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -54,7 +54,7 @@ CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh) config_server_(mutex_, priv_nh_), config_changed_(false), cinfo_manager_(nh) { - enableCamera = nh.advertiseService("/bottom_cam/enabled", &CameraDriver::enabled_callback, this); + enableCamera = nh.advertiseService("/camera_bottom_front_driver/enabled", &CameraDriver::enabled_callback, this); cam_pub_ = it_.advertiseCamera("image_raw", 1, false); } @@ -102,10 +102,10 @@ void CameraDriver::Stop() { } bool CameraDriver::enabled_callback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) { - if(state_ == kRunning) { + if((state_ == kRunning) && (req.data == false)) { Stop(); res.message = "Camera stopped"; - } else if (state_ == kInitial) { + } else if ((state_ == kInitial) && (req.data == true)) { Start(); res.message = "Camera started"; } From ac6d44a5fbf60a252711a10750f78ef411acbbef Mon Sep 17 00:00:00 2001 From: Patrick Date: Wed, 26 Jun 2019 13:49:53 +0200 Subject: [PATCH 08/11] Stop the driver at the start --- libuvc_camera/src/main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libuvc_camera/src/main.cpp b/libuvc_camera/src/main.cpp index e596a36..8347659 100644 --- a/libuvc_camera/src/main.cpp +++ b/libuvc_camera/src/main.cpp @@ -45,6 +45,8 @@ int main (int argc, char **argv) { if (!driver.Start()) return -1; + //We do not start the driver right away + driver.Stop(); ros::spin(); driver.Stop(); From 112387c56eb837edaf3b29d710e94efb02b9cdfc Mon Sep 17 00:00:00 2001 From: doisyg Date: Wed, 26 Jun 2019 14:18:34 +0200 Subject: [PATCH 09/11] Do not start bottom cam at beginning --- libuvc_camera/src/nodelet.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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(); +// } } }; From bea5ff94328f273334a3fab304cca59d054777f7 Mon Sep 17 00:00:00 2001 From: doisyg Date: Wed, 26 Jun 2019 14:19:09 +0200 Subject: [PATCH 10/11] Delete useless line --- libuvc_camera/src/main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libuvc_camera/src/main.cpp b/libuvc_camera/src/main.cpp index 8347659..ce74259 100644 --- a/libuvc_camera/src/main.cpp +++ b/libuvc_camera/src/main.cpp @@ -46,7 +46,6 @@ int main (int argc, char **argv) { return -1; //We do not start the driver right away - driver.Stop(); ros::spin(); driver.Stop(); From b5e94001a3f4c4c3365971d08ba48d1f559f11b7 Mon Sep 17 00:00:00 2001 From: Patrick Lascombe Date: Thu, 8 Aug 2019 15:05:41 +0200 Subject: [PATCH 11/11] Clean up before PR --- libuvc_camera/package.xml | 6 ------ libuvc_camera/src/camera_driver.cpp | 1 - 2 files changed, 7 deletions(-) diff --git a/libuvc_camera/package.xml b/libuvc_camera/package.xml index ec3ce51..974115b 100644 --- a/libuvc_camera/package.xml +++ b/libuvc_camera/package.xml @@ -43,9 +43,6 @@ nodelet sensor_msgs std_srvs - actionlib - actionlib_msgs - message_generation catkin @@ -60,9 +57,6 @@ nodelet sensor_msgs std_srvs - actionlib_msgs - actionlib - message_generation diff --git a/libuvc_camera/src/camera_driver.cpp b/libuvc_camera/src/camera_driver.cpp index e7bf6b2..df2a625 100644 --- a/libuvc_camera/src/camera_driver.cpp +++ b/libuvc_camera/src/camera_driver.cpp @@ -39,7 +39,6 @@ #include #include #include - #define libuvc_VERSION (libuvc_VERSION_MAJOR * 10000 \ + libuvc_VERSION_MINOR * 100 \ + libuvc_VERSION_PATCH)