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

Repository split from ROBOTIS-OP3

Browse Source
This commit is contained in:
Kayman
2017-06-01 14:09:16 +09:00
commit 2ba4510a2b
93 changed files with 7061 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

176
ball_detector/CMakeLists.txt Normal file
View File

@@ -0,0 +1,176 @@
cmake_minimum_required(VERSION 2.8.3)
project(ball_detector)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
cv_bridge
geometry_msgs
image_transport
roscpp
rospy
std_msgs
dynamic_reconfigure
message_generation
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependencies might have been
## pulled in transitively but can be declared for certainty nonetheless:
## * add a build_depend tag for "message_generation"
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
circleSetStamped.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
geometry_msgs std_msgs
)
#dynamic reconfigure cfg file
generate_dynamic_reconfigure_options(cfg/detector_params.cfg)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
# LIBRARIES ball_detector
CATKIN_DEPENDS cv_bridge geometry_msgs image_transport roscpp rospy std_msgs dynamic_reconfigure
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a cpp library
#add_library(ball_detector
# src/BallDetector.cpp
# src/${PROJECT_NAME}/BallDetector.cpp
#)
## Declare a cpp executable
add_executable(ball_detector_node
src/ball_detector.cpp
src/ball_detector_node.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(ball_detector_node ball_detector_generate_messages_cpp)
add_dependencies(ball_detector_node ${PROJECT_NAME}_gencfg)
## Specify libraries to link a library or executable target against
target_link_libraries(ball_detector_node
${catkin_LIBRARIES}
yaml-cpp
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ball_detector ball_detector_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ball_detector.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

33
ball_detector/cfg/detector_params.cfg Normal file
View File

@@ -0,0 +1,33 @@
#!/usr/bin/env python
PACKAGE='ball_detector'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Reconfiguration levexl Description Default Min Max
gen.add("gaussian_blur_size",int_t , -1, "Size of Gaussian Blur Kernel (odd value!)", 7, 1, 11)
gen.add("gaussian_blur_sigma",double_t , -1, "Std deviation of Gaussian Blur Kernel", 2, 1, 5)
gen.add("canny_edge_th",double_t , -1, "Threshold of the edge detector", 50, 50, 200)
gen.add("hough_accum_resolution",double_t , -1, "Resolution of the Hough accumulator, in terms of inverse ratio of image resolution", 2, 1, 8)
gen.add("min_circle_dist",double_t , -1, "Minimum distance between circles", 40, 10, 200)
gen.add("hough_accum_th",double_t , -1, "Accumulator threshold to decide circle detection", 15, 10, 200)
gen.add("min_radius",int_t , -1, "Minimum circle radius allowed", 20, 10, 200)
gen.add("max_radius",int_t , -1, "Maximum circle radius allowed", 150, 100, 600)
gen.add("filter_h_min",int_t , -1, "Threshold of H filter", 180, 0, 359)
gen.add("filter_h_max",int_t , -1, "Threshold of H filter", 245, 0, 359)
gen.add("filter_s_min",int_t , -1, "Threshold of S filter", 200, 0, 255)
gen.add("filter_s_max",int_t , -1, "Threshold of S filter", 255, 0, 255)
gen.add("filter_v_min",int_t , -1, "Threshold of V filter", 50, 0, 255)
gen.add("filter_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255)
gen.add("use_second_filter", bool_t, 0, "Use second filter", False)
gen.add("filter2_h_min",int_t , -1, "Threshold of H filter", 160, 0, 359)
gen.add("filter2_h_max",int_t , -1, "Threshold of H filter", 255, 0, 359)
gen.add("filter2_s_min",int_t , -1, "Threshold of S filter", 0, 0, 255)
gen.add("filter2_s_max",int_t , -1, "Threshold of S filter", 55, 0, 255)
gen.add("filter2_v_min",int_t , -1, "Threshold of V filter", 180, 0, 255)
gen.add("filter2_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255)
gen.add("ellipse_size",int_t , -1, "Ellipse size", 2, 1, 9)
gen.add("debug_image", bool_t, 0, "Show filtered image to debug", False)
exit(gen.generate(PACKAGE, "ball_detector_node", "detectorParams"))

33
ball_detector/cfg/detector_params_blue.cfg Normal file
View File

@@ -0,0 +1,33 @@
#!/usr/bin/env python
PACKAGE='ball_detector'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Reconfiguration levexl Description Default Min Max
gen.add("gaussian_blur_size",int_t , -1, "Size of Gaussian Blur Kernel (odd value!)", 7, 1, 11)
gen.add("gaussian_blur_sigma",double_t , -1, "Std deviation of Gaussian Blur Kernel", 2, 1, 5)
gen.add("canny_edge_th",double_t , -1, "Threshold of the edge detector", 50, 50, 200)
gen.add("hough_accum_resolution",double_t , -1, "Resolution of the Hough accumulator, in terms of inverse ratio of image resolution", 2, 1, 8)
gen.add("min_circle_dist",double_t , -1, "Minimum distance between circles", 40, 10, 200)
gen.add("hough_accum_th",double_t , -1, "Accumulator threshold to decide circle detection", 15, 10, 200)
gen.add("min_radius",int_t , -1, "Minimum circle radius allowed", 20, 10, 200)
gen.add("max_radius",int_t , -1, "Maximum circle radius allowed", 150, 100, 600)
gen.add("filter_h_min",int_t , -1, "Threshold of H filter", 180, 0, 359)
gen.add("filter_h_max",int_t , -1, "Threshold of H filter", 245, 0, 359)
gen.add("filter_s_min",int_t , -1, "Threshold of S filter", 200, 0, 255)
gen.add("filter_s_max",int_t , -1, "Threshold of S filter", 255, 0, 255)
gen.add("filter_v_min",int_t , -1, "Threshold of V filter", 50, 0, 255)
gen.add("filter_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255)
gen.add("use_second_filter", bool_t, 0, "Use second filter", False)
gen.add("filter2_h_min",int_t , -1, "Threshold of H filter", 160, 0, 359)
gen.add("filter2_h_max",int_t , -1, "Threshold of H filter", 255, 0, 359)
gen.add("filter2_s_min",int_t , -1, "Threshold of S filter", 0, 0, 255)
gen.add("filter2_s_max",int_t , -1, "Threshold of S filter", 55, 0, 255)
gen.add("filter2_v_min",int_t , -1, "Threshold of V filter", 180, 0, 255)
gen.add("filter2_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255)
gen.add("ellipse_size",int_t , -1, "Ellipse size", 2, 1, 9)
gen.add("debug_image", bool_t, 0, "Show filtered image to debug", False)
exit(gen.generate(PACKAGE, "ball_detector_node", "detectorParams"))

33
ball_detector/cfg/detector_params_red.cfg Normal file
View File

@@ -0,0 +1,33 @@
#!/usr/bin/env python
PACKAGE='ball_detector'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Reconfiguration levexl Description Default Min Max
gen.add("gaussian_blur_size",int_t , -1, "Size of Gaussian Blur Kernel (odd value!)", 7, 1, 11)
gen.add("gaussian_blur_sigma",double_t , -1, "Std deviation of Gaussian Blur Kernel", 2, 1, 5)
gen.add("canny_edge_th",double_t , -1, "Threshold of the edge detector", 50, 50, 200)
gen.add("hough_accum_resolution",double_t , -1, "Resolution of the Hough accumulator, in terms of inverse ratio of image resolution", 2, 1, 8)
gen.add("min_circle_dist",double_t , -1, "Minimum distance between circles", 40, 10, 200)
gen.add("hough_accum_th",double_t , -1, "Accumulator threshold to decide circle detection", 15, 10, 200)
gen.add("min_radius",int_t , -1, "Minimum circle radius allowed", 20, 10, 200)
gen.add("max_radius",int_t , -1, "Maximum circle radius allowed", 150, 100, 600)
gen.add("filter_h_min",int_t , -1, "Threshold of H filter", 330, 0, 359)
gen.add("filter_h_max",int_t , -1, "Threshold of H filter", 30, 0, 359)
gen.add("filter_s_min",int_t , -1, "Threshold of S filter", 128, 0, 255)
gen.add("filter_s_max",int_t , -1, "Threshold of S filter", 255, 0, 255)
gen.add("filter_v_min",int_t , -1, "Threshold of V filter", 128, 0, 255)
gen.add("filter_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255)
gen.add("use_second_filter", bool_t, 0, "Use second filter", False)
gen.add("filter2_h_min",int_t , -1, "Threshold of H filter", 160, 0, 359)
gen.add("filter2_h_max",int_t , -1, "Threshold of H filter", 255, 0, 359)
gen.add("filter2_s_min",int_t , -1, "Threshold of S filter", 0, 0, 255)
gen.add("filter2_s_max",int_t , -1, "Threshold of S filter", 55, 0, 255)
gen.add("filter2_v_min",int_t , -1, "Threshold of V filter", 180, 0, 255)
gen.add("filter2_v_max",int_t , -1, "Threshold of V filter", 255, 0, 255)
gen.add("ellipse_size",int_t , -1, "Ellipse size", 5, 1, 9)
gen.add("debug_image", bool_t, 0, "Show filtered image to debug", False)
exit(gen.generate(PACKAGE, "ball_detector_node", "detectorParams"))

157
ball_detector/include/ball_detector/ball_detector.h Normal file
View File

@@ -0,0 +1,157 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: Kayman Jung */
#ifndef _BALL_DETECTOR_H_
#define _BALL_DETECTOR_H_
//std
#include <string>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
//ros dependencies
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <dynamic_reconfigure/server.h>
#include "ball_detector/circleSetStamped.h"
#include "ball_detector/ball_detector_config.h"
#include "ball_detector/detectorParamsConfig.h"
namespace robotis_op
{
class BallDetector
{
public:
BallDetector();
~BallDetector();
//checks if a new image has been received
bool newImage();
//execute circle detection with the cureent image
void process();
//publish the output image (input image + marked circles)
void publishImage();
//publish the circle set data
void publishCircles();
protected:
const static int NOT_FOUND_TH = 30;
//callbacks to image subscription
void imageCallback(const sensor_msgs::ImageConstPtr & msg);
//callbacks to camera info subscription
void cameraInfoCallback(const sensor_msgs::CameraInfo & msg);
void dynParamCallback(ball_detector::detectorParamsConfig &config, uint32_t level);
void enableCallback(const std_msgs::Bool::ConstPtr &msg);
void printConfig();
void saveConfig();
void setInputImage(const cv::Mat & inIm);
void getOutputImage(cv::Mat & outIm);
void filterImage();
void makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, int range);
void makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &mask_img);
void inRangeHsv(const cv::Mat &input_img, const HsvFilter &filter_value, cv::Mat &output_img);
void mophology(const cv::Mat &intput_img, cv::Mat &output_img, int ellipse_size);
void houghDetection(const unsigned int imgEncoding);
void drawOutputImage();
//ros node handle
ros::NodeHandle nh_;
ros::Subscriber enable_sub_;
//image publisher/subscriber
image_transport::ImageTransport it_;
image_transport::Publisher image_pub_;
cv_bridge::CvImage cv_img_pub_;
image_transport::Subscriber image_sub_;
cv_bridge::CvImagePtr cv_img_ptr_sub_;
bool enable_;
bool init_param_;
int not_found_count_;
//circle set publisher
ball_detector::circleSetStamped circles_msg_;
ros::Publisher circles_pub_;
//camera info subscriber
sensor_msgs::CameraInfo camera_info_msg_;
ros::Subscriber camera_info_sub_;
ros::Publisher camera_info_pub_;
//dynamic reconfigure
DetectorConfig params_config_;
std::string param_path_;
bool has_path_;
//flag indicating a new image has been received
bool new_image_flag_;
//image time stamp and frame id
ros::Time sub_time_;
std::string image_frame_id_;
//img encoding id
unsigned int img_encoding_;
/** \brief Set of detected circles
*
* Detected circles. For a circle i:
* x_i: circles[i][0]
* y_i: circles[i][1]
* radius_i: circles[i][2]
*
**/
std::vector<cv::Vec3f> circles_;
cv::Mat in_image_;
cv::Mat out_image_;
dynamic_reconfigure::Server<ball_detector::detectorParamsConfig> param_server_;
dynamic_reconfigure::Server<ball_detector::detectorParamsConfig>::CallbackType callback_fnc_;
};
} // namespace robotis_op
#endif // _BALL_DETECTOR_H_

119
ball_detector/include/ball_detector/ball_detector_config.h Normal file
View File

@@ -0,0 +1,119 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: Kayman Jung */
#ifndef _DETECTOR_CONFIG_H_
#define _DETECTOR_CONFIG_H_
namespace robotis_op
{
//constants
const int GAUSSIAN_BLUR_SIZE_DEFAULT = 7;
const double GAUSSIAN_BLUR_SIGMA_DEFAULT = 2;
const double CANNY_EDGE_TH_DEFAULT = 130;
const double HOUGH_ACCUM_RESOLUTION_DEFAULT = 2;
const double MIN_CIRCLE_DIST_DEFAULT = 30;
const double HOUGH_ACCUM_TH_DEFAULT = 120;
const int MIN_RADIUS_DEFAULT = 30;
const int MAX_RADIUS_DEFAULT = 400;
const unsigned int IMG_MONO = 0;
const unsigned int IMG_RGB8 = 1;
const int FILTER_RANGE_DEFAULT_MIN = 160;
const int FILTER_RANGE_DEFAULT_MAX = 255;
const int FILTER_H_MIN_DEFAULT = 0;
const int FILTER_H_MAX_DEFAULT = 30;
const int FILTER_S_MIN_DEFAULT = 0;
const int FILTER_S_MAX_DEFAULT = 255;
const int FILTER_V_MIN_DEFAULT = 0;
const int FILTER_V_MAX_DEFAULT = 255;
const int ELLIPSE_SIZE = 5;
class HsvFilter
{
public:
HsvFilter()
: h_min(FILTER_H_MIN_DEFAULT),
h_max(FILTER_H_MAX_DEFAULT),
s_min(FILTER_S_MIN_DEFAULT),
s_max(FILTER_S_MAX_DEFAULT),
v_min(FILTER_V_MIN_DEFAULT),
v_max(FILTER_V_MAX_DEFAULT)
{
}
int h_min;
int h_max;
int s_min;
int s_max;
int v_min;
int v_max;
};
class DetectorConfig
{
public:
int gaussian_blur_size; // size of gaussian blur kernel mask [pixel]
double gaussian_blur_sigma; // sigma of gaussian blur kernel mask [pixel]
double canny_edge_th; // threshold of the edge detector.
double hough_accum_resolution; // resolution of the Hough accumulator, in terms of inverse ratio of image resolution
double min_circle_dist; // Minimum distance between circles
double hough_accum_th; // accumulator threshold to decide circle detection
int min_radius; // minimum circle radius allowed
int max_radius; // maximum circle radius allowed
HsvFilter filter_threshold; // filter threshold
bool use_second_filter;
HsvFilter filter2_threshold; // filter threshold
int ellipse_size;
bool debug; // to debug log
DetectorConfig()
: canny_edge_th(CANNY_EDGE_TH_DEFAULT),
hough_accum_resolution(HOUGH_ACCUM_RESOLUTION_DEFAULT),
min_circle_dist(MIN_CIRCLE_DIST_DEFAULT),
hough_accum_th(HOUGH_ACCUM_TH_DEFAULT),
min_radius(MIN_RADIUS_DEFAULT),
max_radius(MAX_RADIUS_DEFAULT),
filter_threshold(),
use_second_filter(false),
filter2_threshold(),
ellipse_size(ELLIPSE_SIZE),
debug(false)
{
}
~DetectorConfig()
{
}
};
}
#endif // _DETECTOR_CONFIG_H_

13
ball_detector/launch/ball_detector.launch Normal file
View File

@@ -0,0 +1,13 @@
<?xml version="1.0"?>
<launch>
<arg name="config_path" default="$(find ball_detector)/launch/ball_detector_params.yaml"/>
<!-- ball detector -->
<node pkg="ball_detector" type="ball_detector_node" name="ball_detector_node" args="" output="screen">
<rosparam command="load" file="$(arg config_path)"/>
<param name="yaml_path" type="string" value="$(arg config_path)"/>
<remap from="/ball_detector_node/image_in" to="/usb_cam_node/image_raw" />
<remap from="/ball_detector_node/cameraInfo_in" to="/usb_cam_node/camera_info" />
</node>
</launch>

138
ball_detector/launch/ball_detector.rviz Normal file
View File

@@ -0,0 +1,138 @@
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 355
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Image
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: ""
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/Image
Enabled: true
Image Topic: /ball_detector_node/image_out
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: compressed
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.785398
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.785398
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd00000004000000000000027500000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001d2000001ec0000001600ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003af0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1855
X: 1985
Y: 24

37
ball_detector/launch/ball_detector_from_usb_cam.launch Normal file
View File

@@ -0,0 +1,37 @@
<?xml version="1.0"?>
<!-- Launches an UVC camera, the ball detector and its visualization -->
<launch>
<!-- UVC camera -->
<node pkg="usb_cam" type="usb_cam_node" name="usb_cam_node" output="screen">
<param name="video_device" type="string" value="/dev/video0" />
<param name="image_width" type="int" value="800" />
<param name="image_height" type="int" value="600" />
<!-- <param name="image_width" type="int" value="1600" />
<param name="image_height" type="int" value="896" /> -->
<param name="framerate " type="int" value="30" />
<param name="camera_name" type="string" value="camera" />
<param name="autofocus" type="bool" value="False" />
<param name="autoexposure" type="bool" value="False" />
<param name="auto_white_balance" type="bool" value="False" />
<param name="gain" value="255" />
<param name="brightness" value="64" />
<param name="exposure" value="80" />
</node>
<!-- <param name="gain" value="255" />
<param name="auto_exposure" type="bool" value="False" />
<param name="exposure_absolute" value="1000" />
<param name="brightness" value="127" />
<param name="auto_white_balance" type="bool" value="False" />
<param name="white_balance_temperature" value="2800" /> -->
<!-- <param name="auto_exposure" type="bool" value="False" /> -->
<!-- <param name="exposure_absolute" value="1000" /> -->
<!-- <param name="brightness" value="64" /> -->
<!-- <param name="auto_white_balance" type="bool" value="False" /> -->
<!-- <param name="white_balance_temperature" value="2800" /> -->
<!-- ball detector -->
<include file="$(find ball_detector)/launch/ball_detector.launch" />
</launch>

30
ball_detector/launch/ball_detector_from_uvc.launch Normal file
View File

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<!-- Launches an UVC camera, the ball detector and its visualization -->
<launch>
<!-- UVC camera -->
<node pkg="uvc_camera" type="uvc_camera_node" name="uvc_camera_node" output="screen">
<param name="device" type="string" value="/dev/video0" />
<param name="width" type="int" value="800" />
<param name="height" type="int" value="600" />
<param name="fps" type="int" value="30" />
<param name="auto_gain" value="false" />
<param name="gain" type="int" value="120" />
<param name="exposure" value="100" />
</node>
<!-- <param name="gain" value="255" />
<param name="auto_exposure" type="bool" value="False" />
<param name="exposure_absolute" value="1000" />
<param name="brightness" value="127" />
<param name="auto_white_balance" type="bool" value="False" />
<param name="white_balance_temperature" value="2800" /> -->
<!-- <param name="auto_exposure" type="bool" value="False" /> -->
<!-- <param name="exposure_absolute" value="1000" /> -->
<!-- <param name="brightness" value="64" /> -->
<!-- <param name="auto_white_balance" type="bool" value="False" /> -->
<!-- <param name="white_balance_temperature" value="2800" /> -->
<!-- ball detector -->
<include file="$(find ball_detector)/launch/ball_detector.launch" />
</launch>

23
ball_detector/launch/ball_detector_params.yaml Normal file
View File

@@ -0,0 +1,23 @@
gaussian_blur_size: 7
gaussian_blur_sigma: 2
canny_edge_th: 100
hough_accum_resolution: 1
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_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_v_min: 90
filter2_v_max: 255
ellipse_size: 3
filter_debug: false

23
ball_detector/launch/ball_detector_params_default.yaml Normal file
View File

@@ -0,0 +1,23 @@
gaussian_blur_size: 7
gaussian_blur_sigma: 2.52
canny_edge_th: 100.5
hough_accum_resolution: 1
min_circle_dist: 28.5
hough_accum_th: 26.6
min_radius: 25
max_radius: 150
filter_h_min: 350
filter_h_max: 20
filter_s_min: 90
filter_s_max: 255
filter_v_min: 86
filter_v_max: 255
use_second_filter: true
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: 1
filter_debug: false

23
ball_detector/launch/ball_detector_params_op.yaml Normal file
View File

@@ -0,0 +1,23 @@
gaussian_blur_size: 7
gaussian_blur_sigma: 2.52
canny_edge_th: 100.5
hough_accum_resolution: 1
min_circle_dist: 28.5
hough_accum_th: 26.6
min_radius: 25
max_radius: 150
filter_h_min: 350
filter_h_max: 20
filter_s_min: 90
filter_s_max: 255
filter_v_min: 86
filter_v_max: 255
use_second_filter: true
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: 1
filter_debug: false

9
ball_detector/msg/circleSetStamped.msg Normal file
View File

@@ -0,0 +1,9 @@
# This represents the set of detected circles
#timestamp and frame id of the image frame
std_msgs/Header header
#set of detected circles:
# (circles[i].x, circles[i].y) is the center point in image coordinates
# circles[i].z is the circle radius
geometry_msgs/Point[] circles

42
ball_detector/package.xml Normal file
View File

@@ -0,0 +1,42 @@
<?xml version="1.0"?>
<package>
<name>ball_detector</name>
<version>0.1.0</version>
<description>
This package implements a circle-like shape detector of the input image.
It requires and input image and publish, at frame rate, a marked image
and a stamped array of circle centers and radius.
</description>
<maintainer email="kmjung@robotis.com">kayman</maintainer>
<license>BSD</license>
<!-- <url type="website">http://wiki.ros.org/ball_detector</url> -->
<author email="kmjung@robotis.com">kayman</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>message_runtime</run_depend>
<export>
</export>
</package>

680
ball_detector/src/ball_detector.cpp Normal file
View File

@@ -0,0 +1,680 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: Kayman Jung */
#include <yaml-cpp/yaml.h>
#include <fstream>
#include "ball_detector/ball_detector.h"
namespace robotis_op
{
BallDetector::BallDetector()
: 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_);
if (has_path_)
std::cout << "Path : " << param_path_ << std::endl;
//detector config struct
DetectorConfig detect_config;
//get user parameters from dynamic reconfigure (yaml entries)
nh_.param<int>("gaussian_blur_size", detect_config.gaussian_blur_size, params_config_.gaussian_blur_size);
if (detect_config.gaussian_blur_size % 2 == 0)
detect_config.gaussian_blur_size -= 1;
if (detect_config.gaussian_blur_size <= 0)
detect_config.gaussian_blur_size = 1;
nh_.param<double>("gaussian_blur_sigma", detect_config.gaussian_blur_sigma, params_config_.gaussian_blur_sigma);
nh_.param<double>("canny_edge_th", detect_config.canny_edge_th, params_config_.canny_edge_th);
nh_.param<double>("hough_accum_resolution", detect_config.hough_accum_resolution,
params_config_.hough_accum_resolution);
nh_.param<double>("min_circle_dist", detect_config.min_circle_dist, params_config_.min_circle_dist);
nh_.param<double>("hough_accum_th", detect_config.hough_accum_th, params_config_.hough_accum_th);
nh_.param<int>("min_radius", detect_config.min_radius, params_config_.min_radius);
nh_.param<int>("max_radius", detect_config.max_radius, params_config_.max_radius);
nh_.param<int>("filter_h_min", detect_config.filter_threshold.h_min, params_config_.filter_threshold.h_min);
nh_.param<int>("filter_h_max", detect_config.filter_threshold.h_max, params_config_.filter_threshold.h_max);
nh_.param<int>("filter_s_min", detect_config.filter_threshold.s_min, params_config_.filter_threshold.s_min);
nh_.param<int>("filter_s_max", detect_config.filter_threshold.s_max, params_config_.filter_threshold.s_max);
nh_.param<int>("filter_v_min", detect_config.filter_threshold.v_min, params_config_.filter_threshold.v_min);
nh_.param<int>("filter_v_max", detect_config.filter_threshold.v_max, params_config_.filter_threshold.v_max);
nh_.param<bool>("use_second_filter", detect_config.use_second_filter, params_config_.use_second_filter);
nh_.param<int>("filter2_h_min", detect_config.filter2_threshold.h_min, params_config_.filter2_threshold.h_min);
nh_.param<int>("filter2_h_max", detect_config.filter2_threshold.h_max, params_config_.filter2_threshold.h_max);
nh_.param<int>("filter2_s_min", detect_config.filter2_threshold.s_min, params_config_.filter2_threshold.s_min);
nh_.param<int>("filter2_s_max", detect_config.filter2_threshold.s_max, params_config_.filter2_threshold.s_max);
nh_.param<int>("filter2_v_min", detect_config.filter2_threshold.v_min, params_config_.filter2_threshold.v_min);
nh_.param<int>("filter2_v_max", detect_config.filter2_threshold.v_max, params_config_.filter2_threshold.v_max);
nh_.param<int>("ellipse_size", detect_config.ellipse_size, params_config_.ellipse_size);
nh_.param<bool>("filter_debug", detect_config.debug, params_config_.debug);
//sets publishers
image_pub_ = it_.advertise("image_out", 100);
circles_pub_ = nh_.advertise<ball_detector::circleSetStamped>("circle_set", 100);
camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("camera_info", 100);
//sets subscribers
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);
//initializes newImageFlag
new_image_flag_ = false;
// dynamic_reconfigure
callback_fnc_ = boost::bind(&BallDetector::dynParamCallback, this, _1, _2);
param_server_.setCallback(callback_fnc_);
//sets config and prints it
params_config_ = detect_config;
init_param_ = true;
printConfig();
}
BallDetector::~BallDetector()
{
}
bool BallDetector::newImage()
{
if (new_image_flag_)
{
new_image_flag_ = false;
return true;
}
else
{
return false;
}
}
void BallDetector::process()
{
if(enable_ == false)
return;
if (cv_img_ptr_sub_ != NULL)
{
//sets input image
setInputImage(cv_img_ptr_sub_->image);
// test image filtering
filterImage();
//detect circles
houghDetection(this->img_encoding_);
}
}
void BallDetector::publishImage()
{
if(enable_ == false)
return;
//image_raw topic
cv_img_pub_.header.seq++;
cv_img_pub_.header.stamp = sub_time_;
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;
}
getOutputImage(cv_img_pub_.image);
image_pub_.publish(cv_img_pub_.toImageMsg());
camera_info_pub_.publish(camera_info_msg_);
}
void BallDetector::publishCircles()
{
if(enable_ == false)
return;
if (circles_.size() == 0)
return;
//clears and resize the message
circles_msg_.circles.clear();
circles_msg_.circles.resize(circles_.size());
//fill header
circles_msg_.header.seq++;
circles_msg_.header.stamp = sub_time_;
circles_msg_.header.frame_id = "detector"; //To do: get frame_id from input image
//fill circle data
// top(-1), bottom(+1)
// left(-1), right(+1)
for (int idx = 0; idx < circles_.size(); idx++)
{
circles_msg_.circles[idx].x = circles_[idx][0] / in_image_.cols * 2 - 1; // x (-1 ~ 1)
circles_msg_.circles[idx].y = circles_[idx][1] / in_image_.rows * 2 - 1; // y (-1 ~ 1)
circles_msg_.circles[idx].z = circles_[idx][2]; // radius
}
//publish message
circles_pub_.publish(circles_msg_);
//ball_detecting process time
//ros::Duration _process_dur = ros::Time::now() - sub_time_;
//ROS_INFO_STREAM("== Ball detecting processing time : " << _process_dur);
}
void BallDetector::enableCallback(const std_msgs::Bool::ConstPtr &msg)
{
enable_ = msg->data;
}
void BallDetector::imageCallback(const sensor_msgs::ImageConstPtr & msg)
{
if(enable_ == false)
return;
try
{
if (msg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
this->img_encoding_ = IMG_MONO;
if (msg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)
this->img_encoding_ = IMG_RGB8;
this->cv_img_ptr_sub_ = cv_bridge::toCvCopy(msg, msg->encoding);
} catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
//indicates a new image is available
this->sub_time_ = msg->header.stamp;
this->image_frame_id_ = msg->header.frame_id;
this->new_image_flag_ = true;
return;
}
void BallDetector::dynParamCallback(ball_detector::detectorParamsConfig &config, uint32_t level)
{
params_config_.gaussian_blur_size = config.gaussian_blur_size;
params_config_.gaussian_blur_sigma = config.gaussian_blur_sigma;
params_config_.canny_edge_th = config.canny_edge_th;
params_config_.hough_accum_resolution = config.hough_accum_resolution;
params_config_.min_circle_dist = config.min_circle_dist;
params_config_.hough_accum_th = config.hough_accum_th;
params_config_.min_radius = config.min_radius;
params_config_.max_radius = config.max_radius;
params_config_.filter_threshold.h_min = config.filter_h_min;
params_config_.filter_threshold.h_max = config.filter_h_max;
params_config_.filter_threshold.s_min = config.filter_s_min;
params_config_.filter_threshold.s_max = config.filter_s_max;
params_config_.filter_threshold.v_min = config.filter_v_min;
params_config_.filter_threshold.v_max = config.filter_v_max;
params_config_.use_second_filter = config.use_second_filter;
params_config_.filter2_threshold.h_min = config.filter2_h_min;
params_config_.filter2_threshold.h_max = config.filter2_h_max;
params_config_.filter2_threshold.s_min = config.filter2_s_min;
params_config_.filter2_threshold.s_max = config.filter2_s_max;
params_config_.filter2_threshold.v_min = config.filter2_v_min;
params_config_.filter2_threshold.v_max = config.filter2_v_max;
params_config_.ellipse_size = config.ellipse_size;
params_config_.debug = config.debug_image;
// 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();
}
void BallDetector::cameraInfoCallback(const sensor_msgs::CameraInfo & msg)
{
if(enable_ == false)
return;
camera_info_msg_ = msg;
}
void BallDetector::printConfig()
{
if (init_param_ == false)
return;
std::cout << "Detetctor Configuration:" << std::endl << " gaussian_blur_size: "
<< params_config_.gaussian_blur_size << std::endl << " gaussian_blur_sigma: "
<< params_config_.gaussian_blur_sigma << std::endl << " canny_edge_th: " << params_config_.canny_edge_th
<< std::endl << " hough_accum_resolution: " << params_config_.hough_accum_resolution << std::endl
<< " min_circle_dist: " << params_config_.min_circle_dist << std::endl << " hough_accum_th: "
<< params_config_.hough_accum_th << std::endl << " min_radius: " << params_config_.min_radius
<< std::endl << " max_radius: " << params_config_.max_radius << std::endl << " filter_h_min: "
<< params_config_.filter_threshold.h_min << std::endl << " filter_h_max: "
<< params_config_.filter_threshold.h_max << std::endl << " filter_s_min: "
<< params_config_.filter_threshold.s_min << std::endl << " filter_s_max: "
<< params_config_.filter_threshold.s_max << std::endl << " filter_v_min: "
<< params_config_.filter_threshold.v_min << std::endl << " filter_v_max: "
<< params_config_.filter_threshold.v_max << std::endl << " use_second_filter: "
<< params_config_.use_second_filter << std::endl << " filter2_h_min: "
<< params_config_.filter2_threshold.h_min << std::endl << " filter2_h_max: "
<< params_config_.filter2_threshold.h_max << std::endl << " filter2_s_min: "
<< params_config_.filter2_threshold.s_min << std::endl << " filter2_s_max: "
<< params_config_.filter2_threshold.s_max << std::endl << " filter2_v_min: "
<< params_config_.filter2_threshold.v_min << std::endl << " filter2_v_max: "
<< params_config_.filter2_threshold.v_max << std::endl << " ellipse_size: "
<< params_config_.ellipse_size << std::endl << " filter_image_to_debug: " << params_config_.debug
<< std::endl << std::endl;
}
void BallDetector::saveConfig()
{
if (has_path_ == false)
return;
YAML::Emitter yaml_out;
yaml_out << YAML::BeginMap;
yaml_out << YAML::Key << "gaussian_blur_size" << YAML::Value << params_config_.gaussian_blur_size;
yaml_out << YAML::Key << "gaussian_blur_sigma" << YAML::Value << params_config_.gaussian_blur_sigma;
yaml_out << YAML::Key << "canny_edge_th" << YAML::Value << params_config_.canny_edge_th;
yaml_out << YAML::Key << "hough_accum_resolution" << YAML::Value << params_config_.hough_accum_resolution;
yaml_out << YAML::Key << "min_circle_dist" << YAML::Value << params_config_.min_circle_dist;
yaml_out << YAML::Key << "hough_accum_th" << YAML::Value << params_config_.hough_accum_th;
yaml_out << YAML::Key << "min_radius" << YAML::Value << params_config_.min_radius;
yaml_out << YAML::Key << "max_radius" << YAML::Value << params_config_.max_radius;
yaml_out << YAML::Key << "filter_h_min" << YAML::Value << params_config_.filter_threshold.h_min;
yaml_out << YAML::Key << "filter_h_max" << YAML::Value << params_config_.filter_threshold.h_max;
yaml_out << YAML::Key << "filter_s_min" << YAML::Value << params_config_.filter_threshold.s_min;
yaml_out << YAML::Key << "filter_s_max" << YAML::Value << params_config_.filter_threshold.s_max;
yaml_out << YAML::Key << "filter_v_min" << YAML::Value << params_config_.filter_threshold.v_min;
yaml_out << YAML::Key << "filter_v_max" << YAML::Value << params_config_.filter_threshold.v_max;
yaml_out << YAML::Key << "use_second_filter" << YAML::Value << params_config_.use_second_filter;
yaml_out << YAML::Key << "filter2_h_min" << YAML::Value << params_config_.filter2_threshold.h_min;
yaml_out << YAML::Key << "filter2_h_max" << YAML::Value << params_config_.filter2_threshold.h_max;
yaml_out << YAML::Key << "filter2_s_min" << YAML::Value << params_config_.filter2_threshold.s_min;
yaml_out << YAML::Key << "filter2_s_max" << YAML::Value << params_config_.filter2_threshold.s_max;
yaml_out << YAML::Key << "filter2_v_min" << YAML::Value << params_config_.filter2_threshold.v_min;
yaml_out << YAML::Key << "filter2_v_max" << YAML::Value << params_config_.filter2_threshold.v_max;
yaml_out << YAML::Key << "ellipse_size" << YAML::Value << params_config_.ellipse_size;
yaml_out << YAML::Key << "filter_debug" << YAML::Value << params_config_.debug;
yaml_out << YAML::EndMap;
// output to file
std::ofstream fout(param_path_.c_str());
fout << yaml_out.c_str();
}
void BallDetector::setInputImage(const cv::Mat & inIm)
{
in_image_ = inIm.clone();
if (params_config_.debug == false)
out_image_ = in_image_.clone();
}
void BallDetector::getOutputImage(cv::Mat & outIm)
{
this->drawOutputImage();
outIm = out_image_.clone();
}
void BallDetector::filterImage()
{
if (!in_image_.data)
return;
/*
cv::Mat imgCrCb, imgCr;
cv::cvtColor(in_image_, imgCrCb, cv::COLOR_BGR2YCrCb);
std::vector<cv::Mat> _channels;
cv::split(imgCrCb, _channels);
imgCr = _channels[2];
cv::inRange(imgCr, params_config_.filter_h_min, params_config_.filter_h_max, imgCr);
cv::erode(imgCr, imgCr, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
cv::dilate(imgCr, imgCr, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
cv::dilate(imgCr, imgCr, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
cv::erode(imgCr, imgCr, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5)) );
cv::cvtColor(imgCr, in_image_, cv::COLOR_GRAY2RGB);
*/
cv::Mat img_hsv, img_filtered;
cv::cvtColor(in_image_, img_hsv, cv::COLOR_RGB2HSV);
inRangeHsv(img_hsv, params_config_.filter_threshold, img_filtered);
// mophology : open and close
mophology(img_filtered, img_filtered, params_config_.ellipse_size);
if (params_config_.use_second_filter == true)
{
// mask
cv::Mat img_mask;
// mophology : open and close
//int ellipse_size = 30;
//int dilate_size = params_config_.max_radius; // dilate_size = ellipse_size * 10
//cv::erode(img_filtered, img_mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(ellipse_size, ellipse_size)));
//cv::dilate(img_mask, img_mask,
// cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(dilate_size, dilate_size)));
//makeFilterMask(img_filtered, img_mask, ellipse_size);
// check hsv range
cv::Mat img_filtered2;
inRangeHsv(img_hsv, params_config_.filter2_threshold, img_filtered2);
makeFilterMaskFromBall(img_filtered, img_mask);
cv::bitwise_and(img_filtered2, img_mask, img_filtered2);
// mophology(img_filtered2, img_filtered2, params_config_.ellipse_size);
// or
cv::bitwise_or(img_filtered, img_filtered2, img_filtered);
}
mophology(img_filtered, img_filtered, params_config_.ellipse_size);
cv::cvtColor(img_filtered, in_image_, cv::COLOR_GRAY2RGB);
}
void BallDetector::makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, int range)
{
// source_img.
mask_img = cv::Mat::zeros(source_img.rows, source_img.cols, CV_8UC1);
int source_height = source_img.rows;
int source_width = source_img.cols;
// channel : 1
if (source_img.channels() != 1)
return;
for (int i = 0; i < source_height; i++)
{
for (int j = 0; j < source_width; j++)
{
uint8_t pixel = source_img.at<uint8_t>(i, j);
if (pixel == 0)
continue;
for (int mask_i = i - range; mask_i <= i + range; mask_i++)
{
if (mask_i < 0 || mask_i >= source_height)
continue;
for (int mask_j = j - range; mask_j <= j + range; mask_j++)
{
if (mask_j < 0 || mask_j >= source_width)
continue;
mask_img.at<uchar>(mask_i, mask_j, 0) = 255;
}
}
}
}
}
void BallDetector::makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &mask_img)
{
// source_img.
mask_img = cv::Mat::zeros(source_img.rows, source_img.cols, CV_8UC1);
if(circles_.size() == 0)
return;
// channel : 1
if (source_img.channels() != 1)
return;
// for (int i = 0; i < circles_.size(); i++)
// {
// if (circles_[i][0] != -1)
// {
// // _center = cv::Point(cvRound(circles_[_i][0]), cvRound(circles_[_i][1]));
// int center_x = cvRound(circles_[i][0]);
// int center_y = cvRound(circles_[i][1]);
// int radius = cvRound(circles_[i][2]) * 1.5;
//
// for (int mask_i = center_y - radius; mask_i <= center_y + radius; mask_i++)
// {
// if (mask_i < 0 || mask_i >= source_img.rows)
// continue;
//
// int mask_offset = abs(mask_i - center_y) * 0.5;
//
// for (int mask_j = center_x - radius + mask_offset; mask_j <= center_x + radius - mask_offset; mask_j++)
// {
// if (mask_j < 0 || mask_j >= source_img.cols)
// continue;
//
// mask_img.at<uchar>(mask_i, mask_j, 0) = 255;
// }
// }
//
// }
// }
cv::Mat img_labels, stats, centroids;
int numOfLables = cv::connectedComponentsWithStats(source_img, img_labels,
stats, centroids, 8,CV_32S);
for (int j = 1; j < numOfLables; j++)
{
int area = stats.at<int>(j, cv::CC_STAT_AREA);
int left = stats.at<int>(j, cv::CC_STAT_LEFT);
int top = stats.at<int>(j, cv::CC_STAT_TOP);
int width = stats.at<int>(j, cv::CC_STAT_WIDTH);
int height = stats.at<int>(j, cv::CC_STAT_HEIGHT);
int center_x = left + width * 0.5;
int center_y = top + height * 0.5;
int radius = (width + height) * 0.5;
for (int mask_i = center_y - radius; mask_i <= center_y + radius; mask_i++)
{
if (mask_i < 0 || mask_i >= source_img.rows)
continue;
int mask_offset = abs(mask_i - center_y) * 0.5;
for (int mask_j = center_x - radius + mask_offset; mask_j <= center_x + radius - mask_offset; mask_j++)
{
if (mask_j < 0 || mask_j >= source_img.cols)
continue;
mask_img.at<uchar>(mask_i, mask_j, 0) = 255;
}
}
}
}
void BallDetector::inRangeHsv(const cv::Mat &input_img, const HsvFilter &filter_value, cv::Mat &output_img)
{
// 0-360 -> 0-180
int scaled_hue_min = static_cast<int>(filter_value.h_min * 0.5);
int scaled_hue_max = static_cast<int>(filter_value.h_max * 0.5);
if (scaled_hue_min <= scaled_hue_max)
{
cv::Scalar min_value = cv::Scalar(scaled_hue_min, filter_value.s_min, filter_value.v_min, 0);
cv::Scalar max_value = cv::Scalar(scaled_hue_max, filter_value.s_max, filter_value.v_max, 0);
cv::inRange(input_img, min_value, max_value, output_img);
}
else
{
cv::Mat lower_hue_range, upper_hue_range;
cv::Scalar min_value, max_value;
min_value = cv::Scalar(0, filter_value.s_min, filter_value.v_min, 0);
max_value = cv::Scalar(scaled_hue_max, filter_value.s_max, filter_value.v_max, 0);
cv::inRange(input_img, min_value, max_value, lower_hue_range);
min_value = cv::Scalar(scaled_hue_min, filter_value.s_min, filter_value.v_min, 0);
max_value = cv::Scalar(179, filter_value.s_max, filter_value.v_max, 0);
cv::inRange(input_img, min_value, max_value, upper_hue_range);
cv::bitwise_or(lower_hue_range, upper_hue_range, output_img);
}
}
void BallDetector::mophology(const cv::Mat &intput_img, cv::Mat &output_img, int ellipse_size)
{
cv::erode(intput_img, output_img, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(ellipse_size, ellipse_size)));
cv::dilate(output_img, output_img,
cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(ellipse_size * 2, ellipse_size * 2)));
cv::dilate(output_img, output_img,
cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(ellipse_size, ellipse_size)));
cv::erode(output_img, output_img, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(ellipse_size, ellipse_size)));
}
void BallDetector::houghDetection(const unsigned int imgEncoding)
{
cv::Mat gray_image;
std::vector<cv::Vec3f> circles_current;
std::vector<cv::Vec3f> prev_circles = circles_;
//clear previous circles
circles_.clear();
// If input image is RGB, convert it to gray
if (imgEncoding == IMG_RGB8)
cv::cvtColor(in_image_, gray_image, CV_RGB2GRAY);
//Reduce the noise so we avoid false circle detection
cv::GaussianBlur(gray_image, gray_image, cv::Size(params_config_.gaussian_blur_size, params_config_.gaussian_blur_size),
params_config_.gaussian_blur_sigma);
//double hough_accum_th = (not_found_count_ < NOT_FOUND_TH) ?
// params_config_.hough_accum_th : params_config_.hough_accum_th * 0.5;
double hough_accum_th = params_config_.hough_accum_th;
// std::cout << "Not found : " << not_found_count_ << ", value : " << hough_accum_th << std::endl;
//Apply the Hough Transform to find the circles
cv::HoughCircles(gray_image, circles_current, CV_HOUGH_GRADIENT, params_config_.hough_accum_resolution,
params_config_.min_circle_dist, params_config_.canny_edge_th, hough_accum_th,
params_config_.min_radius, params_config_.max_radius);
//set found circles to circles set. Apply some condition if desired.
//for (int ii = 0; ii < circles_current.size(); ii++)
//{
// circles_.push_back(circles_current.at(ii));
// // std::cout << "circle " << ii << ": (" << circles.at(ii)[0] << "," << circles.at(ii)[1] << ")" << std::endl;
//}
if(circles_current.size() == 0)
not_found_count_ += 1;
else
not_found_count_ = 0;
double alpha = 0.2;
for(int ix = 0; ix < circles_current.size(); ix++)
{
cv::Point2d center = cv::Point(circles_current[ix][0], circles_current[ix][1]);
double radius = circles_current[ix][2];
for(int prev_ix = 0; prev_ix < prev_circles.size(); prev_ix++)
{
cv::Point2d prev_center = cv::Point(prev_circles[prev_ix][0], prev_circles[prev_ix][1]);
double prev_radius = prev_circles[prev_ix][2];
cv::Point2d diff = center - prev_center;
double radius_th = std::max(radius, prev_radius) * 0.75;
if(sqrt(diff.dot(diff)) < radius_th)
{
if(abs(radius - prev_radius) < radius_th)
{
// std::cout << "circle - raw " << ix << ": (" << circles_current.at(ix)[0] << "," << circles_current.at(ix)[1] << ")" << std::endl;
circles_current[ix] = circles_current[ix] * alpha + prev_circles[prev_ix] * (1 - alpha);
// std::cout << "circle - lpf " << ix << ": (" << circles_current.at(ix)[0] << "," << circles_current.at(ix)[1] << ")" << std::endl;
}
break;
}
}
circles_.push_back(circles_current[ix]);
}
}
void BallDetector::drawOutputImage()
{
cv::Point center_position;
int radius = 0;
size_t ii;
//draws results to output Image
if (params_config_.debug == true)
out_image_ = in_image_.clone();
for (ii = 0; ii < circles_.size(); ii++)
{
{
// cv::Point _center = cv::Point(cvRound(circles_[ii][0]), cvRound(circles_[ii][1]));
// int _radius = cvRound(circles_[ii][2]);
// cv::circle( out_image_, _center, 5, cv::Scalar(0,0,255), -1, 8, 0 );// circle center in green
// cv::circle( out_image_, _center, _radius, cv::Scalar(0,0,255), 3, 8, 0 );// circle outline in red
int this_radius = cvRound(circles_[ii][2]);
if (this_radius > radius)
{
radius = this_radius;
center_position = cv::Point(cvRound(circles_[ii][0]), cvRound(circles_[ii][1]));
}
}
}
cv::circle(out_image_, center_position, 5, cv::Scalar(0, 0, 255), -1, 8, 0); // circle center in blue
cv::circle(out_image_, center_position, radius, cv::Scalar(0, 0, 255), 3, 8, 0); // circle outline in blue
}
} // namespace robotis_op

69
ball_detector/src/ball_detector_node.cpp Normal file
View File

@@ -0,0 +1,69 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of ROBOTIS nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/
/* Author: Kayman Jung */
#include "ball_detector/ball_detector.h"
//node main
int main(int argc, char **argv)
{
//init ros
ros::init(argc, argv, "ball_detector_node");
//create ros wrapper object
robotis_op::BallDetector detector;
//set node loop rate
ros::Rate loop_rate(30);
//node loop
while (ros::ok())
{
//if new image , do things
if (detector.newImage())
{
detector.process();
detector.publishImage();
detector.publishCircles();
}
//execute pending callbacks
ros::spinOnce();
//relax to fit output rate
loop_rate.sleep();
}
//exit program
return 0;
}