implemented service server

HeikoE · Jan 27, 2021 · f3de6a6 · f3de6a6
1 parent 95dfd73
commit f3de6a6
Show file tree

Hide file tree

Showing 4 changed files with 90 additions and 0 deletions.
diff --git a/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp b/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp
@@ -16,6 +16,8 @@
 #include <string>
 #include <thread>
 #include <vector>
+#include <memory>
+#include <condition_variable>
 
 // ROS
 #include <actionlib/server/simple_action_server.h>
@@ -36,6 +38,7 @@
 #include <darknet_ros_msgs/BoundingBox.h>
 #include <darknet_ros_msgs/BoundingBoxes.h>
 #include <darknet_ros_msgs/CheckForObjectsAction.h>
+#include <darknet_ros_msgs/CheckForObjects.h>
 #include <darknet_ros_msgs/ObjectCount.h>
 
 // Darknet.
@@ -114,12 +117,19 @@ class YoloObjectDetector {
    */
   void checkForObjectsActionPreemptCB();
 
+  /*!
+   * Check for objects service callback.
+   */
+  bool checkForObjectsServiceCB(darknet_ros_msgs::CheckForObjects::Request &req, darknet_ros_msgs::CheckForObjects::Response &res);
+
   /*!
    * Check if a preempt for the check for objects action has been requested.
    * @return false if preempt has been requested or inactive.
    */
   bool isCheckingForObjects() const;
 
+  bool isCheckingInService();
+
   /*!
    * Publishes the detection image.
    * @return true if successful.
@@ -140,6 +150,14 @@ class YoloObjectDetector {
   //! Check for objects action server.
   CheckForObjectsActionServerPtr checkForObjectsActionServer_;
 
+  //! Check for objects service server.
+  ros::ServiceServer checkForObjectsServiceServer_;
+  darknet_ros_msgs::CheckForObjects::Response serviceResponse_;
+  std::mutex mutexServiceResponse_;
+  bool serviceRunning_ = false;
+  std::mutex mutexServiceRunning_;
+  std::condition_variable serviceCV_;
+
   //! Advertise and subscribe to image topics.
   image_transport::ImageTransport imageTransport_;
 

diff --git a/darknet_ros/src/YoloObjectDetector.cpp b/darknet_ros/src/YoloObjectDetector.cpp
@@ -154,6 +154,11 @@ void YoloObjectDetector::init() {
   checkForObjectsActionServer_->registerGoalCallback(boost::bind(&YoloObjectDetector::checkForObjectsActionGoalCB, this));
   checkForObjectsActionServer_->registerPreemptCallback(boost::bind(&YoloObjectDetector::checkForObjectsActionPreemptCB, this));
   checkForObjectsActionServer_->start();
+
+  // Service servers.
+  std::string checkForObjectsServiceName;
+  nodeHandle_.param("services/camera_reading/topic", checkForObjectsServiceName, std::string("check_for_objects"));
+  checkForObjectsServiceServer_ = nodeHandle_.advertiseService(checkForObjectsServiceName, &YoloObjectDetector::checkForObjectsServiceCB, this);
 }
 
 void YoloObjectDetector::cameraCallback(const sensor_msgs::ImageConstPtr& msg) {
@@ -225,10 +230,58 @@ void YoloObjectDetector::checkForObjectsActionPreemptCB() {
   checkForObjectsActionServer_->setPreempted();
 }
 
+bool YoloObjectDetector::checkForObjectsServiceCB(darknet_ros_msgs::CheckForObjects::Request &req, darknet_ros_msgs::CheckForObjects::Response &res) {
+  ROS_DEBUG("[YoloObjectDetector] Start check for objects service.");
+
+  cv_bridge::CvImagePtr cam_image;
+
+  try {
+    cam_image = cv_bridge::toCvCopy(req.image, sensor_msgs::image_encodings::BGR8);
+  } catch (cv_bridge::Exception& e) {
+    ROS_ERROR("cv_bridge exception: %s", e.what());
+    return false;
+  }
+
+  if (cam_image) {
+    {
+      boost::unique_lock<boost::shared_mutex> lockImageCallback(mutexImageCallback_);
+      imageHeader_ = req.image.header;
+      imageHeader_.seq = imageHeader_.seq * (req.id + 1);
+      camImageCopy_ = cam_image->image.clone();
+    }
+    {
+      std::unique_lock<std::mutex> lockServiceRunning {mutexServiceRunning_};
+      serviceRunning_ = true;
+    }
+    {
+      boost::unique_lock<boost::shared_mutex> lockImageStatus(mutexImageStatus_);
+      imageStatus_ = true;
+    }
+    frameWidth_ = cam_image->image.size().width;
+    frameHeight_ = cam_image->image.size().height;
+
+    {
+      std::unique_lock<std::mutex> lockWaitForResult {mutexServiceResponse_};
+      serviceCV_.wait(lockWaitForResult);
+      res = serviceResponse_;
+    }
+  }
+  {
+      std::lock_guard<std::mutex> lockServiceRunning {mutexServiceRunning_};
+      serviceRunning_ = false;
+  }
+  return true;
+}
+
 bool YoloObjectDetector::isCheckingForObjects() const {
   return (ros::ok() && checkForObjectsActionServer_->isActive() && !checkForObjectsActionServer_->isPreemptRequested());
 }
 
+bool YoloObjectDetector::isCheckingInService() {
+  std::lock_guard<std::mutex> lk {mutexServiceRunning_};
+  return (ros::ok() && serviceRunning_);
+}
+
 bool YoloObjectDetector::publishDetectionImage(const cv::Mat& detectionImage) {
   if (detectionImagePublisher_.getNumSubscribers() < 1) return false;
   cv_bridge::CvImage cvImage;
@@ -606,6 +659,15 @@ void* YoloObjectDetector::publishInThread() {
     objectsActionResult.bounding_boxes = boundingBoxesResults_;
     checkForObjectsActionServer_->setSucceeded(objectsActionResult, "Send bounding boxes.");
   }
+  if (isCheckingInService()) {
+    ROS_DEBUG("[YoloObjectDetector] check for objects in image (service).");
+    {
+      std::unique_lock<std::mutex> lk {mutexServiceResponse_};
+      serviceResponse_.id = static_cast<darknet_ros_msgs::CheckForObjects::Response::_id_type>(buffId_[0]);
+      serviceResponse_.bounding_boxes = boundingBoxesResults_;
+    }
+    serviceCV_.notify_all();
+  }
   boundingBoxesResults_.bounding_boxes.clear();
   for (int i = 0; i < numClasses_; i++) {
     rosBoxes_[i].clear();

diff --git a/darknet_ros_msgs/CMakeLists.txt b/darknet_ros_msgs/CMakeLists.txt
@@ -20,6 +20,11 @@ add_message_files(
     ObjectCount.msg
 )
 
+add_service_files(
+  FILES
+    CheckForObjects.srv
+)
+
 add_action_files(
   FILES
     CheckForObjects.action

diff --git a/darknet_ros_msgs/srv/CheckForObjects.srv b/darknet_ros_msgs/srv/CheckForObjects.srv
@@ -0,0 +1,5 @@
+int16 id
+sensor_msgs/Image image
+---
+int16 id
+darknet_ros_msgs/BoundingBoxes bounding_boxes