BRSRI/bellande_humanoid_robot_intelligence_control_system_architecture
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Merge branch 'feature_web_setting' of https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo into feature_web_setting

Browse Source
This commit is contained in:
Kayman 2018-02-23 20:33:42 +09:00
commit d56c6d48e4
5 changed files with 110 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ball_detector/CMakeLists.txt
View File

@ -8,6 +8,7 @@ project(ball_detector)
# Packages
################################################################################
find_package(catkin REQUIRED COMPONENTS
roslib
cv_bridge
geometry_msgs
image_transport

5
ball_detector/include/ball_detector/ball_detector.h
View File

@ -26,6 +26,7 @@
//ros dependencies
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <image_transport/image_transport.h>
@ -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_;

16
ball_detector/launch/ball_detector_params_default.yaml
View File

@ -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

4
ball_detector/package.xml
View File

@ -11,7 +11,8 @@
<author email="kmjung@robotis.com">kayman</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roslib</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>image_transport</build_depend>
@ -20,6 +21,7 @@
<build_depend>std_msgs</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>roslib</run_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>image_transport</run_depend>

110
ball_detector/src/ball_detector.cpp
View File

@ -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<ball_detector::BallDetectorParams>("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<int>();
params_config_.gaussian_blur_sigma = doc["gaussian_blur_sigma"].as<double>();
params_config_.canny_edge_th = doc["canny_edge_th"].as<double>();
params_config_.hough_accum_resolution = doc["hough_accum_resolution"].as<double>();
params_config_.min_circle_dist = doc["min_circle_dist"].as<double>();
params_config_.hough_accum_th = doc["hough_accum_th"].as<double>();
params_config_.min_radius = doc["min_radius"].as<int>();
params_config_.max_radius = doc["max_radius"].as<int>();
params_config_.filter_threshold.h_min = doc["filter_h_min"].as<int>();
params_config_.filter_threshold.h_max = doc["filter_h_max"].as<int>();
params_config_.filter_threshold.s_min = doc["filter_s_min"].as<int>();
params_config_.filter_threshold.s_max = doc["filter_s_max"].as<int>();
params_config_.filter_threshold.v_min = doc["filter_v_min"].as<int>();
params_config_.filter_threshold.v_max = doc["filter_v_max"].as<int>();
params_config_.use_second_filter = doc["use_second_filter"].as<bool>();
params_config_.filter2_threshold.h_min = doc["filter2_h_min"].as<int>();
params_config_.filter2_threshold.h_max = doc["filter2_h_max"].as<int>();
params_config_.filter2_threshold.s_min = doc["filter2_s_min"].as<int>();
params_config_.filter2_threshold.s_max = doc["filter2_s_max"].as<int>();
params_config_.filter2_threshold.v_min = doc["filter2_v_min"].as<int>();
params_config_.filter2_threshold.v_max = doc["filter2_v_max"].as<int>();
params_config_.ellipse_size = doc["ellipse_size"].as<int>();
params_config_.debug = doc["filter_debug"].as<bool>();
// 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)