From 090897d9e33270f733c4f4a61c162a83d4755a97 Mon Sep 17 00:00:00 2001
From: Josh Marshall <josh.marshall@tecevo.com>
Date: Tue, 12 Nov 2019 14:50:52 +1100
Subject: [PATCH] Add option for single-shot detection and corresponding
 service.

---
 .../apriltag_ros/continuous_detector.h        |  9 +++-
 apriltag_ros/src/continuous_detector.cpp      | 49 +++++++++++++++++--
 2 files changed, 52 insertions(+), 6 deletions(-)

diff --git a/apriltag_ros/include/apriltag_ros/continuous_detector.h b/apriltag_ros/include/apriltag_ros/continuous_detector.h
index 3dfb5703..6b9b141f 100644
--- a/apriltag_ros/include/apriltag_ros/continuous_detector.h
+++ b/apriltag_ros/include/apriltag_ros/continuous_detector.h
@@ -48,6 +48,7 @@
 #include <memory>
 
 #include <nodelet/nodelet.h>
+#include <std_srvs/Trigger.h>
 
 namespace apriltag_ros
 {
@@ -55,21 +56,25 @@ namespace apriltag_ros
 class ContinuousDetector: public nodelet::Nodelet
 {
  public:
-   ContinuousDetector();
+  ContinuousDetector();
   void onInit();
 
   void imageCallback(const sensor_msgs::ImageConstPtr& image_rect,
                      const sensor_msgs::CameraInfoConstPtr& camera_info);
-
+                     
+  bool singleShotService (std_srvs::Trigger::Request& request, 
+                          std_srvs::Trigger::Response& response);
  private:
   std::shared_ptr<TagDetector> tag_detector_;
   bool draw_tag_detections_image_;
+  bool single_shot_detection_;
   cv_bridge::CvImagePtr cv_image_;
 
   std::shared_ptr<image_transport::ImageTransport> it_;
   image_transport::CameraSubscriber camera_image_subscriber_;
   image_transport::Publisher tag_detections_image_publisher_;
   ros::Publisher tag_detections_publisher_;
+  ros::ServiceServer single_shot_service_;
 };
 
 } // namespace apriltag_ros
diff --git a/apriltag_ros/src/continuous_detector.cpp b/apriltag_ros/src/continuous_detector.cpp
index 6700d1a0..41ed4ea0 100644
--- a/apriltag_ros/src/continuous_detector.cpp
+++ b/apriltag_ros/src/continuous_detector.cpp
@@ -52,16 +52,29 @@ void ContinuousDetector::onInit ()
       "publish_tag_detections_image", false);
   it_ = std::shared_ptr<image_transport::ImageTransport>(
       new image_transport::ImageTransport(nh));
-
-  camera_image_subscriber_ =
-      it_->subscribeCamera("image_rect", 1,
-                          &ContinuousDetector::imageCallback, this);
+  
+  single_shot_detection_ = getAprilTagOption<bool>(pnh, 
+      "single_shot_detection", false);
+      
+  if (!single_shot_detection_)
+  {
+    camera_image_subscriber_ =
+        it_->subscribeCamera("image_rect", 1,
+                            &ContinuousDetector::imageCallback, this);
+    ROS_INFO("Running in continous mode.");
+  }
+  else {
+    ROS_INFO("Running in single shot detection mode.");
+  }
+  
   tag_detections_publisher_ =
       nh.advertise<AprilTagDetectionArray>("tag_detections", 1);
   if (draw_tag_detections_image_)
   {
     tag_detections_image_publisher_ = it_->advertise("tag_detections_image", 1);
   }
+
+  single_shot_service_ = nh.advertiseService("single_shot_detect_tags", &ContinuousDetector::singleShotService, this);
 }
 
 void ContinuousDetector::imageCallback (
@@ -91,6 +104,34 @@ void ContinuousDetector::imageCallback (
     tag_detector_->drawDetections(cv_image_);
     tag_detections_image_publisher_.publish(cv_image_->toImageMsg());
   }
+  
+  if (single_shot_detection_) 
+  {
+      camera_image_subscriber_.shutdown();
+  }
+}
+
+bool ContinuousDetector::singleShotService (
+    std_srvs::Trigger::Request& request, 
+    std_srvs::Trigger::Response& response)
+{
+    if (!camera_image_subscriber_) {
+        camera_image_subscriber_ =
+            it_->subscribeCamera("image_rect", 1,
+                                &ContinuousDetector::imageCallback, this);
+        response.success = true;
+        response.message = "Single shot tag detection activated.";
+    }
+    else 
+    {
+        response.success = false;
+        if (!single_shot_detection_)
+            response.message = "Not in single shot detection mode.";
+        else
+            response.message = "Already waiting for single shot detection.";
+        
+    }
+    return true;
 }
 
 } // namespace apriltag_ros