From 3d5c51a42eeeb8ebca431ac1e1bb690da681dd57 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Tue, 24 Nov 2020 22:34:12 +0300
Subject: [PATCH] aruco_detect: add 'enabled' dynamic param

---
 aruco_pose/cfg/DetectorParams.cfg | 2 ++
 aruco_pose/src/aruco_detect.cpp   | 4 ++++
 2 files changed, 6 insertions(+)

diff --git a/aruco_pose/cfg/DetectorParams.cfg b/aruco_pose/cfg/DetectorParams.cfg
index c96c942b..5788e9e9 100755
--- a/aruco_pose/cfg/DetectorParams.cfg
+++ b/aruco_pose/cfg/DetectorParams.cfg
@@ -8,6 +8,8 @@ p = cv2.aruco.DetectorParameters_create()
 
 gen = ParameterGenerator()
 
+gen.add("enabled", bool_t, 0, "if detection enabled", True)
+
 gen.add("adaptiveThreshConstant",                 double_t, 0,
         "Constant for adaptive thresholding before finding contours",
          p.adaptiveThreshConstant, 0, 100)
diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp
index 7d8d1d99..92422c0c 100644
--- a/aruco_pose/src/aruco_detect.cpp
+++ b/aruco_pose/src/aruco_detect.cpp
@@ -62,6 +62,7 @@ private:
 	std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>> dyn_srv_;
+	bool enabled_ = true;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
 	image_transport::Publisher debug_pub_;
@@ -128,6 +129,8 @@ public:
 private:
 	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
 	{
+		if (!enabled_) return;
+
 		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
 
 		vector<int> ids;
@@ -356,6 +359,7 @@ private:
 
 	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
 	{
+		enabled_ = config.enabled;
 		parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
 		parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
 		parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;