From 9559b06983f6364ce4c32504d8d767b01df4c8f6 Mon Sep 17 00:00:00 2001 From: Kayman Date: Wed, 8 Nov 2017 17:53:39 +0900 Subject: [PATCH 01/18] first commit for walking demo. --- .../launch/ball_detector_from_usb_cam.launch | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/ball_detector/launch/ball_detector_from_usb_cam.launch b/ball_detector/launch/ball_detector_from_usb_cam.launch index 2ae61c6..1535edd 100644 --- a/ball_detector/launch/ball_detector_from_usb_cam.launch +++ b/ball_detector/launch/ball_detector_from_usb_cam.launch @@ -1,14 +1,15 @@ - + - - - + + + + @@ -16,6 +17,8 @@ + - + @@ -34,4 +37,3 @@ - From 72afe216cc310b2f0de1a09240b0d89e612c279c Mon Sep 17 00:00:00 2001 From: Kayman Date: Fri, 23 Feb 2018 14:14:32 +0900 Subject: [PATCH 02/18] added msg and srv for web setting tool applied test code --- ball_detector/CMakeLists.txt | 9 ++++++++- .../include/ball_detector/ball_detector.h | 4 ++++ ball_detector/msg/BallDetectorParams.msg | 16 ++++++++++++++++ ball_detector/src/ball_detector.cpp | 19 +++++++++++++++++++ ball_detector/srv/GetParameters.srv | 3 +++ ball_detector/srv/SetParameters.srv | 3 +++ 6 files changed, 53 insertions(+), 1 deletion(-) create mode 100644 ball_detector/msg/BallDetectorParams.msg create mode 100644 ball_detector/srv/GetParameters.srv create mode 100644 ball_detector/srv/SetParameters.srv diff --git a/ball_detector/CMakeLists.txt b/ball_detector/CMakeLists.txt index 1e4219f..6e4d658 100644 --- a/ball_detector/CMakeLists.txt +++ b/ball_detector/CMakeLists.txt @@ -24,6 +24,13 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( FILES circleSetStamped.msg + BallDetectorParams.msg +) + +add_service_files( + FILES + GetParameters.srv + SetParameters.srv ) generate_messages( @@ -70,4 +77,4 @@ target_link_libraries(ball_detector_node ################################################################################ # Test -################################################################################ \ No newline at end of file +################################################################################ diff --git a/ball_detector/include/ball_detector/ball_detector.h b/ball_detector/include/ball_detector/ball_detector.h index bb65988..ad147a5 100644 --- a/ball_detector/include/ball_detector/ball_detector.h +++ b/ball_detector/include/ball_detector/ball_detector.h @@ -27,6 +27,7 @@ //ros dependencies #include #include +#include #include #include #include @@ -70,6 +71,8 @@ class BallDetector void dynParamCallback(ball_detector::detectorParamsConfig &config, uint32_t level); void enableCallback(const std_msgs::Bool::ConstPtr &msg); + void paramCommandCallback(const std_msgs::String::ConstPtr &msg); + void printConfig(); void saveConfig(); void setInputImage(const cv::Mat & inIm); @@ -86,6 +89,7 @@ class BallDetector ros::NodeHandle nh_; ros::Subscriber enable_sub_; + ros::Subscriber param_command_sub_; //image publisher/subscriber image_transport::ImageTransport it_; diff --git a/ball_detector/msg/BallDetectorParams.msg b/ball_detector/msg/BallDetectorParams.msg new file mode 100644 index 0000000..e72a4ad --- /dev/null +++ b/ball_detector/msg/BallDetectorParams.msg @@ -0,0 +1,16 @@ +# This represents the parameters of ball_detector + +uint32 gaussian_blur_size # only odd number, 1 - 11 +float32 gaussian_blur_sigma # 1 - 5 +float32 canny_edge_th # 50 - 200 +float32 hough_accum_resolution # 1 - 8 +float32 hough_accum_th # 10 - 200 +float32 min_circle_dist # 10 - 200 +uint32 min_radius # 10 - 200 +uint32 max_radius # 100 - 600 +uint32 filter_h_min # 0 - 359 +uint32 filter_h_max +uint32 filter_s_min # 0 - 255 +uint32 filter_s_max +uint32 filter_v_min # 0 - 255 +uint32 filter_v_max diff --git a/ball_detector/src/ball_detector.cpp b/ball_detector/src/ball_detector.cpp index c41c716..3b54a6c 100644 --- a/ball_detector/src/ball_detector.cpp +++ b/ball_detector/src/ball_detector.cpp @@ -79,6 +79,7 @@ BallDetector::BallDetector() enable_sub_ = nh_.subscribe("enable", 1, &BallDetector::enableCallback, this); image_sub_ = it_.subscribe("image_in", 1, &BallDetector::imageCallback, this); camera_info_sub_ = nh_.subscribe("cameraInfo_in", 100, &BallDetector::cameraInfoCallback, this); + param_command_sub_ = nh_.subscribe("param_command", 1, &BallDetector::paramCommandCallback, this); //initializes newImageFlag new_image_flag_ = false; @@ -260,6 +261,24 @@ void BallDetector::cameraInfoCallback(const sensor_msgs::CameraInfo & msg) camera_info_msg_ = msg; } +void BallDetector::paramCommandCallback(const std_msgs::String::ConstPtr &msg) +{ + if(msg->data == "debug") + { + params_config_.debug = true; + saveConfig(); + } + else if(msg->data == "normal") + { + params_config_.debug = false; + saveConfig(); + } + else if(msg->data == "reset") + { + // load default parameters and apply + } +} + void BallDetector::printConfig() { if (init_param_ == false) diff --git a/ball_detector/srv/GetParameters.srv b/ball_detector/srv/GetParameters.srv new file mode 100644 index 0000000..eea6f98 --- /dev/null +++ b/ball_detector/srv/GetParameters.srv @@ -0,0 +1,3 @@ + +--- +BallDetectorParams returns diff --git a/ball_detector/srv/SetParameters.srv b/ball_detector/srv/SetParameters.srv new file mode 100644 index 0000000..0cb13ed --- /dev/null +++ b/ball_detector/srv/SetParameters.srv @@ -0,0 +1,3 @@ +BallDetectorParams params +--- +BallDetectorParams returns From 40d99493da3b2b325b6a4db61093e6bde6deeef7 Mon Sep 17 00:00:00 2001 From: Kayman Date: Fri, 23 Feb 2018 14:16:52 +0900 Subject: [PATCH 03/18] changed some for web setting --- ball_detector/launch/ball_detector_from_usb_cam.launch | 3 +-- op3_demo/launch/demo.launch | 6 +++++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ball_detector/launch/ball_detector_from_usb_cam.launch b/ball_detector/launch/ball_detector_from_usb_cam.launch index 1535edd..0809458 100644 --- a/ball_detector/launch/ball_detector_from_usb_cam.launch +++ b/ball_detector/launch/ball_detector_from_usb_cam.launch @@ -17,8 +17,7 @@ - + - + + + + + From 72933c92ea084ffbe68ddedeb5ae44d01f3d27d8 Mon Sep 17 00:00:00 2001 From: Kayman Date: Fri, 23 Feb 2018 16:24:03 +0900 Subject: [PATCH 04/18] 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) From d1248b8e84f7a0c8b8d4aa93ca8f42ef8380766a Mon Sep 17 00:00:00 2001 From: Kayman Date: Fri, 23 Feb 2018 20:33:09 +0900 Subject: [PATCH 05/18] added function of reset param changed default parameter value --- ball_detector/CMakeLists.txt | 1 + .../include/ball_detector/ball_detector.h | 5 + .../launch/ball_detector_params_default.yaml | 16 +-- ball_detector/package.xml | 4 +- ball_detector/src/ball_detector.cpp | 110 +++++++++++++++--- 5 files changed, 110 insertions(+), 26 deletions(-) diff --git a/ball_detector/CMakeLists.txt b/ball_detector/CMakeLists.txt index 6e4d658..a79af76 100644 --- a/ball_detector/CMakeLists.txt +++ b/ball_detector/CMakeLists.txt @@ -8,6 +8,7 @@ project(ball_detector) # Packages ################################################################################ find_package(catkin REQUIRED COMPONENTS + roslib cv_bridge geometry_msgs image_transport diff --git a/ball_detector/include/ball_detector/ball_detector.h b/ball_detector/include/ball_detector/ball_detector.h index e8fff30..6e0a3ef 100644 --- a/ball_detector/include/ball_detector/ball_detector.h +++ b/ball_detector/include/ball_detector/ball_detector.h @@ -26,6 +26,7 @@ //ros dependencies #include +#include #include #include #include @@ -76,6 +77,8 @@ class BallDetector 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 resetParameter(); + void publishParam(); void printConfig(); void saveConfig(); @@ -120,6 +123,8 @@ class BallDetector bool has_path_; // web setting + std::string default_setting_path_; + ros::Publisher param_pub_; ros::Subscriber param_command_sub_; ros::ServiceServer get_param_client_; ros::ServiceServer set_param_client_; diff --git a/ball_detector/launch/ball_detector_params_default.yaml b/ball_detector/launch/ball_detector_params_default.yaml index 7310922..a97969a 100644 --- a/ball_detector/launch/ball_detector_params_default.yaml +++ b/ball_detector/launch/ball_detector_params_default.yaml @@ -1,16 +1,16 @@ gaussian_blur_size: 7 -gaussian_blur_sigma: 2.52 -canny_edge_th: 100.5 +gaussian_blur_sigma: 2.0 +canny_edge_th: 100 hough_accum_resolution: 1 -min_circle_dist: 28.5 -hough_accum_th: 26.6 -min_radius: 25 -max_radius: 150 +min_circle_dist: 100 +hough_accum_th: 28 +min_radius: 20 +max_radius: 300 filter_h_min: 350 filter_h_max: 20 -filter_s_min: 90 +filter_s_min: 220 filter_s_max: 255 -filter_v_min: 86 +filter_v_min: 30 filter_v_max: 255 use_second_filter: true filter2_h_min: 30 diff --git a/ball_detector/package.xml b/ball_detector/package.xml index 91ee2a7..79f988f 100644 --- a/ball_detector/package.xml +++ b/ball_detector/package.xml @@ -11,7 +11,8 @@ kayman Pyo - catkin + catkin + roslib cv_bridge geometry_msgs image_transport @@ -20,6 +21,7 @@ std_msgs dynamic_reconfigure message_generation + roslib cv_bridge geometry_msgs image_transport diff --git a/ball_detector/src/ball_detector.cpp b/ball_detector/src/ball_detector.cpp index 0d387db..55aa21e 100644 --- a/ball_detector/src/ball_detector.cpp +++ b/ball_detector/src/ball_detector.cpp @@ -25,12 +25,12 @@ namespace robotis_op { BallDetector::BallDetector() - : nh_(ros::this_node::getName()), - it_(this->nh_), - enable_(true), - params_config_(), - init_param_(false), - not_found_count_(0) + : nh_(ros::this_node::getName()), + it_(this->nh_), + enable_(true), + params_config_(), + init_param_(false), + not_found_count_(0) { has_path_ = nh_.getParam("yaml_path", param_path_); @@ -79,7 +79,6 @@ BallDetector::BallDetector() enable_sub_ = nh_.subscribe("enable", 1, &BallDetector::enableCallback, this); image_sub_ = it_.subscribe("image_in", 1, &BallDetector::imageCallback, this); camera_info_sub_ = nh_.subscribe("cameraInfo_in", 100, &BallDetector::cameraInfoCallback, this); - param_command_sub_ = nh_.subscribe("param_command", 1, &BallDetector::paramCommandCallback, this); //initializes newImageFlag new_image_flag_ = false; @@ -89,8 +88,11 @@ BallDetector::BallDetector() param_server_.setCallback(callback_fnc_); // web setting + param_pub_ = nh_.advertise("current_params", 1); + param_command_sub_ = nh_.subscribe("param_command", 1, &BallDetector::paramCommandCallback, this); set_param_client_ = nh_.advertiseService("set_param", &BallDetector::setParamCallback, this); get_param_client_ = nh_.advertiseService("get_param", &BallDetector::getParamCallback, this); + default_setting_path_ = ros::package::getPath(ROS_PACKAGE_NAME) + "/launch/ball_detector_params_default.yaml"; //sets config and prints it params_config_ = detect_config; @@ -145,15 +147,15 @@ void BallDetector::publishImage() cv_img_pub_.header.frame_id = image_frame_id_; switch (img_encoding_) { - case IMG_RGB8: - cv_img_pub_.encoding = sensor_msgs::image_encodings::RGB8; - break; - case IMG_MONO: - cv_img_pub_.encoding = sensor_msgs::image_encodings::MONO8; - break; - default: - cv_img_pub_.encoding = sensor_msgs::image_encodings::MONO8; - break; + case IMG_RGB8: + cv_img_pub_.encoding = sensor_msgs::image_encodings::RGB8; + break; + case IMG_MONO: + cv_img_pub_.encoding = sensor_msgs::image_encodings::MONO8; + break; + default: + cv_img_pub_.encoding = sensor_msgs::image_encodings::MONO8; + break; } getOutputImage(cv_img_pub_.image); image_pub_.publish(cv_img_pub_.toImageMsg()); @@ -307,7 +309,7 @@ bool BallDetector::setParamCallback(ball_detector::SetParameters::Request &req, return true; } -bool BallDetector::getParamCallback(ball_detector::GetParameters::Request &req, ball_detector::GetParameters::Response &res) +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; @@ -327,6 +329,80 @@ bool BallDetector::getParamCallback(ball_detector::GetParameters::Request &req, return true; } +void BallDetector::resetParameter() +{ + + YAML::Node doc; + + try + { + // load yaml + doc = YAML::LoadFile(default_setting_path_.c_str()); + + // parse + params_config_.gaussian_blur_size = doc["gaussian_blur_size"].as(); + params_config_.gaussian_blur_sigma = doc["gaussian_blur_sigma"].as(); + params_config_.canny_edge_th = doc["canny_edge_th"].as(); + params_config_.hough_accum_resolution = doc["hough_accum_resolution"].as(); + params_config_.min_circle_dist = doc["min_circle_dist"].as(); + params_config_.hough_accum_th = doc["hough_accum_th"].as(); + params_config_.min_radius = doc["min_radius"].as(); + params_config_.max_radius = doc["max_radius"].as(); + params_config_.filter_threshold.h_min = doc["filter_h_min"].as(); + params_config_.filter_threshold.h_max = doc["filter_h_max"].as(); + params_config_.filter_threshold.s_min = doc["filter_s_min"].as(); + params_config_.filter_threshold.s_max = doc["filter_s_max"].as(); + params_config_.filter_threshold.v_min = doc["filter_v_min"].as(); + params_config_.filter_threshold.v_max = doc["filter_v_max"].as(); + params_config_.use_second_filter = doc["use_second_filter"].as(); + params_config_.filter2_threshold.h_min = doc["filter2_h_min"].as(); + params_config_.filter2_threshold.h_max = doc["filter2_h_max"].as(); + params_config_.filter2_threshold.s_min = doc["filter2_s_min"].as(); + params_config_.filter2_threshold.s_max = doc["filter2_s_max"].as(); + params_config_.filter2_threshold.v_min = doc["filter2_v_min"].as(); + params_config_.filter2_threshold.v_max = doc["filter2_v_max"].as(); + params_config_.ellipse_size = doc["ellipse_size"].as(); + params_config_.debug = doc["filter_debug"].as(); + + // gaussian_blur has to be odd number. + if (params_config_.gaussian_blur_size % 2 == 0) + params_config_.gaussian_blur_size -= 1; + if (params_config_.gaussian_blur_size <= 0) + params_config_.gaussian_blur_size = 1; + + printConfig(); + saveConfig(); + + publishParam(); + } catch (const std::exception& e) + { + ROS_ERROR_STREAM("Failed to Get default parameters : " << default_setting_path_); + return; + } +} + +void BallDetector::publishParam() +{ + ball_detector::BallDetectorParams params; + + params.gaussian_blur_size = params_config_.gaussian_blur_size; + params.gaussian_blur_sigma = params_config_.gaussian_blur_sigma; + params.canny_edge_th = params_config_.canny_edge_th; + params.hough_accum_resolution = params_config_.hough_accum_resolution; + params.min_circle_dist = params_config_.min_circle_dist; + params.hough_accum_th = params_config_.hough_accum_th; + params.min_radius = params_config_.min_radius; + params.max_radius = params_config_.max_radius; + params.filter_h_min = params_config_.filter_threshold.h_min; + params.filter_h_max = params_config_.filter_threshold.h_max; + params.filter_s_min = params_config_.filter_threshold.s_min; + params.filter_s_max = params_config_.filter_threshold.s_max; + params.filter_v_min = params_config_.filter_threshold.v_min; + params.filter_v_max = params_config_.filter_threshold.v_max; + + param_pub_.publish(params); +} + void BallDetector::printConfig() { if (init_param_ == false) From e1f3607568848be117a314b2906ebc3b33b92f83 Mon Sep 17 00:00:00 2001 From: Kayman Date: Fri, 23 Feb 2018 20:41:24 +0900 Subject: [PATCH 06/18] fixed some line --- ball_detector/src/ball_detector.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ball_detector/src/ball_detector.cpp b/ball_detector/src/ball_detector.cpp index 55aa21e..945654b 100644 --- a/ball_detector/src/ball_detector.cpp +++ b/ball_detector/src/ball_detector.cpp @@ -282,6 +282,7 @@ void BallDetector::paramCommandCallback(const std_msgs::String::ConstPtr &msg) else if(msg->data == "reset") { // load default parameters and apply + resetParameter(); } } From c064c779e1158eb73240c5ea8e59666d7ef601e5 Mon Sep 17 00:00:00 2001 From: Kayman Date: Fri, 23 Feb 2018 21:03:49 +0900 Subject: [PATCH 07/18] added missing parameter --- ball_detector/launch/ball_detector_params_default.yaml | 4 ++-- ball_detector/msg/BallDetectorParams.msg | 1 + ball_detector/src/ball_detector.cpp | 3 +++ 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ball_detector/launch/ball_detector_params_default.yaml b/ball_detector/launch/ball_detector_params_default.yaml index a97969a..be36dee 100644 --- a/ball_detector/launch/ball_detector_params_default.yaml +++ b/ball_detector/launch/ball_detector_params_default.yaml @@ -12,7 +12,7 @@ filter_s_min: 220 filter_s_max: 255 filter_v_min: 30 filter_v_max: 255 -use_second_filter: true +use_second_filter: false filter2_h_min: 30 filter2_h_max: 355 filter2_s_min: 0 @@ -20,4 +20,4 @@ filter2_s_max: 40 filter2_v_min: 200 filter2_v_max: 255 ellipse_size: 1 -filter_debug: false \ No newline at end of file +filter_debug: false diff --git a/ball_detector/msg/BallDetectorParams.msg b/ball_detector/msg/BallDetectorParams.msg index e72a4ad..cd1c210 100644 --- a/ball_detector/msg/BallDetectorParams.msg +++ b/ball_detector/msg/BallDetectorParams.msg @@ -14,3 +14,4 @@ uint32 filter_s_min # 0 - 255 uint32 filter_s_max uint32 filter_v_min # 0 - 255 uint32 filter_v_max +uint32 ellipse_size # 1 - 9 diff --git a/ball_detector/src/ball_detector.cpp b/ball_detector/src/ball_detector.cpp index 945654b..a38c2b7 100644 --- a/ball_detector/src/ball_detector.cpp +++ b/ball_detector/src/ball_detector.cpp @@ -302,6 +302,7 @@ bool BallDetector::setParamCallback(ball_detector::SetParameters::Request &req, 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; + params_config_.ellipse_size = req.params.ellipse_size; saveConfig(); @@ -326,6 +327,7 @@ bool BallDetector:: getParamCallback(ball_detector::GetParameters::Request &req, 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; + res.returns.ellipse_size = params_config_.ellipse_size; return true; } @@ -400,6 +402,7 @@ void BallDetector::publishParam() params.filter_s_max = params_config_.filter_threshold.s_max; params.filter_v_min = params_config_.filter_threshold.v_min; params.filter_v_max = params_config_.filter_threshold.v_max; + params.filter_v_max = params_config_.ellipse_size; param_pub_.publish(params); } From 390866a7557ca2e8f49430f890056fa9ed204953 Mon Sep 17 00:00:00 2001 From: Kayman Date: Fri, 23 Feb 2018 21:04:24 +0900 Subject: [PATCH 08/18] changed default params --- ball_detector/launch/ball_detector_params.yaml | 14 +++++++------- .../launch/ball_detector_params_default.yaml | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ball_detector/launch/ball_detector_params.yaml b/ball_detector/launch/ball_detector_params.yaml index 86bd8e9..dd4c366 100644 --- a/ball_detector/launch/ball_detector_params.yaml +++ b/ball_detector/launch/ball_detector_params.yaml @@ -6,18 +6,18 @@ min_circle_dist: 100 hough_accum_th: 28 min_radius: 20 max_radius: 300 -filter_h_min: 350 -filter_h_max: 20 -filter_s_min: 230 +filter_h_min: 247 +filter_h_max: 221 +filter_s_min: 220 filter_s_max: 255 filter_v_min: 30 filter_v_max: 255 use_second_filter: false filter2_h_min: 40 filter2_h_max: 345 -filter2_s_min: 0 -filter2_s_max: 44 +filter2_s_min: 1 +filter2_s_max: 45 filter2_v_min: 90 filter2_v_max: 255 -ellipse_size: 3 -filter_debug: false \ No newline at end of file +ellipse_size: 5 +filter_debug: true \ No newline at end of file diff --git a/ball_detector/launch/ball_detector_params_default.yaml b/ball_detector/launch/ball_detector_params_default.yaml index a97969a..439d4f1 100644 --- a/ball_detector/launch/ball_detector_params_default.yaml +++ b/ball_detector/launch/ball_detector_params_default.yaml @@ -10,9 +10,9 @@ filter_h_min: 350 filter_h_max: 20 filter_s_min: 220 filter_s_max: 255 -filter_v_min: 30 +filter_v_min: 80 filter_v_max: 255 -use_second_filter: true +use_second_filter: false filter2_h_min: 30 filter2_h_max: 355 filter2_s_min: 0 From a6480f8c802b4b37f4b0029e34704753ba44491c Mon Sep 17 00:00:00 2001 From: Kayman Date: Fri, 23 Feb 2018 21:14:05 +0900 Subject: [PATCH 09/18] fixed typo --- ball_detector/src/ball_detector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ball_detector/src/ball_detector.cpp b/ball_detector/src/ball_detector.cpp index a38c2b7..fb37112 100644 --- a/ball_detector/src/ball_detector.cpp +++ b/ball_detector/src/ball_detector.cpp @@ -402,7 +402,7 @@ void BallDetector::publishParam() params.filter_s_max = params_config_.filter_threshold.s_max; params.filter_v_min = params_config_.filter_threshold.v_min; params.filter_v_max = params_config_.filter_threshold.v_max; - params.filter_v_max = params_config_.ellipse_size; + params.ellipse_size = params_config_.ellipse_size; param_pub_.publish(params); } From a4eec6160ee8ffe838acce98457b9ed4cfcec012 Mon Sep 17 00:00:00 2001 From: Kayman Date: Mon, 26 Feb 2018 19:38:33 +0900 Subject: [PATCH 10/18] changed a launch file in order to move the camera setting to op3_camera_setting package --- .../launch/ball_detector_from_usb_cam.launch | 28 ++++++------------- 1 file changed, 9 insertions(+), 19 deletions(-) diff --git a/ball_detector/launch/ball_detector_from_usb_cam.launch b/ball_detector/launch/ball_detector_from_usb_cam.launch index 0809458..e84838b 100644 --- a/ball_detector/launch/ball_detector_from_usb_cam.launch +++ b/ball_detector/launch/ball_detector_from_usb_cam.launch @@ -4,33 +4,23 @@ - - - - - - - - - - - - - + + + + + + + + + From 6de2df04572e77dfc96a4bc471ee1f60da150d35 Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 27 Feb 2018 10:24:54 +0900 Subject: [PATCH 11/18] added callback function for control on the web --- op3_demo/include/op3_demo/action_demo.h | 2 + op3_demo/include/op3_demo/vision_demo.h | 1 + op3_demo/src/action/action_demo.cpp | 17 ++++++++ op3_demo/src/demo_node.cpp | 55 +++++++++++++++++++++++++ op3_demo/src/soccer/soccer_demo.cpp | 2 +- op3_demo/src/vision/vision_demo.cpp | 39 +++++++----------- 6 files changed, 92 insertions(+), 24 deletions(-) diff --git a/op3_demo/include/op3_demo/action_demo.h b/op3_demo/include/op3_demo/action_demo.h index bae084b..f96e6ef 100644 --- a/op3_demo/include/op3_demo/action_demo.h +++ b/op3_demo/include/op3_demo/action_demo.h @@ -88,12 +88,14 @@ class ActionDemo : public OPDemo void actionSetNameCallback(const std_msgs::String::ConstPtr& msg); void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); + void demoCommandCallback(const std_msgs::String::ConstPtr &msg); ros::Publisher module_control_pub_; ros::Publisher motion_index_pub_; ros::Publisher play_sound_pub_; ros::Subscriber buttuon_sub_; + ros::Subscriber demo_command_sub_; ros::ServiceClient is_running_client_; diff --git a/op3_demo/include/op3_demo/vision_demo.h b/op3_demo/include/op3_demo/vision_demo.h index b50918d..75d90ac 100644 --- a/op3_demo/include/op3_demo/vision_demo.h +++ b/op3_demo/include/op3_demo/vision_demo.h @@ -56,6 +56,7 @@ class VisionDemo : public OPDemo void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); void facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg); + void demoCommandCallback(const std_msgs::String::ConstPtr &msg); void setModuleToDemo(const std::string &module_name); diff --git a/op3_demo/src/action/action_demo.cpp b/op3_demo/src/action/action_demo.cpp index 68b84ff..c14339b 100644 --- a/op3_demo/src/action/action_demo.cpp +++ b/op3_demo/src/action/action_demo.cpp @@ -37,6 +37,8 @@ ActionDemo::ActionDemo() std::string default_play_list = "default"; play_list_name_ = nh.param("action_script_play_list", default_play_list); + demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, &ActionDemo::demoCommandCallback, this); + parseActionScript (script_path_); boost::thread queue_thread = boost::thread(boost::bind(&ActionDemo::callbackThread, this)); @@ -360,4 +362,19 @@ void ActionDemo::setModuleToDemo(const std::string &module_name) std::cout << "enable module : " << module_name << std::endl; } +void ActionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) +{ + if (enable_ == false) + return; + + if (msg->data == "start") + { + resumeProcess(); + } + else if (msg->data == "stop") + { + pauseProcess(); + } +} + } /* namespace robotis_op */ diff --git a/op3_demo/src/demo_node.cpp b/op3_demo/src/demo_node.cpp index fd6742a..19b0643 100644 --- a/op3_demo/src/demo_node.cpp +++ b/op3_demo/src/demo_node.cpp @@ -41,6 +41,8 @@ void setLED(int led); bool checkManagerRunning(std::string& manager_name); void dxlTorqueChecker(); +void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg); + const int SPIN_RATE = 30; const bool DEBUG_PRINT = false; @@ -73,6 +75,7 @@ int main(int argc, char **argv) led_pub = nh.advertise("/robotis/sync_write_item", 0); dxl_torque_pub = nh.advertise("/robotis/dxl_torque", 0); ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); + ros::Subscriber mode_command_sub = nh.subscribe("/robotis/command_mode", 1, demoModeCommandCallback); default_mp3_path = ros::package::getPath("op3_demo") + "/Data/mp3/"; @@ -313,3 +316,55 @@ void dxlTorqueChecker() dxl_torque_pub.publish(check_msg); } + +void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) +{ + // In demo mode + if (current_status != Ready) + { + if (msg->data == "ready") + { + // go to mode selection status + desired_status = Ready; + apply_desired = true; + + playSound(default_mp3_path + "Demonstration ready mode.mp3"); + setLED(0x01 | 0x02 | 0x04); + } + } + // In ready mode + else + { + if(msg->data == "soccer") + { + desired_status = SoccerDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start soccer demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + else if(msg->data == "vision") + { + desired_status = VisionDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start vision processing demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + else if(msg->data == "action") + { + desired_status = ActionDemo; + apply_desired = true; + + // play sound + dxlTorqueChecker(); + playSound(default_mp3_path + "Start motion demonstration.mp3"); + ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); + } + } +} + diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index d63622e..c361764 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -208,7 +208,7 @@ void SoccerDemo::callbackThread() rgb_led_pub_ = nh.advertise("/robotis/sync_write_item", 0); buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &SoccerDemo::buttonHandlerCallback, this); - demo_command_sub_ = nh.subscribe("/ball_tracker/command", 1, &SoccerDemo::demoCommandCallback, this); + demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, &SoccerDemo::demoCommandCallback, this); imu_data_sub_ = nh.subscribe("/robotis/open_cr/imu", 1, &SoccerDemo::imuDataCallback, this); is_running_client_ = nh.serviceClient("/robotis/action/is_running"); diff --git a/op3_demo/src/vision/vision_demo.cpp b/op3_demo/src/vision/vision_demo.cpp index 5058ada..fbe9060 100644 --- a/op3_demo/src/vision/vision_demo.cpp +++ b/op3_demo/src/vision/vision_demo.cpp @@ -141,29 +141,7 @@ void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) if (msg->data == "start") { -// switch (play_status_) -// { -// case PlayAction: -// { -// pauseProcess(); -// break; -// } -// -// case PauseAction: -// { -// resumeProcess(); -// break; -// } -// -// case StopAction: -// { -// resumeProcess(); -// break; -// } -// -// default: -// break; -// } + } else if (msg->data == "mode") { @@ -171,6 +149,21 @@ void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) } } +void VisionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) +{ + if (enable_ == false) + return; + + if (msg->data == "start") + { + + } + else if (msg->data == "stop") + { + + } +} + void VisionDemo::setModuleToDemo(const std::string &module_name) { std_msgs::String control_msg; From 2549fe19b9cab43c646fbcaa5fdff29255a8d7cc Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 27 Feb 2018 10:27:39 +0900 Subject: [PATCH 12/18] changed topic name --- op3_demo/src/demo_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/op3_demo/src/demo_node.cpp b/op3_demo/src/demo_node.cpp index 19b0643..91429c1 100644 --- a/op3_demo/src/demo_node.cpp +++ b/op3_demo/src/demo_node.cpp @@ -75,7 +75,7 @@ int main(int argc, char **argv) led_pub = nh.advertise("/robotis/sync_write_item", 0); dxl_torque_pub = nh.advertise("/robotis/dxl_torque", 0); ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); - ros::Subscriber mode_command_sub = nh.subscribe("/robotis/command_mode", 1, demoModeCommandCallback); + ros::Subscriber mode_command_sub = nh.subscribe("/robotis/mode_command", 1, demoModeCommandCallback); default_mp3_path = ros::package::getPath("op3_demo") + "/Data/mp3/"; @@ -256,7 +256,7 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) case ActionDemo: playSound(default_mp3_path + "Interactive motion mode.mp3"); - setLED(0x04); + setLED(0x04);ball_tracker break; default: From 9dcb075e329d229091ac16ff70a22dcef77465bb Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 27 Feb 2018 10:28:19 +0900 Subject: [PATCH 13/18] changed parameter setting value --- .../launch/ball_detector_params.yaml | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/ball_detector/launch/ball_detector_params.yaml b/ball_detector/launch/ball_detector_params.yaml index dd4c366..b7c5ec4 100644 --- a/ball_detector/launch/ball_detector_params.yaml +++ b/ball_detector/launch/ball_detector_params.yaml @@ -6,18 +6,18 @@ min_circle_dist: 100 hough_accum_th: 28 min_radius: 20 max_radius: 300 -filter_h_min: 247 -filter_h_max: 221 -filter_s_min: 220 +filter_h_min: 350 +filter_h_max: 20 +filter_s_min: 222 filter_s_max: 255 -filter_v_min: 30 +filter_v_min: 65 filter_v_max: 255 use_second_filter: false -filter2_h_min: 40 -filter2_h_max: 345 -filter2_s_min: 1 -filter2_s_max: 45 -filter2_v_min: 90 +filter2_h_min: 30 +filter2_h_max: 355 +filter2_s_min: 0 +filter2_s_max: 40 +filter2_v_min: 200 filter2_v_max: 255 -ellipse_size: 5 -filter_debug: true \ No newline at end of file +ellipse_size: 2 +filter_debug: false \ No newline at end of file From af6206c321344021518f20a5f7c4eb129e44e486 Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 27 Feb 2018 10:32:56 +0900 Subject: [PATCH 14/18] fixed minor bug --- op3_demo/src/demo_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/op3_demo/src/demo_node.cpp b/op3_demo/src/demo_node.cpp index 91429c1..8039aff 100644 --- a/op3_demo/src/demo_node.cpp +++ b/op3_demo/src/demo_node.cpp @@ -256,7 +256,7 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) case ActionDemo: playSound(default_mp3_path + "Interactive motion mode.mp3"); - setLED(0x04);ball_tracker + setLED(0x04); break; default: From f8e7806adb60b55cf4befb0edb47260d827408b2 Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 27 Feb 2018 10:59:27 +0900 Subject: [PATCH 15/18] changed delay to wait for changing module --- op3_demo/src/action/action_demo.cpp | 2 +- op3_demo/src/soccer/soccer_demo.cpp | 4 ++-- op3_demo/src/vision/vision_demo.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/op3_demo/src/action/action_demo.cpp b/op3_demo/src/action/action_demo.cpp index c14339b..67bb393 100644 --- a/op3_demo/src/action/action_demo.cpp +++ b/op3_demo/src/action/action_demo.cpp @@ -53,7 +53,7 @@ void ActionDemo::setDemoEnable() { setModuleToDemo("action_module"); - usleep(10 * 1000); + usleep(100 * 1000); enable_ = true; diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index c361764..d4f2410 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -414,7 +414,7 @@ void SoccerDemo::startSoccerMode() { setModuleToDemo("action_module"); - usleep(10 * 1000); + usleep(100 * 1000); playMotion(WalkingReady); @@ -422,7 +422,7 @@ void SoccerDemo::startSoccerMode() setBodyModuleToDemo("walking_module"); - usleep(10 * 1000); + usleep(20 * 1000); ROS_INFO("Start Soccer Demo"); on_following_ball_ = true; diff --git a/op3_demo/src/vision/vision_demo.cpp b/op3_demo/src/vision/vision_demo.cpp index fbe9060..7de7362 100644 --- a/op3_demo/src/vision/vision_demo.cpp +++ b/op3_demo/src/vision/vision_demo.cpp @@ -52,7 +52,7 @@ void VisionDemo::setDemoEnable() setModuleToDemo("head_control_module"); - usleep(10 * 1000); + usleep(20 * 1000); enable_ = true; face_tracker_.startTracking(); From 6fe214d9b050b814d0b31dd23f86fa96d61eae9f Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 27 Feb 2018 11:14:05 +0900 Subject: [PATCH 16/18] fixed wrong path of yaml --- op3_demo/src/soccer/soccer_demo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index d4f2410..bd887b7 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -42,7 +42,7 @@ SoccerDemo::SoccerDemo() ros::NodeHandle nh(ros::this_node::getName()); - std::string default_path = ros::package::getPath("op3_gui_demo") + "/config/demo_config.yaml"; + std::string default_path = ros::package::getPath("op3_gui_demo") + "/config/gui_config.yaml"; std::string path = nh.param("demo_config", default_path); parseJointNameFromYaml(path); From 251b9c8e197f09071de64ea916dbd8701a96a3ff Mon Sep 17 00:00:00 2001 From: Kayman Date: Tue, 27 Feb 2018 17:16:15 +0900 Subject: [PATCH 17/18] changed action stop --- op3_demo/include/op3_demo/action_demo.h | 1 + op3_demo/src/action/action_demo.cpp | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/op3_demo/include/op3_demo/action_demo.h b/op3_demo/include/op3_demo/action_demo.h index f96e6ef..bac1eab 100644 --- a/op3_demo/include/op3_demo/action_demo.h +++ b/op3_demo/include/op3_demo/action_demo.h @@ -55,6 +55,7 @@ class ActionDemo : public OPDemo PlayAction = 1, PauseAction = 2, StopAction = 3, + ReadyAction = 4, }; const int SPIN_RATE; diff --git a/op3_demo/src/action/action_demo.cpp b/op3_demo/src/action/action_demo.cpp index 67bb393..76a238c 100644 --- a/op3_demo/src/action/action_demo.cpp +++ b/op3_demo/src/action/action_demo.cpp @@ -110,6 +110,8 @@ void ActionDemo::process() stopMP3(); brakeAction(); + play_status_ = ReadyAction; + break; } @@ -118,6 +120,8 @@ void ActionDemo::process() stopMP3(); stopAction(); + play_status_ = ReadyAction; + break; } From f9e9dc0cd6780a69fd39f4ab8717f5da1a8d9391 Mon Sep 17 00:00:00 2001 From: Kayman Date: Wed, 28 Feb 2018 10:12:58 +0900 Subject: [PATCH 18/18] changed op3_demo launch file --- ball_detector/launch/ball_detector_params.yaml | 6 +++--- op3_demo/launch/demo.launch | 3 +-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ball_detector/launch/ball_detector_params.yaml b/ball_detector/launch/ball_detector_params.yaml index b7c5ec4..d6310e1 100644 --- a/ball_detector/launch/ball_detector_params.yaml +++ b/ball_detector/launch/ball_detector_params.yaml @@ -8,9 +8,9 @@ min_radius: 20 max_radius: 300 filter_h_min: 350 filter_h_max: 20 -filter_s_min: 222 +filter_s_min: 220 filter_s_max: 255 -filter_v_min: 65 +filter_v_min: 80 filter_v_max: 255 use_second_filter: false filter2_h_min: 30 @@ -19,5 +19,5 @@ filter2_s_min: 0 filter2_s_max: 40 filter2_v_min: 200 filter2_v_max: 255 -ellipse_size: 2 +ellipse_size: 1 filter_debug: false \ No newline at end of file diff --git a/op3_demo/launch/demo.launch b/op3_demo/launch/demo.launch index 17db508..391a9f3 100644 --- a/op3_demo/launch/demo.launch +++ b/op3_demo/launch/demo.launch @@ -18,8 +18,7 @@ - - +