From 72933c92ea084ffbe68ddedeb5ae44d01f3d27d8 Mon Sep 17 00:00:00 2001
From: Kayman <kmjung@robotis.com>
Date: Fri, 23 Feb 2018 16:24:03 +0900
Subject: [PATCH] added service for setting ball_detector params

---
 .../include/ball_detector/ball_detector.h     | 10 +++-
 ball_detector/src/ball_detector.cpp           | 48 +++++++++++++++++++
 2 files changed, 57 insertions(+), 1 deletion(-)

diff --git a/ball_detector/include/ball_detector/ball_detector.h b/ball_detector/include/ball_detector/ball_detector.h
index ad147a5..e8fff30 100644
--- a/ball_detector/include/ball_detector/ball_detector.h
+++ b/ball_detector/include/ball_detector/ball_detector.h
@@ -37,6 +37,8 @@
 #include "ball_detector/circleSetStamped.h"
 #include "ball_detector/ball_detector_config.h"
 #include "ball_detector/detectorParamsConfig.h"
+#include "ball_detector/GetParameters.h"
+#include "ball_detector/SetParameters.h"
 
 namespace robotis_op
 {
@@ -72,6 +74,8 @@ class BallDetector
   void enableCallback(const std_msgs::Bool::ConstPtr &msg);
 
   void paramCommandCallback(const std_msgs::String::ConstPtr &msg);
+  bool setParamCallback(ball_detector::SetParameters::Request &req, ball_detector::SetParameters::Response &res);
+  bool getParamCallback(ball_detector::GetParameters::Request &req, ball_detector::GetParameters::Response &res);
 
   void printConfig();
   void saveConfig();
@@ -89,7 +93,6 @@ class BallDetector
   ros::NodeHandle nh_;
 
   ros::Subscriber enable_sub_;
-  ros::Subscriber param_command_sub_;
 
   //image publisher/subscriber
   image_transport::ImageTransport it_;
@@ -116,6 +119,11 @@ class BallDetector
   std::string param_path_;
   bool has_path_;
 
+  // web setting
+  ros::Subscriber param_command_sub_;
+  ros::ServiceServer get_param_client_;
+  ros::ServiceServer set_param_client_;
+
   //flag indicating a new image has been received
   bool new_image_flag_;
 
diff --git a/ball_detector/src/ball_detector.cpp b/ball_detector/src/ball_detector.cpp
index 3b54a6c..0d387db 100644
--- a/ball_detector/src/ball_detector.cpp
+++ b/ball_detector/src/ball_detector.cpp
@@ -88,6 +88,10 @@ BallDetector::BallDetector()
   callback_fnc_ = boost::bind(&BallDetector::dynParamCallback, this, _1, _2);
   param_server_.setCallback(callback_fnc_);
 
+  // web setting
+  set_param_client_ = nh_.advertiseService("set_param", &BallDetector::setParamCallback, this);
+  get_param_client_ = nh_.advertiseService("get_param", &BallDetector::getParamCallback, this);
+
   //sets config and prints it
   params_config_ = detect_config;
   init_param_ = true;
@@ -279,6 +283,50 @@ void BallDetector::paramCommandCallback(const std_msgs::String::ConstPtr &msg)
   }
 }
 
+bool BallDetector::setParamCallback(ball_detector::SetParameters::Request &req, ball_detector::SetParameters::Response &res)
+{
+  params_config_.gaussian_blur_size = req.params.gaussian_blur_size;
+  params_config_.gaussian_blur_sigma = req.params.gaussian_blur_sigma;
+  params_config_.canny_edge_th = req.params.canny_edge_th;
+  params_config_.hough_accum_resolution = req.params.hough_accum_resolution;
+  params_config_.min_circle_dist = req.params.min_circle_dist;
+  params_config_.hough_accum_th = req.params.hough_accum_th;
+  params_config_.min_radius = req.params.min_radius;
+  params_config_.max_radius = req.params.max_radius;
+  params_config_.filter_threshold.h_min = req.params.filter_h_min;
+  params_config_.filter_threshold.h_max = req.params.filter_h_max;
+  params_config_.filter_threshold.s_min = req.params.filter_s_min;
+  params_config_.filter_threshold.s_max = req.params.filter_s_max;
+  params_config_.filter_threshold.v_min = req.params.filter_v_min;
+  params_config_.filter_threshold.v_max = req.params.filter_v_max;
+
+  saveConfig();
+
+  res.returns = req.params;
+
+  return true;
+}
+
+bool BallDetector::getParamCallback(ball_detector::GetParameters::Request &req, ball_detector::GetParameters::Response &res)
+{
+  res.returns.gaussian_blur_size = params_config_.gaussian_blur_size;
+  res.returns.gaussian_blur_sigma = params_config_.gaussian_blur_sigma;
+  res.returns.canny_edge_th = params_config_.canny_edge_th;
+  res.returns.hough_accum_resolution = params_config_.hough_accum_resolution;
+  res.returns.min_circle_dist = params_config_.min_circle_dist;
+  res.returns.hough_accum_th = params_config_.hough_accum_th;
+  res.returns.min_radius = params_config_.min_radius;
+  res.returns.max_radius = params_config_.max_radius;
+  res.returns.filter_h_min = params_config_.filter_threshold.h_min;
+  res.returns.filter_h_max = params_config_.filter_threshold.h_max;
+  res.returns.filter_s_min = params_config_.filter_threshold.s_min;
+  res.returns.filter_s_max = params_config_.filter_threshold.s_max;
+  res.returns.filter_v_min = params_config_.filter_threshold.v_min;
+  res.returns.filter_v_max = params_config_.filter_threshold.v_max;
+
+  return true;
+}
+
 void BallDetector::printConfig()
 {
   if (init_param_ == false)