Repository split from ROBOTIS-OP3

.gitignore

@@ -0,0 +1,11 @@
+build
+devel
+bin
+lib
+msg_gen
+srv_gen
+qtcreator-build
+*~
+*.backup
+*.user
+*.autosave

LICENSE

@@ -0,0 +1,26 @@
+Software License Agreement (BSD License)
+ 
+Copyright (c) 2014, ROBOTIS Inc.
+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 ROBOTIS "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 ROBOTIS 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.

ball_detector/CMakeLists.txt

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

ball_detector/cfg/detector_params.cfg

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

ball_detector/cfg/detector_params_blue.cfg

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

ball_detector/cfg/detector_params_red.cfg

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

ball_detector/include/ball_detector/ball_detector.h

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

ball_detector/include/ball_detector/ball_detector_config.h

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

ball_detector/launch/ball_detector.launch

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

ball_detector/launch/ball_detector.rviz

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

ball_detector/launch/ball_detector_from_usb_cam.launch

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

ball_detector/launch/ball_detector_from_uvc.launch

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

ball_detector/launch/ball_detector_params.yaml

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

ball_detector/launch/ball_detector_params_default.yaml

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

ball_detector/launch/ball_detector_params_op.yaml

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

ball_detector/msg/circleSetStamped.msg

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

ball_detector/package.xml

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

ball_detector/src/ball_detector.cpp

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

ball_detector/src/ball_detector_node.cpp

@@ -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;
+}
+

op3_demo/CMakeLists.txt

@@ -0,0 +1,256 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(op3_demo)
+
+## 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
+  roscpp
+  roslib
+  sensor_msgs
+  ball_detector
+  op3_walking_module_msgs
+  cmake_modules
+  robotis_math
+)
+
+## System dependencies are found with CMake's conventions
+find_package(Eigen REQUIRED)
+
+
+## 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 tag for "message_generation"
+##   * 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 dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * 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
+#   Message1.msg
+#   Message2.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
+#   sensor_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.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_tracker
+  CATKIN_DEPENDS roscpp sensor_msgs
+#  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}
+  ${Eigen_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(ball_tracking
+#   src/${PROJECT_NAME}/ball_tracking.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(ball_tracking ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+add_executable(ball_tracker_node 
+  src/ball_tracker_node.cpp
+  src/soccer/ball_tracker.cpp
+)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(ball_tracker_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(ball_tracker_node
+  ${catkin_LIBRARIES}
+  yaml-cpp
+)
+
+add_executable(soccer_demo_node 
+  src/soccer_demo_node.cpp
+  src/soccer/ball_tracker.cpp
+  src/soccer/ball_follower.cpp
+)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(soccer_demo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(soccer_demo_node
+  ${catkin_LIBRARIES}
+  yaml-cpp
+)
+
+add_executable(op_demo_node 
+  src/demo_node.cpp
+  src/soccer/soccer_demo.cpp
+  src/soccer/ball_tracker.cpp
+  src/soccer/ball_follower.cpp
+  src/action/action_demo.cpp
+  src/vision/vision_demo.cpp
+  src/vision/face_tracker.cpp
+)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(op_demo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(op_demo_node
+  ${catkin_LIBRARIES}
+  yaml-cpp
+)
+
+add_executable(self_test_node 
+  src/test_node.cpp
+  src/soccer/soccer_demo.cpp
+  src/soccer/ball_tracker.cpp
+  src/soccer/ball_follower.cpp
+  src/action/action_demo.cpp
+  src/vision/vision_demo.cpp
+  src/vision/face_tracker.cpp
+  src/test/button_test.cpp
+  src/test/mic_test.cpp
+)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(self_test_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(self_test_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_tracking ball_tracking_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_tracking.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)

op3_demo/include/op3_demo/action_demo.h

@@ -0,0 +1,130 @@
+/*******************************************************************************
+ * 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 ACTION_DEMO_H_
+#define ACTION_DEMO_H_
+
+#include <boost/thread.hpp>
+#include <yaml-cpp/yaml.h>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+
+#include "op3_demo/op_demo.h"
+#include "robotis_controller_msgs/JointCtrlModule.h"
+#include "op3_action_module_msgs/IsRunning.h"
+
+namespace robotis_op
+{
+
+class ActionDemo : public OPDemo
+{
+ public:
+  ActionDemo();
+  ~ActionDemo();
+
+  void setDemoEnable();
+  void setDemoDisable();
+
+ protected:
+  enum ActionCommandIndex
+  {
+    BrakeActionCommand = -2,
+    StopActionCommand = -1,
+  };
+
+  enum ActionStatus
+  {
+    PlayAction = 1,
+    PauseAction = 2,
+    StopAction = 3,
+  };
+
+  const int SPIN_RATE;
+  const int DEMO_INIT_POSE;
+
+  void processThread();
+  void callbackThread();
+
+  void process();
+  void startProcess(const std::string &set_name = "default");
+  void resumeProcess();
+  void pauseProcess();
+  void stopProcess();
+
+  void handleStatus();
+
+  void parseActionScript(const std::string &path);
+  bool parseActionScriptSetName(const std::string &path, const std::string &set_name);
+
+  bool playActionWithSound(int motion_index);
+
+  void playMP3(std::string &path);
+  void stopMP3();
+
+  void playAction(int motion_index);
+  void stopAction();
+  void brakeAction();
+  bool isActionRunning();
+
+  void setModuleToDemo(const std::string &module_name);
+
+  void actionSetNameCallback(const std_msgs::String::ConstPtr& msg);
+  void buttonHandlerCallback(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::ServiceClient is_running_client_;
+
+  std::map<int, std::string> action_sound_table_;
+  std::vector<int> play_list_;
+
+  std::string script_path_;
+  std::string play_list_name_;
+  int play_index_;
+
+  bool start_play_;
+  bool stop_play_;
+  bool pause_play_;
+
+  int play_status_;
+};
+
+} /* namespace robotis_op */
+
+#endif /* ACTION_DEMO_H_ */

op3_demo/include/op3_demo/ball_follower.h

@@ -0,0 +1,136 @@
+/*******************************************************************************
+ * 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_FOLLOWER_H_
+#define BALL_FOLLOWER_H_
+
+#include <math.h>
+#include <yaml-cpp/yaml.h>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Int32.h>
+#include <sensor_msgs/JointState.h>
+
+#include "robotis_controller_msgs/JointCtrlModule.h"
+#include "ball_detector/circleSetStamped.h"
+#include "op3_walking_module_msgs/WalkingParam.h"
+#include "op3_walking_module_msgs/GetWalkingParam.h"
+
+namespace robotis_op
+{
+
+// following the ball using walking
+class BallFollower
+{
+ public:
+  enum
+  {
+    NotFound = 0,
+    OnRight = 1,
+    OnLeft = 2,
+  };
+
+  BallFollower();
+  ~BallFollower();
+
+  bool processFollowing(double x_angle, double y_angle, double ball_size);
+  void waitFollowing();
+  void startFollowing();
+  void stopFollowing();
+
+  int getBallPosition()
+  {
+    return approach_ball_position_;
+  }
+
+ protected:
+  const double CAMERA_HEIGHT;
+  const int NOT_FOUND_THRESHOLD;
+  const double FOV_WIDTH;
+  const double FOV_HEIGHT;
+  const double MAX_FB_STEP;
+  const double MAX_RL_TURN;
+  const double MIN_FB_STEP;
+  const double MIN_RL_TURN;
+  const double UNIT_FB_STEP;
+  const double UNIT_RL_TURN;
+
+  const double SPOT_FB_OFFSET;
+  const double SPOT_RL_OFFSET;
+  const double SPOT_ANGLE_OFFSET;
+
+  void currentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
+  void setWalkingCommand(const std::string &command);
+  void setWalkingParam(double x_move, double y_move, double rotation_angle, bool balance = true);
+  bool getWalkingParam();
+
+  bool debug_print_;
+
+  //ros node handle
+  ros::NodeHandle nh_;
+
+  //image publisher/subscriber
+  ros::Publisher module_control_pub_;
+  ros::Publisher head_joint_pub_;
+  ros::Publisher head_scan_pub_;
+  ros::Publisher set_walking_command_pub_;
+  ros::Publisher set_walking_param_pub_;
+  ;
+  ros::Publisher motion_index_pub_;
+  ros::ServiceClient get_walking_param_client_;
+
+  ros::Subscriber ball_position_sub_;
+  ros::Subscriber ball_tracking_command_sub_;
+  ros::Subscriber current_joint_states_sub_;
+
+  // (x, y) is the center position of the ball in image coordinates
+  // z is the ball radius
+  geometry_msgs::Point ball_position_;
+  op3_walking_module_msgs::WalkingParam current_walking_param_;
+
+  int count_not_found_;
+  int count_to_kick_;
+  int accum_ball_position_;
+  bool on_tracking_;
+  int approach_ball_position_;
+  double current_pan_, current_tilt_;
+  double current_x_move_, current_r_angle_;
+  int kick_motion_index_;
+  double hip_pitch_offset_;
+  ros::Time prev_time_;
+
+};
+}
+
+#endif /* BALL_FOLLOWER_H_ */

op3_demo/include/op3_demo/ball_tracker.h

@@ -0,0 +1,123 @@
+/*******************************************************************************
+ * 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_TRACKING_H_
+#define BALL_TRACKING_H_
+
+#include <math.h>
+#include <yaml-cpp/yaml.h>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Int32.h>
+#include <sensor_msgs/JointState.h>
+
+#include "robotis_controller_msgs/JointCtrlModule.h"
+#include "ball_detector/circleSetStamped.h"
+#include "op3_walking_module_msgs/WalkingParam.h"
+#include "op3_walking_module_msgs/GetWalkingParam.h"
+
+namespace robotis_op
+{
+
+// head tracking for looking the ball
+class BallTracker
+{
+ public:
+  BallTracker();
+  ~BallTracker();
+
+  bool processTracking();
+
+  void startTracking();
+  void stopTracking();
+
+  void setUsingHeadScan(bool use_scan);
+
+  double getPanOfBall()
+  {
+    // left (+) ~ right (-)
+    return current_ball_pan_;
+  }
+  double getTiltOfBall()
+  {
+    // top (+) ~ bottom (-)
+    return current_ball_tilt_;
+  }
+  double getBallSize()
+  {
+    return current_ball_bottom_;
+  }
+
+ protected:
+  const double FOV_WIDTH;
+  const double FOV_HEIGHT;
+  const int NOT_FOUND_THRESHOLD;
+  const bool DEBUG_PRINT;
+
+  void ballPositionCallback(const ball_detector::circleSetStamped::ConstPtr &msg);
+  void ballTrackerCommandCallback(const std_msgs::String::ConstPtr &msg);
+  void currentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
+  void publishHeadJoint(double pan, double tilt);
+  void scanBall();
+
+  //ros node handle
+  ros::NodeHandle nh_;
+
+  //image publisher/subscriber
+  ros::Publisher module_control_pub_;
+  ros::Publisher head_joint_pub_;
+  ros::Publisher head_scan_pub_;
+
+  ros::Publisher motion_index_pub_;
+
+  ros::Subscriber ball_position_sub_;
+  ros::Subscriber ball_tracking_command_sub_;
+  ros::Subscriber current_joint_states_sub_;
+
+  // (x, y) is the center position of the ball in image coordinates
+  // z is the ball radius
+  geometry_msgs::Point ball_position_;
+
+  bool use_head_scan_;
+  int count_not_found_;
+  bool on_tracking_;
+  double current_head_pan_, current_head_tilt_;
+  double current_ball_pan_, current_ball_tilt_;
+  double current_ball_bottom_;
+  ros::Time prev_time_;
+
+};
+}
+
+#endif /* BALL_TRACKING_H_ */

op3_demo/include/op3_demo/button_test.h

@@ -0,0 +1,101 @@
+/*******************************************************************************
+ * 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 BUTTON_TEST_H_
+#define BUTTON_TEST_H_
+
+#include <boost/thread.hpp>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/String.h>
+//#include <std_msgs/Int32MultiArray.h>
+//#include <geometry_msgs/Point.h>
+
+#include "op3_demo/op_demo.h"
+//#include "op3_demo/face_tracker.h"
+
+#include "robotis_controller_msgs/SyncWriteItem.h"
+
+namespace robotis_op
+{
+
+class ButtonTest : public OPDemo
+{
+ public:
+  ButtonTest();
+  ~ButtonTest();
+
+  void setDemoEnable();
+  void setDemoDisable();
+
+ protected:
+  const int SPIN_RATE;
+
+  void processThread();
+  void callbackThread();
+
+  void process();
+
+//  void playMotion(int motion_index);
+  void setRGBLED(int blue, int green, int red);
+  void setLED(int led);
+
+  void playSound(const std::string &path);
+
+  void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
+//  void facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg);
+
+//  void setModuleToDemo(const std::string &module_name);
+
+//  FaceTracker face_tracker_;
+
+//  ros::Publisher module_control_pub_;
+//  ros::Publisher motion_index_pub_;
+  ros::Publisher rgb_led_pub_;
+  ros::Publisher play_sound_pub_;
+
+  ros::Subscriber buttuon_sub_;
+//  ros::Subscriber faceCoord_sub_;
+
+//  geometry_msgs::Point face_position_;
+
+//  bool is_tracking_;
+//  int tracking_status_;
+  std::string default_mp3_path_;
+  int led_count_;
+  int rgb_led_count_;
+};
+
+} /* namespace robotis_op */
+
+#endif /* BUTTON_TEST_H_ */

op3_demo/include/op3_demo/face_tracker.h

@@ -0,0 +1,116 @@
+/*******************************************************************************
+ * 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 FACE_TRACKING_H_
+#define FACE_TRACKING_H_
+
+#include <math.h>
+#include <yaml-cpp/yaml.h>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Int32.h>
+#include <sensor_msgs/JointState.h>
+#include <geometry_msgs/Point.h>
+
+namespace robotis_op
+{
+
+// head tracking for looking the ball
+class FaceTracker
+{
+ public:
+  enum TrackingStatus
+  {
+    NotFound = -1,
+    Waiting = 0,
+    Found = 1,
+
+  };
+
+  FaceTracker();
+  ~FaceTracker();
+
+  int processTracking();
+
+  void startTracking();
+  void stopTracking();
+
+  void setUsingHeadScan(bool use_scan);
+  void setFacePosition(geometry_msgs::Point &face_position);
+
+  double getPanOfFace()
+  {
+    return current_face_pan_;
+  }
+  double getTiltOfFace()
+  {
+    return current_face_tilt_;
+  }
+
+ protected:
+  const double FOV_WIDTH;
+  const double FOV_HEIGHT;
+  const int NOT_FOUND_THRESHOLD;
+
+  void facePositionCallback(const geometry_msgs::Point::ConstPtr &msg);
+  void faceTrackerCommandCallback(const std_msgs::String::ConstPtr &msg);
+  void publishHeadJoint(double pan, double tilt);
+  void scanFace();
+
+  //ros node handle
+  ros::NodeHandle nh_;
+
+  //image publisher/subscriber
+  ros::Publisher module_control_pub_;
+  ros::Publisher head_joint_pub_;
+  ros::Publisher head_scan_pub_;
+
+  ros::Subscriber face_position_sub_;
+  ros::Subscriber face_tracking_command_sub_;
+
+  // (x, y) is the center position of the ball in image coordinates
+  // z is the face size
+  geometry_msgs::Point face_position_;
+
+  bool use_head_scan_;
+  int count_not_found_;
+  bool on_tracking_;
+  double current_face_pan_, current_face_tilt_;
+
+  int dismissed_count_;
+
+};
+}
+
+#endif /* FACE_TRACKING_H_ */

op3_demo/include/op3_demo/mic_test.h

@@ -0,0 +1,105 @@
+/*******************************************************************************
+ * 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 MIC_TEST_H_
+#define MIC_TEST_H_
+
+#include <signal.h>
+#include <boost/thread.hpp>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/String.h>
+
+#include "op3_demo/op_demo.h"
+
+#include "robotis_controller_msgs/SyncWriteItem.h"
+
+namespace robotis_op
+{
+
+class MicTest : public OPDemo
+{
+ public:
+  enum Mic_Test_Status
+  {
+    Ready = 0,
+    AnnounceRecording = 1,
+    MicRecording = 2,
+    PlayingSound = 3,
+    DeleteTempFile = 4,
+    DemoCount = 5
+  };
+
+  MicTest();
+  ~MicTest();
+
+  void setDemoEnable();
+  void setDemoDisable();
+
+ protected:
+  const int SPIN_RATE;
+
+  void processThread();
+  void callbackThread();
+
+  void process();
+
+  void announceTest();
+  void recordSound(int recording_time);
+  void recordSound();
+  void playTestSound(const std::string &path);
+  void playSound(const std::string &file_path);
+  void deleteSoundFile(const std::string &file_path);
+
+  void startTimer(double wait_time);
+  void finishTimer();
+
+  void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
+
+  std::string recording_file_name_;
+  std::string default_mp3_path_;
+
+  ros::Publisher play_sound_pub_;
+  ros::Subscriber buttuon_sub_;
+
+  ros::Time start_time_;
+  double wait_time_;
+  bool is_wait_;
+  int record_pid_;
+  int play_pid_;
+  int test_status_;
+};
+
+} /* namespace robotis_op */
+
+#endif /* MIC_TEST_H_ */

op3_demo/include/op3_demo/op_demo.h

@@ -0,0 +1,77 @@
+/*******************************************************************************
+ * 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 OP_DEMO_H_
+#define OP_DEMO_H_
+
+namespace robotis_op
+{
+
+class OPDemo
+{
+ public:
+  enum Motion_Index
+  {
+    InitPose = 1,
+    WalkingReady = 9,
+    //GetUpFront = 81,
+    //GetUpBack = 82,
+    //RightKick = 83,
+    //LeftKick = 84,
+    GetUpFront = 122,
+    GetUpBack = 123,
+    RightKick = 121,
+    LeftKick = 120,
+    Ceremony = 85,
+  };
+
+  OPDemo()
+  {
+  }
+  virtual ~OPDemo()
+  {
+  }
+
+  virtual void setDemoEnable()
+  {
+  }
+  virtual void setDemoDisable()
+  {
+  }
+
+ protected:
+  bool enable_;
+};
+
+} /* namespace robotis_op */
+
+#endif /* OP_DEMO_H_ */

op3_demo/include/op3_demo/soccer_demo.h

@@ -0,0 +1,126 @@
+/*******************************************************************************
+ * 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 SOCCER_DEMO_H
+#define SOCCER_DEMO_H
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <sensor_msgs/Imu.h>
+#include <boost/thread.hpp>
+
+#include "op3_demo/op_demo.h"
+#include "op3_demo/ball_tracker.h"
+#include "op3_demo/ball_follower.h"
+#include "robotis_math/robotis_linear_algebra.h"
+
+namespace robotis_op
+{
+
+class SoccerDemo : public OPDemo
+{
+ public:
+  enum Stand_Status
+  {
+    Stand = 0,
+    Fallen_Forward = 1,
+    Fallen_Behind = 2,
+  };
+
+  enum Robot_Status
+  {
+    Waited = 0,
+    TrackingAndFollowing = 1,
+    ReadyToKick = 2,
+    ReadyToCeremony = 3,
+    ReadyToGetup = 4,
+  };
+
+  SoccerDemo();
+  ~SoccerDemo();
+
+  void setDemoEnable();
+  void setDemoDisable();
+
+ protected:
+  const double FALLEN_FORWARD_LIMIT;
+  const double FALLEN_BEHIND_LIMIT;
+  const int SPIN_RATE;
+
+  void processThread();
+  void callbackThread();
+
+  void setBodyModuleToDemo(const std::string &body_module, bool with_head_control = true);
+  void setModuleToDemo(const std::string &module_name);
+  void parseJointNameFromYaml(const std::string &path);
+  bool getJointNameFromID(const int &id, std::string &joint_name);
+  bool getIDFromJointName(const std::string &joint_name, int &id);
+  int getJointCount();
+  bool isHeadJoint(const int &id);
+  void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
+  void demoCommandCallback(const std_msgs::String::ConstPtr& msg);
+  void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg);
+
+  void startSoccerMode();
+  void stopSoccerMode();
+
+  void process();
+  void handleKick(int ball_position);
+  bool handleFallen(int fallen_status);
+
+  void playMotion(int motion_index);
+
+  BallTracker ball_tracker_;
+  BallFollower ball_follower_;
+
+  ros::Publisher module_control_pub_;
+  ros::Publisher motion_index_pub_;
+  ros::Subscriber buttuon_sub_;
+  ros::Subscriber demo_command_sub_;
+  ros::Subscriber imu_data_sub_;
+  std::map<int, std::string> id_joint_table_;
+  std::map<std::string, int> joint_id_table_;
+
+  bool debug_code_;
+  int wait_count_;
+  bool on_following_ball_;
+  bool restart_soccer_;
+  bool start_following_;
+  bool stop_following_;
+  bool stop_fallen_check_;
+  int robot_status_;
+  int stand_state_;
+  double present_pitch_;
+};
+
+}  // namespace robotis_op
+#endif // SOCCER_DEMO_H

op3_demo/include/op3_demo/vision_demo.h

@@ -0,0 +1,93 @@
+/*******************************************************************************
+ * 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 VISION_DEMO_H_
+#define VISION_DEMO_H_
+
+#include <boost/thread.hpp>
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Int32MultiArray.h>
+#include <geometry_msgs/Point.h>
+
+#include "op3_demo/op_demo.h"
+#include "op3_demo/face_tracker.h"
+
+#include "robotis_controller_msgs/SyncWriteItem.h"
+
+namespace robotis_op
+{
+
+class VisionDemo : public OPDemo
+{
+ public:
+  VisionDemo();
+  ~VisionDemo();
+
+  void setDemoEnable();
+  void setDemoDisable();
+
+ protected:
+  const int SPIN_RATE;
+
+  void processThread();
+  void callbackThread();
+
+  void process();
+
+  void playMotion(int motion_index);
+  void setRGBLED(int blue, int green, int red);
+
+  void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
+  void facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg);
+
+  void setModuleToDemo(const std::string &module_name);
+
+  FaceTracker face_tracker_;
+
+  ros::Publisher module_control_pub_;
+  ros::Publisher motion_index_pub_;
+  ros::Publisher rgb_led_pub_;
+
+  ros::Subscriber buttuon_sub_;
+  ros::Subscriber faceCoord_sub_;
+
+  geometry_msgs::Point face_position_;
+
+  bool is_tracking_;
+  int tracking_status_;
+};
+
+} /* namespace robotis_op */
+
+#endif /* VISION_DEMO_H_ */

op3_demo/launch/ball_tracker.launch

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<!-- Launches an UVC camera, the ball detector and its visualization  -->
+<launch>    
+    <!-- Camera and Ball detector -->
+    <include file="$(find ball_detector)/launch/ball_detector_from_usb_cam.launch"/> 
+
+    <!-- <node pkg="ball_tracker" type="ball_tracker_node" name="ball_tracker" output="screen"/> -->
+    <node pkg="op3_demo" type="ball_tracker_node" name="ball_tracker" output="screen"/>
+</launch>
+

op3_demo/launch/demo.launch

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<!-- Launches an UVC camera, the ball detector and its visualization  -->
+<launch>    
+
+    <!-- op3 manager -->
+    <include file="$(find op3_manager)/launch/op3_manager.launch"/> 
+
+    <!-- Camera and Ball detector -->
+    <include file="$(find ball_detector)/launch/ball_detector_from_usb_cam.launch"/> 
+   
+    <!-- face tracking -->
+    <include file="$(find op3_demo)/launch/face_detection_op3.launch" />
+
+    <!-- camera setting tool -->
+    <include file="$(find op3_camera_setting_tool)/launch/op3_camera_setting_tool.launch" />
+    
+    <!-- sound player -->
+    <node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen"/>
+    
+    <!-- op3 demo -->
+    <node pkg="op3_demo" type="op_demo_node" name="op3_demo" output="screen"/>
+</launch>
+

op3_demo/launch/face_detection_op3.launch

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="face_cascade_name_0"  default="$(find face_detection)/include/face_detection/HaarCascades/haarcascade_frontalface_alt.xml" />
+  <arg name="face_cascade_name_1"  default="$(find face_detection)/include/face_detection//HaarCascades/haarcascade_frontalface_alt2.xml" />
+  <arg name="face_cascade_name_2"  default="$(find face_detection)/include/face_detection//HaarCascades/haarcascade_frontalface_alt_tree.xml" />
+  <arg name="face_cascade_name_3"  default="$(find face_detection)/include/face_detection//HaarCascades/haarcascade_frontalface_default.xml" />
+  <arg name="face_cascade_name_4"  default="$(find face_detection)/include/face_detection/lbpCascades/lbpcascade_frontalface.xml" />
+
+  <node pkg="face_detection" type="face_tracking" name="face_tracking"
+      args="$(arg face_cascade_name_0)
+      $(arg face_cascade_name_1)
+      $(arg face_cascade_name_2)
+      $(arg face_cascade_name_3)
+      $(arg face_cascade_name_4)"
+      output="screen">      
+        <param name="imageInput" type="string" value="/usb_cam_node/image_raw" />
+        <param name="displayed_Image" type="int" value="0" />
+        <!-- <param name="publish" type="int" value="2" /> -->
+        <param name="publish" type="int" value="3" />
+  </node>
+</launch>

op3_demo/launch/self_test.launch

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<!-- Launches an UVC camera, the ball detector and its visualization  -->
+<launch>    
+
+    <!-- op3 manager -->
+    <include file="$(find op3_manager)/launch/op3_manager.launch"/> 
+
+    <!-- Camera and Ball detector -->
+    <include file="$(find ball_detector)/launch/ball_detector_from_usb_cam.launch"/> 
+   
+    <!-- face tracking -->
+    <include file="$(find op3_demo)/launch/face_detection_op3.launch" />
+
+    <!-- camera setting tool -->
+    <include file="$(find op3_camera_setting_tool)/launch/op3_camera_setting_tool.launch" />
+    
+    <!-- sound player -->
+    <node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen"/>
+    
+    <!-- op3 demo -->
+    <node pkg="op3_demo" type="self_test_node" name="op3_self_test" output="screen"/>
+</launch>
+

op3_demo/package.xml

@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<package>
+  <name>op3_demo</name>
+  <version>0.1.0</version>
+  <description>
+    op3 default demo
+    It includes three demontrations(soccer demo, vision demo, action script demo)
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="kmjung@robotis.com">kayman</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ball_tracking</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <author email="kmjung@robotis.com">kayman</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  
+  <build_depend>roscpp</build_depend>
+  <build_depend>roslib</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>ball_detector</build_depend>
+  <build_depend>op3_walking_module_msgs</build_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>robotis_math</build_depend>
+  
+  <run_depend>roscpp</run_depend>
+  <run_depend>roslib</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>ball_detector</run_depend>
+  <run_depend>op3_walking_module_msgs</run_depend>
+  <run_depend>cmake_modules</run_depend>
+  <run_depend>robotis_math</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

op3_demo/script/action_script.yaml

@@ -0,0 +1,23 @@
+# combination action page number and mp3 file path
+action_and_sound: 
+  4 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Thank you.mp3"
+  41: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Introduction.mp3"
+  24: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Wow.mp3"
+  23: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Yes go.mp3"
+  15: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Sit down.mp3"
+  1: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Stand up.mp3"
+  54: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Clap please.mp3"
+  27: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3"
+  38: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Bye bye.mp3"
+#  101 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3"
+  110 : ""
+  111 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro01.mp3"
+  115 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro02.mp3"
+  118 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Intro03.mp3"
+
+# play list
+prev_default: [4, 41, 24, 23, 15, 1, 54, 27, 38]
+default: [4, 110, 111, 115, 118, 24, 54, 27, 38]
+
+# example of play list
+#certification: [101]

op3_demo/script/action_script_bk.yaml

@@ -0,0 +1,18 @@
+# combination action page number and mp3 file path
+action_and_sound: 
+  4 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Thank you.mp3"
+  41: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Introduction.mp3"
+  24: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Wow.mp3"
+  23: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Yes go.mp3"
+  15: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Sit down.mp3"
+  1: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Stand up.mp3"
+  54: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Clap please.mp3"
+  27: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3"
+  38: "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Bye bye.mp3"
+  101 : "/home/robotis/catkin_ws/src/ROBOTIS-OP3/ROBOTIS-OP3-Demo/op3_demo/Data/mp3/Oops.mp3"
+
+# play list
+default: [4, 41, 24, 23, 15, 1, 54, 27, 38]
+
+# example of play list
+certificatino: [101]

op3_demo/src/action/action_demo.cpp

@@ -0,0 +1,430 @@
+/*******************************************************************************
+ * 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 "op3_demo/action_demo.h"
+
+namespace robotis_op
+{
+
+ActionDemo::ActionDemo()
+    : SPIN_RATE(30),
+      DEMO_INIT_POSE(8),
+      play_index_(0),
+      start_play_(false),
+      stop_play_(false),
+      pause_play_(false),
+      play_status_(StopAction)
+{
+  enable_ = false;
+
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  std::string default_path = ros::package::getPath("op3_demo") + "/script/action_script.yaml";
+  script_path_ = nh.param<std::string>("action_script", default_path);
+
+  std::string default_play_list = "default";
+  play_list_name_ = nh.param<std::string>("action_script_play_list", default_play_list);
+
+  parseActionScript (script_path_);
+
+  boost::thread queue_thread = boost::thread(boost::bind(&ActionDemo::callbackThread, this));
+  boost::thread process_thread = boost::thread(boost::bind(&ActionDemo::processThread, this));
+}
+
+ActionDemo::~ActionDemo()
+{
+  // TODO Auto-generated destructor stub
+}
+
+void ActionDemo::setDemoEnable()
+{
+  setModuleToDemo("action_module");
+
+  usleep(10 * 1000);
+
+  enable_ = true;
+
+  ROS_INFO("Start ActionScript Demo");
+
+  playAction(DEMO_INIT_POSE);
+
+  startProcess(play_list_name_);
+}
+
+void ActionDemo::setDemoDisable()
+{
+  stopProcess();
+
+  enable_ = false;
+
+  play_list_.resize(0);
+}
+
+void ActionDemo::process()
+{
+  // check current status
+  //handleStatus();
+
+  switch (play_status_)
+  {
+    case PlayAction:
+    {
+      if (play_list_.size() == 0)
+      {
+        ROS_WARN("Play List is empty.");
+        return;
+      }
+
+      // action is not running
+      if (isActionRunning() == false)
+      {
+        // play
+        bool result_play = playActionWithSound(play_list_.at(play_index_));
+
+        ROS_INFO_COND(!result_play, "Fail to play action script.");
+
+        // add play index
+        int index_to_play = (play_index_ + 1) % play_list_.size();
+        play_index_ = index_to_play;
+      }
+      else
+      {
+        // wait
+        return;
+      }
+      break;
+    }
+
+    case PauseAction:
+    {
+      stopMP3();
+      stopAction();
+
+      break;
+    }
+
+    case StopAction:
+    {
+      stopMP3();
+      stopAction();
+
+      break;
+    }
+
+    default:
+      break;
+  }
+}
+
+void ActionDemo::handleStatus()
+{
+  if (start_play_ == true)
+  {
+    play_status_ = PlayAction;
+    start_play_ = false;
+  }
+
+  if (pause_play_ == true)
+  {
+    play_status_ = PauseAction;
+    pause_play_ = false;
+  }
+
+  if (stop_play_ == true)
+  {
+    play_status_ = StopAction;
+    stop_play_ = false;
+  }
+}
+
+void ActionDemo::startProcess(const std::string &set_name)
+{
+  parseActionScriptSetName(script_path_, set_name);
+
+  start_play_ = true;
+  play_status_ = PlayAction;
+}
+
+void ActionDemo::resumeProcess()
+{
+  start_play_ = true;
+  play_status_ = PlayAction;
+}
+
+void ActionDemo::pauseProcess()
+{
+  pause_play_ = true;
+  play_status_ = PauseAction;
+}
+
+void ActionDemo::stopProcess()
+{
+  stop_play_ = true;
+  play_index_ = 0;
+  play_status_ = StopAction;
+}
+
+void ActionDemo::processThread()
+{
+  //set node loop rate
+  ros::Rate loop_rate(SPIN_RATE);
+
+  //node loop
+  while (ros::ok())
+  {
+    if (enable_ == true)
+      process();
+
+    //relax to fit output rate
+    loop_rate.sleep();
+  }
+}
+
+void ActionDemo::callbackThread()
+{
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  // subscriber & publisher
+  module_control_pub_ = nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 0);
+  motion_index_pub_ = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
+  play_sound_pub_ = nh.advertise<std_msgs::String>("/play_sound_file", 0);
+
+  buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &ActionDemo::buttonHandlerCallback, this);
+
+  is_running_client_ = nh.serviceClient<op3_action_module_msgs::IsRunning>("/robotis/action/is_running");
+
+  while (nh.ok())
+  {
+    ros::spinOnce();
+
+    usleep(1000);
+  }
+}
+
+void ActionDemo::parseActionScript(const std::string &path)
+{
+  YAML::Node doc;
+
+  try
+  {
+    // load yaml
+    doc = YAML::LoadFile(path.c_str());
+  } catch (const std::exception& e)
+  {
+    ROS_ERROR_STREAM("Fail to load action script yaml. - " << e.what());
+    ROS_ERROR_STREAM("Script Path : " << path);
+    return;
+  }
+
+  // parse action_sound table
+  YAML::Node sub_node = doc["action_and_sound"];
+  for (YAML::iterator yaml_it = sub_node.begin(); yaml_it != sub_node.end(); ++yaml_it)
+  {
+    int action_index = yaml_it->first.as<int>();
+    std::string mp3_path = yaml_it->second.as<std::string>();
+
+    action_sound_table_[action_index] = mp3_path;
+  }
+
+  // default action set
+  if (doc["default"])
+    play_list_ = doc["default"].as<std::vector<int> >();
+}
+
+bool ActionDemo::parseActionScriptSetName(const std::string &path, const std::string &set_name)
+{
+
+  YAML::Node doc;
+
+  try
+  {
+    // load yaml
+    doc = YAML::LoadFile(path.c_str());
+  } catch (const std::exception& e)
+  {
+    ROS_ERROR("Fail to load yaml.");
+    return false;
+  }
+
+  // parse action_sound table
+  if (doc[set_name])
+  {
+    play_list_ = doc[set_name].as<std::vector<int> >();
+    return true;
+  }
+  else
+    return false;
+}
+
+bool ActionDemo::playActionWithSound(int motion_index)
+{
+  std::map<int, std::string>::iterator map_it = action_sound_table_.find(motion_index);
+  if (map_it == action_sound_table_.end())
+    return false;
+
+  playAction(motion_index);
+  playMP3(map_it->second);
+
+  ROS_INFO_STREAM("action : " << motion_index << ", mp3 path : " << map_it->second);
+
+  return true;
+}
+
+void ActionDemo::playMP3(std::string &path)
+{
+  std_msgs::String sound_msg;
+  sound_msg.data = path;
+
+  play_sound_pub_.publish(sound_msg);
+}
+
+void ActionDemo::stopMP3()
+{
+  std_msgs::String sound_msg;
+  sound_msg.data = "";
+
+  play_sound_pub_.publish(sound_msg);
+}
+
+void ActionDemo::playAction(int motion_index)
+{
+  std_msgs::Int32 motion_msg;
+  motion_msg.data = motion_index;
+
+  motion_index_pub_.publish(motion_msg);
+}
+
+void ActionDemo::stopAction()
+{
+  std_msgs::Int32 motion_msg;
+  motion_msg.data = StopActionCommand;
+
+  motion_index_pub_.publish(motion_msg);
+}
+
+void ActionDemo::brakeAction()
+{
+  std_msgs::Int32 motion_msg;
+  motion_msg.data = BrakeActionCommand;
+
+  motion_index_pub_.publish(motion_msg);
+}
+
+// check running of action
+bool ActionDemo::isActionRunning()
+{
+  op3_action_module_msgs::IsRunning is_running_srv;
+
+  if (is_running_client_.call(is_running_srv) == false)
+  {
+    ROS_ERROR("Failed to get action status");
+    return true;
+  }
+  else
+  {
+    if (is_running_srv.response.is_running == true)
+    {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+void ActionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
+{
+  if (enable_ == false)
+    return;
+
+  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")
+  {
+
+  }
+}
+
+void ActionDemo::setModuleToDemo(const std::string &module_name)
+{
+//  robotis_controller_msgs::JointCtrlModule control_msg;
+//
+//  // todo : remove hard coding
+//  for (int ix = 1; ix <= 20; ix++)
+//  {
+//    std::string joint_name;
+//
+//    if (getJointNameFromID(ix, joint_name) == false)
+//      continue;
+//
+//    control_msg.joint_name.push_back(joint_name);
+//    control_msg.module_name.push_back(module_name);
+//  }
+//
+//  // no control
+//  if (control_msg.joint_name.size() == 0)
+//    return;
+//
+//  module_control_pub_.publish(control_msg);
+//  std::cout << "enable module : " << module_name << std::endl;
+
+  std_msgs::String control_msg;
+  control_msg.data = "action_module";
+
+  module_control_pub_.publish(control_msg);
+  std::cout << "enable module : " << module_name << std::endl;
+}
+
+} /* namespace robotis_op */

op3_demo/src/ball_tracker_node.cpp

@@ -0,0 +1,133 @@
+/*******************************************************************************
+ * 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 "op3_demo/ball_tracker.h"
+
+enum Demo_Status
+{
+  Ready = 0,
+  DesireToTrack = 1,
+  DesireToStop = 2,
+  Tracking = 3,
+  DemoCount = 4,
+};
+int current_status = Ready;
+
+void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
+
+
+//node main
+int main(int argc, char **argv)
+{
+  //init ros
+  ros::init(argc, argv, "ball_tracker_node");
+  ros::NodeHandle nh("~");
+  ros::Publisher module_control_pub_ = nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 0);
+  ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback);
+
+  //create ros wrapper object
+  robotis_op::BallTracker tracker;
+
+  // set head_control_module
+  std_msgs::String control_msg;
+  control_msg.data = "head_control_module";
+
+  usleep(1000 * 1000);
+
+  module_control_pub_.publish(control_msg);
+
+  // start ball tracking
+  tracker.startTracking();
+  current_status = Tracking;
+
+  //set node loop rate
+  ros::Rate loop_rate(30);
+
+  //node loop
+  while (ros::ok())
+  {
+    switch (current_status)
+    {
+      case DesireToTrack:
+        tracker.startTracking();
+        current_status = Tracking;
+        break;
+
+      case DesireToStop:
+        tracker.stopTracking();
+        current_status = Ready;
+        break;
+
+      case Tracking:
+        tracker.processTracking();
+        break;
+
+      default:
+        break;
+    }
+
+    //execute pending callback
+    ros::spinOnce();
+
+    //relax to fit output rate
+    loop_rate.sleep();
+  }
+
+  //exit program
+  return 0;
+}
+
+void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
+{
+  if (msg->data == "mode_long")
+  {
+
+  }
+  else if (msg->data == "start_long")
+  {
+    // it's using in op3_manager
+    // torque on and going to init pose
+  }
+
+  if (msg->data == "start")
+  {
+    if (current_status == Ready)
+      current_status = DesireToTrack;
+    else if (current_status == Tracking)
+      current_status = DesireToStop;
+  }
+  else if (msg->data == "mode")
+  {
+
+  }
+
+}

op3_demo/src/demo_node.cpp

@@ -0,0 +1,334 @@
+/*******************************************************************************
+ * 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 <ros/ros.h>
+#include <std_msgs/String.h>
+
+#include "op3_demo/soccer_demo.h"
+#include "op3_demo/action_demo.h"
+#include "op3_demo/vision_demo.h"
+#include "robotis_math/robotis_linear_algebra.h"
+#include "robotis_controller_msgs/SyncWriteItem.h"
+
+enum Demo_Status
+{
+  Ready = 0,
+  SoccerDemo = 1,
+  VisionDemo = 2,
+  ActionDemo = 3,
+  DemoCount = 4,
+};
+
+void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
+void goInitPose();
+void playSound(const std::string &path);
+void setLED(int led);
+bool checkManagerRunning(std::string& manager_name);
+
+const int SPIN_RATE = 30;
+
+ros::Publisher init_pose_pub;
+ros::Publisher play_sound_pub;
+ros::Publisher led_pub;
+
+std::string default_mp3_path = "";
+int current_status = Ready;
+int desired_status = Ready;
+bool apply_desired = false;
+
+//node main
+int main(int argc, char **argv)
+{
+  //init ros
+  ros::init(argc, argv, "demo_node");
+
+  //create ros wrapper object
+  robotis_op::OPDemo *current_demo = NULL;
+  robotis_op::SoccerDemo *soccer_demo = new robotis_op::SoccerDemo();
+  robotis_op::ActionDemo *action_demo = new robotis_op::ActionDemo();
+  robotis_op::VisionDemo *vision_demo = new robotis_op::VisionDemo();
+
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  init_pose_pub = nh.advertise<std_msgs::String>("/robotis/base/ini_pose", 0);
+  play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
+  led_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
+  ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback);
+
+  default_mp3_path = ros::package::getPath("op3_demo") + "/Data/mp3/";
+
+  ros::start();
+
+  //set node loop rate
+  ros::Rate loop_rate(SPIN_RATE);
+
+  // wait for starting of manager
+  std::string manager_name = "/op3_manager";
+  while (ros::ok())
+  {
+    ros::Duration(1.0).sleep();
+
+    if (checkManagerRunning(manager_name) == true)
+    {
+      break;
+      ROS_INFO("Succeed to connect");
+    }
+    ROS_WARN("Waiting for op3 manager");
+  }
+
+  // init procedure
+  playSound(default_mp3_path + "Demonstration ready mode.mp3");
+  setLED(0x01 | 0x02 | 0x04);
+
+  //node loop
+  while (ros::ok())
+  {
+    // process
+    if (apply_desired == true)
+    {
+      switch (desired_status)
+      {
+        case Ready:
+        {
+
+          if (current_demo != NULL)
+            current_demo->setDemoDisable();
+
+          current_demo = NULL;
+
+          goInitPose();
+
+          ROS_INFO("[Go to Demo READY!]");
+          break;
+        }
+
+        case SoccerDemo:
+        {
+          if (current_demo != NULL)
+            current_demo->setDemoDisable();
+
+          current_demo = soccer_demo;
+          current_demo->setDemoEnable();
+
+          ROS_INFO("[Start] Soccer Demo");
+          break;
+        }
+
+        case VisionDemo:
+        {
+          if (current_demo != NULL)
+            current_demo->setDemoDisable();
+
+          current_demo = vision_demo;
+          current_demo->setDemoEnable();
+          ROS_INFO("[Start] Vision Demo");
+          break;
+        }
+        case ActionDemo:
+        {
+          if (current_demo != NULL)
+            current_demo->setDemoDisable();
+
+          current_demo = action_demo;
+          current_demo->setDemoEnable();
+          ROS_INFO("[Start] Action Demo");
+          break;
+        }
+
+        default:
+        {
+          break;
+        }
+      }
+
+      apply_desired = false;
+      current_status = desired_status;
+    }
+    else
+    {
+      if (current_status != desired_status)
+      {
+//        // sound out desired status
+//        switch (desired_status)
+//        {
+//          case SoccerDemo:
+//            playSound(default_path + "Autonomous soccer mode.mp3");
+//            break;
+//
+//          case VisionDemo:
+//            playSound(default_path + "Vision processing mode.mp3");
+//            break;
+//
+//          case ActionDemo:
+//            playSound(default_path + "Interactive motion mode.mp3");
+//            break;
+//
+//          default:
+//            break;
+//        }
+      }
+    }
+
+    //execute pending callbacks
+    ros::spinOnce();
+
+    //relax to fit output rate
+    loop_rate.sleep();
+  }
+
+  //exit program
+  return 0;
+}
+
+void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
+{
+  // in the middle of playing demo
+  if (current_status != Ready)
+  {
+    if (msg->data == "mode_long")
+    {
+      // go to mode selection status
+      desired_status = Ready;
+      apply_desired = true;
+
+      playSound(default_mp3_path + "Demonstration ready mode.mp3");
+      setLED(0x01 | 0x02 | 0x04);
+    }
+    else if (msg->data == "user_long")
+    {
+      // it's using in op3_manager
+      // torque on and going to init pose
+    }
+  }
+  // ready to start demo
+  else
+  {
+    if (msg->data == "start")
+    {
+      // select current demo
+      desired_status = (desired_status == Ready) ? desired_status + 1 : desired_status;
+      apply_desired = true;
+
+      // sound out desired status
+      switch (desired_status)
+      {
+        case SoccerDemo:
+          playSound(default_mp3_path + "Start soccer demonstration.mp3");
+          break;
+
+        case VisionDemo:
+          playSound(default_mp3_path + "Start vision processing demonstration.mp3");
+          break;
+
+        case ActionDemo:
+          playSound(default_mp3_path + "Start motion demonstration.mp3");
+          break;
+
+        default:
+          break;
+      }
+
+      ROS_INFO("= Start Demo Mode : %d", desired_status);
+    }
+    else if (msg->data == "mode")
+    {
+      // change to next demo
+      desired_status = (desired_status + 1) % DemoCount;
+      desired_status = (desired_status == Ready) ? desired_status + 1 : desired_status;
+
+      // sound out desired status and changign LED
+      switch (desired_status)
+      {
+        case SoccerDemo:
+          playSound(default_mp3_path + "Autonomous soccer mode.mp3");
+          setLED(0x01);
+          break;
+
+        case VisionDemo:
+          playSound(default_mp3_path + "Vision processing mode.mp3");
+          setLED(0x02);
+          break;
+
+        case ActionDemo:
+          playSound(default_mp3_path + "Interactive motion mode.mp3");
+          setLED(0x04);
+          break;
+
+        default:
+          break;
+      }
+
+      ROS_INFO("= Demo Mode : %d", desired_status);
+    }
+  }
+}
+
+void goInitPose()
+{
+  std_msgs::String init_msg;
+  init_msg.data = "ini_pose";
+
+  init_pose_pub.publish(init_msg);
+}
+
+void playSound(const std::string &path)
+{
+  std_msgs::String sound_msg;
+  sound_msg.data = path;
+
+  play_sound_pub.publish(sound_msg);
+}
+
+void setLED(int led)
+{
+  robotis_controller_msgs::SyncWriteItem syncwrite_msg;
+  syncwrite_msg.item_name = "LED";
+  syncwrite_msg.joint_name.push_back("open-cr");
+  syncwrite_msg.value.push_back(led);
+
+  led_pub.publish(syncwrite_msg);
+}
+
+bool checkManagerRunning(std::string& manager_name)
+{
+  std::vector<std::string> node_list;
+  ros::master::getNodes(node_list);
+
+  for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); node_list_idx++)
+  {
+    if (node_list[node_list_idx] == manager_name)
+      return true;
+  }
+
+  ROS_ERROR("Can't find op3_manager");
+  return false;
+}

op3_demo/src/soccer/ball_follower.cpp

@@ -0,0 +1,331 @@
+/*******************************************************************************
+ * 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 "op3_demo/ball_follower.h"
+
+namespace robotis_op
+{
+
+BallFollower::BallFollower()
+    : nh_(ros::this_node::getName()),
+      FOV_WIDTH(26.4 * M_PI / 180),
+      FOV_HEIGHT(21.6 * M_PI / 180),
+      count_not_found_(0),
+      count_to_kick_(0),
+      on_tracking_(false),
+      approach_ball_position_(NotFound),
+      kick_motion_index_(83),
+      CAMERA_HEIGHT(0.46),
+      NOT_FOUND_THRESHOLD(50),
+      MAX_FB_STEP(35.0 * 0.001),
+      MAX_RL_TURN(15.0 * M_PI / 180),
+      MIN_FB_STEP(5.0 * 0.001),
+      MIN_RL_TURN(5.0 * M_PI / 180),
+      UNIT_FB_STEP(0.5 * 0.001),
+      UNIT_RL_TURN(0.5 * M_PI / 180),
+      SPOT_FB_OFFSET(0.0 * 0.001),
+      SPOT_RL_OFFSET(0.0 * 0.001),
+      SPOT_ANGLE_OFFSET(0.0 * M_PI / 180),
+      hip_pitch_offset_(7.0),
+      current_pan_(-10),
+      current_tilt_(-10),
+      current_x_move_(0.005),
+      current_r_angle_(0),
+      debug_print_(false)
+{
+  current_joint_states_sub_ = nh_.subscribe("/robotis/goal_joint_states", 10, &BallFollower::currentJointStatesCallback,
+                                            this);
+
+  set_walking_command_pub_ = nh_.advertise<std_msgs::String>("/robotis/walking/command", 0);
+  set_walking_param_pub_ = nh_.advertise<op3_walking_module_msgs::WalkingParam>("/robotis/walking/set_params", 0);
+  get_walking_param_client_ = nh_.serviceClient<op3_walking_module_msgs::GetWalkingParam>(
+      "/robotis/walking/get_params");
+
+  prev_time_ = ros::Time::now();
+
+}
+
+BallFollower::~BallFollower()
+{
+
+}
+
+void BallFollower::startFollowing()
+{
+  on_tracking_ = true;
+  ROS_INFO("Start Ball following");
+
+  setWalkingCommand("start");
+
+  bool result = getWalkingParam();
+  if (result == true)
+  {
+    hip_pitch_offset_ = current_walking_param_.hip_pitch_offset;
+  }
+  else
+  {
+    hip_pitch_offset_ = 7.0 * M_PI / 180;
+  }
+}
+
+void BallFollower::stopFollowing()
+{
+  on_tracking_ = false;
+  approach_ball_position_ = NotFound;
+  count_to_kick_ = 0;
+  accum_ball_position_ = 0;
+  ROS_INFO("Stop Ball following");
+
+  setWalkingCommand("stop");
+}
+
+void BallFollower::currentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
+{
+  double pan, tilt;
+  int get_count = 0;
+
+  for (int ix = 0; ix < msg->name.size(); ix++)
+  {
+    if (msg->name[ix] == "head_pan")
+    {
+      pan = msg->position[ix];
+      get_count += 1;
+    }
+    else if (msg->name[ix] == "head_tilt")
+    {
+      tilt = msg->position[ix];
+      get_count += 1;
+    }
+
+    if (get_count == 2)
+      break;
+  }
+
+  // check variation
+  current_pan_ = pan;
+  current_tilt_ = tilt;
+}
+
+// x_angle : ball position (pan), y_angle : ball position (tilt)
+bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_size)
+{
+  ros::Time curr_time = ros::Time::now();
+  ros::Duration dur = curr_time - prev_time_;
+  double delta_time = dur.nsec * 0.000000001 + dur.sec;
+  prev_time_ = curr_time;
+
+  count_not_found_ = 0;
+  int ball_position_sum = 0;
+
+  // check of getting head joints angle
+  if (current_tilt_ == -10 && current_pan_ == -10)
+  {
+    ROS_ERROR("Failed to get current angle of head joints.");
+    setWalkingCommand("stop");
+
+    on_tracking_ = false;
+    approach_ball_position_ = NotFound;
+    return false;
+  }
+
+  ROS_INFO_COND(debug_print_, "   ============== Head | Ball ==============   ");
+  ROS_INFO_STREAM_COND(debug_print_,
+                       "== Head Pan : " << (current_pan_ * 180 / M_PI) << " | Ball X : " << (x_angle * 180 / M_PI));
+  ROS_INFO_STREAM_COND(debug_print_,
+                       "== Head Tilt : " << (current_tilt_ * 180 / M_PI) << " | Ball Y : " << (y_angle * 180 / M_PI));
+
+  approach_ball_position_ = NotFound;
+
+  // clac fb
+  double distance_to_ball = CAMERA_HEIGHT * tan(M_PI * 0.5 + current_tilt_ - hip_pitch_offset_ - ball_size);
+
+  double ball_y_angle = (current_tilt_ + y_angle) * 180 / M_PI;
+  double ball_x_angle = (current_pan_ + x_angle) * 180 / M_PI;
+
+  if (distance_to_ball < 0)
+    distance_to_ball *= (-1);
+
+  double fb_goal, fb_move;
+  double distance_to_kick = 0.25;
+
+  // check whether ball is correct position.
+  if ((distance_to_ball < distance_to_kick) && (fabs(ball_x_angle) < 25.0))
+  //if ((ball_y_angle < -65) && (fabs(current_pan_) < 25))
+  {
+    count_to_kick_ += 1;
+
+    ROS_INFO_STREAM_COND(debug_print_,
+                         "head pan : " << (current_pan_ * 180 / M_PI) << " | ball pan : " << (x_angle * 180 / M_PI));
+    ROS_INFO_STREAM_COND(debug_print_,
+                         "head tilt : " << (current_tilt_ * 180 / M_PI) << " | ball tilt : " << (y_angle * 180 / M_PI));
+    ROS_INFO_STREAM_COND(debug_print_, "foot to kick : " << accum_ball_position_);
+
+    ROS_INFO("In range [%d]", count_to_kick_);
+
+    //if (fabs(x_angle) < 10 * M_PI / 180)
+    //{
+    if (count_to_kick_ > 20)
+    {
+      setWalkingCommand("stop");
+      on_tracking_ = false;
+
+      // check direction of the ball
+      if (accum_ball_position_ > 0)
+      {
+        ROS_INFO("Ready to kick : left");  // left
+        approach_ball_position_ = OnLeft;
+      }
+      else
+      {
+        ROS_INFO("Ready to kick : right");  // right
+        approach_ball_position_ = OnRight;
+      }
+
+      return true;
+    }
+    else if (count_to_kick_ > 15)
+    {
+      if (ball_x_angle > 0)
+        //if( current_pan_ > 0)
+        accum_ball_position_ += 1;
+      else
+        accum_ball_position_ -= 1;
+
+      // send message
+      setWalkingParam(MIN_FB_STEP, 0, 0);
+
+      return false;
+    }
+    //}
+  }
+  else
+  {
+    count_to_kick_ = 0;
+    accum_ball_position_ = 0;
+  }
+
+  fb_goal = fmin(distance_to_ball * 0.1, MAX_FB_STEP);
+  if ((distance_to_ball * 0.1 / 2) < current_x_move_)
+  {
+    fb_goal = fmin(current_x_move_ - UNIT_FB_STEP, fb_goal);
+    fb_move = fmax(fb_goal, MIN_FB_STEP);
+  }
+  else
+  {
+    fb_goal = fmin(current_x_move_ + UNIT_FB_STEP, fb_goal);
+    fb_move = fmax(fb_goal, MIN_FB_STEP);
+  }
+
+  ROS_INFO("distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", distance_to_ball, fb_move, delta_time);
+  ROS_INFO("==============================================");
+
+  // calc rl angle
+  double rl_goal, rl_angle;
+  if (fabs(current_pan_) * 180 / M_PI > 5.0)
+  {
+    double rl_offset = fabs(current_pan_) * 0.3;
+    //double rl_offset = fabs(ball_x_angle) * 0.3;
+    rl_goal = fmin(rl_offset, MAX_RL_TURN);
+    rl_goal = fmax(rl_goal, MIN_RL_TURN);
+    rl_angle = fmin(fabs(current_r_angle_) + UNIT_RL_TURN, rl_goal);
+
+    if (current_pan_ < 0)
+      rl_angle *= (-1);
+  }
+
+  // send message
+  setWalkingParam(fb_move, 0, rl_angle);
+
+  return false;
+}
+
+void BallFollower::waitFollowing()
+{
+  count_not_found_++;
+
+  if (count_not_found_ > NOT_FOUND_THRESHOLD * 0.5)
+    setWalkingParam(0.0, 0.0, 0.0);
+}
+
+void BallFollower::setWalkingCommand(const std::string &command)
+{
+  // get param
+  if (command == "start")
+  {
+    getWalkingParam();
+    setWalkingParam(MIN_FB_STEP, 0, 0, true);
+  }
+
+  std_msgs::String _command_msg;
+  _command_msg.data = command;
+  set_walking_command_pub_.publish(_command_msg);
+
+  ROS_INFO_STREAM("Send Walking command : " << command);
+}
+
+void BallFollower::setWalkingParam(double x_move, double y_move, double rotation_angle, bool balance)
+{
+  current_walking_param_.balance_enable = balance;
+  current_walking_param_.x_move_amplitude = x_move + SPOT_FB_OFFSET;
+  current_walking_param_.y_move_amplitude = y_move + SPOT_RL_OFFSET;
+  current_walking_param_.angle_move_amplitude = rotation_angle + SPOT_ANGLE_OFFSET;
+
+  set_walking_param_pub_.publish(current_walking_param_);
+
+  current_x_move_ = x_move;
+  current_r_angle_ = rotation_angle;
+}
+
+bool BallFollower::getWalkingParam()
+{
+  op3_walking_module_msgs::GetWalkingParam walking_param_msg;
+
+  if (get_walking_param_client_.call(walking_param_msg))
+  {
+    current_walking_param_ = walking_param_msg.response.parameters;
+
+    // update ui
+    ROS_INFO("Get walking parameters");
+
+    return true;
+  }
+  else
+  {
+    ROS_ERROR("Fail to get walking parameters.");
+
+    return false;
+  }
+
+}
+
+}
+

op3_demo/src/soccer/ball_tracker.cpp

@@ -0,0 +1,261 @@
+/*******************************************************************************
+ * 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 "op3_demo/ball_tracker.h"
+
+namespace robotis_op
+{
+
+BallTracker::BallTracker()
+    : nh_(ros::this_node::getName()),
+      //FOV_WIDTH(35.2 * M_PI / 180),
+      FOV_WIDTH(26.4 * M_PI / 180),
+      FOV_HEIGHT(21.6 * M_PI / 180),
+      NOT_FOUND_THRESHOLD(50),
+      use_head_scan_(true),
+      count_not_found_(0),
+      on_tracking_(false),
+      current_head_pan_(-10),
+      current_head_tilt_(-10),
+      current_ball_pan_(0),
+      current_ball_tilt_(0),
+      current_ball_bottom_(0),
+      DEBUG_PRINT(false)
+{
+  head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
+  head_scan_pub_ = nh_.advertise<std_msgs::String>("/robotis/head_control/scan_command", 0);
+
+  ball_position_sub_ = nh_.subscribe("/ball_detector_node/circle_set", 1, &BallTracker::ballPositionCallback, this);
+  ball_tracking_command_sub_ = nh_.subscribe("/ball_tracker/command", 1, &BallTracker::ballTrackerCommandCallback,
+                                             this);
+  // todo : remove
+  current_joint_states_sub_ = nh_.subscribe("/robotis/goal_joint_states", 10, &BallTracker::currentJointStatesCallback,
+                                            this);
+
+}
+
+BallTracker::~BallTracker()
+{
+
+}
+
+void BallTracker::ballPositionCallback(const ball_detector::circleSetStamped::ConstPtr &msg)
+{
+  for (int idx = 0; idx < msg->circles.size(); idx++)
+  {
+    if (ball_position_.z >= msg->circles[idx].z)
+      continue;
+
+    ball_position_ = msg->circles[idx];
+  }
+}
+
+void BallTracker::ballTrackerCommandCallback(const std_msgs::String::ConstPtr &msg)
+{
+  if (msg->data == "start")
+  {
+    startTracking();
+  }
+  else if (msg->data == "stop")
+  {
+    stopTracking();
+  }
+  else if (msg->data == "toggle_start")
+  {
+    if (on_tracking_ == false)
+      startTracking();
+    else
+      stopTracking();
+  }
+}
+
+void BallTracker::startTracking()
+{
+  on_tracking_ = true;
+  ROS_INFO("Start Ball tracking");
+}
+
+void BallTracker::stopTracking()
+{
+  on_tracking_ = false;
+  ROS_INFO("Stop Ball tracking");
+}
+
+void BallTracker::setUsingHeadScan(bool use_scan)
+{
+  use_head_scan_ = use_scan;
+}
+
+// todo : remove
+void BallTracker::currentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
+{
+  double pan, tilt;
+  int get_count = 0;
+
+  // pan : right (-) , left (+)
+  // tilt : top (+), bottom (-)
+  for (int ix = 0; ix < msg->name.size(); ix++)
+  {
+    if (msg->name[ix] == "head_pan")
+    {
+      // convert direction for internal variable(x / pan)
+      pan = -msg->position[ix];
+      get_count += 1;
+    }
+    else if (msg->name[ix] == "head_tilt")
+    {
+      tilt = msg->position[ix];
+      get_count += 1;
+    }
+
+    if (get_count == 2)
+      break;
+  }
+
+  // check variation
+  if (current_head_pan_ == -10 || fabs(pan - current_head_pan_) < 5 * M_PI / 180)
+    current_head_pan_ = pan;
+  if (current_head_tilt_ == -10 || fabs(tilt - current_head_tilt_) < 5 * M_PI / 180)
+    current_head_tilt_ = tilt;
+}
+
+bool BallTracker::processTracking()
+{
+  if (on_tracking_ == false)
+  {
+    ball_position_.z = 0;
+    count_not_found_ = 0;
+    current_head_pan_ = -10;
+    current_head_tilt_ = -10;
+    return false;
+  }
+
+  // check ball position
+  if (ball_position_.z <= 0)
+  {
+    count_not_found_++;
+
+    if (count_not_found_ > NOT_FOUND_THRESHOLD)
+    {
+      scanBall();
+      count_not_found_ = 0;
+    }
+
+    return false;
+  }
+
+  // if ball is found
+  // convert ball position to desired angle(rad) of head
+  // ball_position : top-left is (-1, -1), bottom-right is (+1, +1)
+  // offset_rad : top-left(+, +), bottom-right(-, -)
+  double x_error = -atan(ball_position_.x * tan(FOV_WIDTH));
+  double y_error = -atan(ball_position_.y * tan(FOV_HEIGHT));
+
+  ball_position_.z = 0;
+  count_not_found_ = 0;
+
+  if (DEBUG_PRINT == true)
+  {
+    std::cout << "--------------------------------------------------------------" << std::endl;
+    std::cout << "Ball position : " << ball_position_.x << " | " << ball_position_.y << std::endl;
+    std::cout << "Target angle : " << (x_error * 180 / M_PI) << " | " << (y_error * 180 / M_PI) << std::endl;
+    std::cout << "Head angle : " << (current_head_pan_ * 180 / M_PI) << " | " << (current_head_tilt_ * 180 / M_PI)
+              << std::endl;
+  }
+
+  ros::Time curr_time = ros::Time::now();
+  ros::Duration dur = curr_time - prev_time_;
+  double delta_time = dur.nsec * 0.000000001 + dur.sec;
+  prev_time_ = curr_time;
+
+  double p_gain = 0.7, d_gain = 0.02;
+  double x_error_diff = (x_error - current_ball_pan_) / delta_time;
+  double y_error_diff = (y_error - current_ball_tilt_) / delta_time;
+  double x_error_target = x_error * p_gain + x_error_diff * d_gain;
+  double y_error_target = y_error * p_gain + y_error_diff * d_gain;
+
+  if (DEBUG_PRINT == true)
+  {
+    std::cout << "--------------------------------------------------------------" << std::endl;
+    std::cout << "error         : " << (x_error * 180 / M_PI) << " | " << (y_error * 180 / M_PI) << std::endl;
+    std::cout << "error_diff    : " << (x_error_diff * 180 / M_PI) << " | " << (y_error_diff * 180 / M_PI) << " | "
+              << delta_time << std::endl;
+    std::cout << "error_target  : " << (x_error_target * 180 / M_PI) << " | " << (y_error_target * 180 / M_PI)
+              << " | P : " << p_gain << " | D : " << d_gain << std::endl;
+  }
+
+  // move head joint
+  publishHeadJoint(x_error_target, y_error_target);
+
+  // args for following ball
+  current_ball_pan_ = x_error;
+  current_ball_tilt_ = y_error;
+  current_ball_bottom_ = -atan(ball_position_.z * tan(FOV_HEIGHT));
+
+  return true;
+}
+
+void BallTracker::publishHeadJoint(double pan, double tilt)
+{
+  double min_angle = 1 * M_PI / 180;
+  if (fabs(pan) < min_angle && fabs(tilt) < min_angle)
+    return;
+
+  sensor_msgs::JointState head_angle_msg;
+
+  head_angle_msg.name.push_back("head_pan");
+  head_angle_msg.name.push_back("head_tilt");
+
+  head_angle_msg.position.push_back(pan);
+  head_angle_msg.position.push_back(tilt);
+
+  head_joint_pub_.publish(head_angle_msg);
+}
+
+void BallTracker::scanBall()
+{
+  if (use_head_scan_ == false)
+    return;
+
+  // check head control module enabled
+  // ...
+
+  // send message to head control module
+  std_msgs::String scan_msg;
+  scan_msg.data = "scan";
+
+  head_scan_pub_.publish(scan_msg);
+  // ROS_INFO("Scan the ball");
+}
+
+}
+

op3_demo/src/soccer/soccer_demo.cpp

@@ -0,0 +1,517 @@
+/*******************************************************************************
+ * 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 "op3_demo/soccer_demo.h"
+
+namespace robotis_op
+{
+
+SoccerDemo::SoccerDemo()
+    : FALLEN_FORWARD_LIMIT(60),
+      FALLEN_BEHIND_LIMIT(-60),
+      SPIN_RATE(30),
+      debug_code_(false),
+      //enable_(false),
+      wait_count_(0),
+      on_following_ball_(false),
+      restart_soccer_(false),
+      start_following_(false),
+      stop_following_(false),
+      stop_fallen_check_(false),
+      robot_status_(Waited),
+      stand_state_(Stand),
+      present_pitch_(0)
+{
+  //init ros
+  enable_ = false;
+
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  std::string default_path = ros::package::getPath("op3_gui_demo") + "/config/demo_config.yaml";
+  std::string path = nh.param<std::string>("demo_config", default_path);
+  parseJointNameFromYaml(path);
+
+  boost::thread queue_thread = boost::thread(boost::bind(&SoccerDemo::callbackThread, this));
+  boost::thread process_thread = boost::thread(boost::bind(&SoccerDemo::processThread, this));
+}
+
+SoccerDemo::~SoccerDemo()
+{
+
+}
+
+void SoccerDemo::setDemoEnable()
+{
+  enable_ = true;
+
+  startSoccerMode();
+
+  // handle enable procedure
+  //  ball_tracker_.startTracking();
+  //  ball_follower_.startFollowing();
+
+  //  wait_count_ = 1 * SPIN_RATE;
+}
+
+void SoccerDemo::setDemoDisable()
+{
+  setModuleToDemo("base_module");
+
+  // handle disable procedure
+  ball_tracker_.stopTracking();
+  ball_follower_.stopFollowing();
+
+  enable_ = false;
+  wait_count_ = 0;
+  on_following_ball_ = false;
+  restart_soccer_ = false;
+  start_following_ = false;
+  stop_following_ = false;
+  stop_fallen_check_ = false;
+}
+
+void SoccerDemo::process()
+{
+  // ball tracking
+  bool is_tracked;
+
+  is_tracked = ball_tracker_.processTracking();
+
+  // check to start
+  if (start_following_ == true)
+  {
+    ball_tracker_.startTracking();
+    ball_follower_.startFollowing();
+    start_following_ = false;
+
+    wait_count_ = 1 * SPIN_RATE;  // wait 1 sec
+  }
+
+  // check to stop
+  if (stop_following_ == true)
+  {
+    //ball_tracker_.stopTracking();
+    ball_follower_.stopFollowing();
+    stop_following_ = false;
+
+    wait_count_ = 0;
+  }
+
+  if (wait_count_ <= 0)
+  {
+    // ball following
+    if (on_following_ball_ == true)
+    {
+      if (is_tracked)
+        ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), ball_tracker_.getBallSize());
+      else
+        ball_follower_.waitFollowing();
+    }
+
+    // check fallen states
+    switch (stand_state_)
+    {
+      case Stand:
+      {
+        // check restart soccer
+        if (restart_soccer_ == true)
+        {
+          restart_soccer_ = false;
+          startSoccerMode();
+          break;
+        }
+
+        // check states for kick
+        int ball_position = ball_follower_.getBallPosition();
+        if (ball_position != robotis_op::BallFollower::NotFound)
+        {
+          ball_follower_.stopFollowing();
+          handleKick(ball_position);
+        }
+        break;
+      }
+        // fallen state : Fallen_Forward, Fallen_Behind
+      default:
+      {
+        ball_follower_.stopFollowing();
+        handleFallen(stand_state_);
+        break;
+      }
+    }
+  }
+  else
+  {
+    wait_count_ -= 1;
+  }
+
+}
+
+void SoccerDemo::processThread()
+{
+  bool result = false;
+
+  //set node loop rate
+  ros::Rate loop_rate(SPIN_RATE);
+
+  ball_tracker_.startTracking();
+
+  //node loop
+  while (ros::ok())
+  {
+    if (enable_ == true)
+      process();
+
+    //relax to fit output rate
+    loop_rate.sleep();
+  }
+}
+
+void SoccerDemo::callbackThread()
+{
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  // subscriber & publisher
+  module_control_pub_ = nh.advertise<robotis_controller_msgs::JointCtrlModule>("/robotis/set_joint_ctrl_modules", 0);
+  motion_index_pub_ = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 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);
+  imu_data_sub_ = nh.subscribe("/robotis/open_cr/imu", 1, &SoccerDemo::imuDataCallback, this);
+
+  while (nh.ok())
+  {
+    ros::spinOnce();
+
+    usleep(1000);
+  }
+}
+
+void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_head_control)
+{
+  robotis_controller_msgs::JointCtrlModule control_msg;
+
+  std::string head_module = "head_control_module";
+  std::map<int, std::string>::iterator joint_iter;
+
+  for (joint_iter = id_joint_table_.begin(); joint_iter != id_joint_table_.end(); ++joint_iter)
+  {
+    // check whether joint name contains "head"
+    if (joint_iter->second.find("head") != std::string::npos)
+    {
+      if (with_head_control == true)
+      {
+        control_msg.joint_name.push_back(joint_iter->second);
+        control_msg.module_name.push_back(head_module);
+      }
+      else
+        continue;
+    }
+    else
+    {
+      control_msg.joint_name.push_back(joint_iter->second);
+      control_msg.module_name.push_back(body_module);
+    }
+  }
+
+  // no control
+  if (control_msg.joint_name.size() == 0)
+    return;
+
+  module_control_pub_.publish(control_msg);
+  std::cout << "enable module of body : " << body_module << std::endl;
+}
+
+void SoccerDemo::setModuleToDemo(const std::string &module_name)
+{
+  robotis_controller_msgs::JointCtrlModule control_msg;
+  std::map<int, std::string>::iterator joint_iter;
+
+  for (joint_iter = id_joint_table_.begin(); joint_iter != id_joint_table_.end(); ++joint_iter)
+  {
+    control_msg.joint_name.push_back(joint_iter->second);
+    control_msg.module_name.push_back(module_name);
+  }
+
+  // no control
+  if (control_msg.joint_name.size() == 0)
+    return;
+
+  module_control_pub_.publish(control_msg);
+  std::cout << "enable module : " << module_name << std::endl;
+}
+
+void SoccerDemo::parseJointNameFromYaml(const std::string &path)
+{
+  YAML::Node doc;
+  try
+  {
+    // load yaml
+    doc = YAML::LoadFile(path.c_str());
+  } catch (const std::exception& e)
+  {
+    ROS_ERROR("Fail to load id_joint table yaml.");
+    return;
+  }
+
+  // parse id_joint table
+  YAML::Node _id_sub_node = doc["id_joint"];
+  for (YAML::iterator _it = _id_sub_node.begin(); _it != _id_sub_node.end(); ++_it)
+  {
+    int _id;
+    std::string _joint_name;
+
+    _id = _it->first.as<int>();
+    _joint_name = _it->second.as<std::string>();
+
+    id_joint_table_[_id] = _joint_name;
+    joint_id_table_[_joint_name] = _id;
+  }
+}
+
+// joint id -> joint name
+bool SoccerDemo::getJointNameFromID(const int &id, std::string &joint_name)
+{
+  std::map<int, std::string>::iterator _iter;
+
+  _iter = id_joint_table_.find(id);
+  if (_iter == id_joint_table_.end())
+    return false;
+
+  joint_name = _iter->second;
+  return true;
+}
+
+// joint name -> joint id
+bool SoccerDemo::getIDFromJointName(const std::string &joint_name, int &id)
+{
+  std::map<std::string, int>::iterator _iter;
+
+  _iter = joint_id_table_.find(joint_name);
+  if (_iter == joint_id_table_.end())
+    return false;
+
+  id = _iter->second;
+  return true;
+}
+
+int SoccerDemo::getJointCount()
+{
+  return joint_id_table_.size();
+}
+
+bool SoccerDemo::isHeadJoint(const int &id)
+{
+  std::map<std::string, int>::iterator _iter;
+
+  for (_iter = joint_id_table_.begin(); _iter != joint_id_table_.end(); ++_iter)
+  {
+    if (_iter->first.find("head") != std::string::npos)
+      return true;
+  }
+
+  return false;
+}
+
+void SoccerDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
+{
+  if (enable_ == false)
+    return;
+
+  if (msg->data == "start")
+  {
+    if (on_following_ball_ == true)
+      stopSoccerMode();
+    else
+      startSoccerMode();
+  }
+}
+
+void SoccerDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg)
+{
+  if (enable_ == false)
+    return;
+
+  if (msg->data == "start")
+  {
+    if (on_following_ball_ == true)
+      stopSoccerMode();
+    else
+      startSoccerMode();
+  }
+  else if (msg->data == "stop")
+  {
+    stopSoccerMode();
+  }
+}
+
+// check fallen states
+void SoccerDemo::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
+{
+  if (enable_ == false)
+    return;
+
+  if (stop_fallen_check_ == true)
+    return;
+
+  Eigen::Quaterniond orientation(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
+  Eigen::MatrixXd rpy_orientation = robotis_framework::convertQuaternionToRPY(orientation);
+  rpy_orientation *= (180 / M_PI);
+
+  ROS_INFO_COND(debug_code_, "Roll : %3.2f, Pitch : %2.2f", rpy_orientation.coeff(0, 0), rpy_orientation.coeff(1, 0));
+
+  double pitch = rpy_orientation.coeff(1, 0);
+
+  double alpha = 0.4;
+  if (present_pitch_ == 0)
+    present_pitch_ = pitch;
+  else
+    present_pitch_ = present_pitch_ * (1 - alpha) + pitch * alpha;
+
+  if (present_pitch_ > FALLEN_FORWARD_LIMIT)
+    stand_state_ = Fallen_Forward;
+  else if (present_pitch_ < FALLEN_BEHIND_LIMIT)
+    stand_state_ = Fallen_Behind;
+  else
+    stand_state_ = Stand;
+}
+
+void SoccerDemo::startSoccerMode()
+{
+  setBodyModuleToDemo("walking_module");
+
+  usleep(10 * 1000);
+
+  ROS_INFO("Start Soccer Demo");
+  on_following_ball_ = true;
+  start_following_ = true;
+}
+
+void SoccerDemo::stopSoccerMode()
+{
+  ROS_INFO("Stop Soccer Demo");
+  on_following_ball_ = false;
+  stop_following_ = true;
+}
+
+void SoccerDemo::handleKick(int ball_position)
+{
+  usleep(1000 * 1000);
+
+  // change to motion module
+  setModuleToDemo("action_module");
+
+  usleep(1500 * 1000);
+
+  if (handleFallen(stand_state_) == true)
+    return;
+
+  // kick motion
+  switch (ball_position)
+  {
+    case robotis_op::BallFollower::OnRight:
+      std::cout << "Kick Motion [R]: " << ball_position << std::endl;
+      playMotion(RightKick);
+      break;
+
+    case robotis_op::BallFollower::OnLeft:
+      std::cout << "Kick Motion [L]: " << ball_position << std::endl;
+      playMotion(LeftKick);
+      break;
+
+    default:
+      break;
+  }
+
+  // todo : remove this in order to play soccer repeatedly
+  on_following_ball_ = false;
+
+  usleep(2000 * 1000);
+
+  if (handleFallen(stand_state_) == true)
+    return;
+
+  // ceremony
+  //std::cout << "Go Ceremony!!!" << std::endl;
+  //playMotion(Ceremony);
+
+  //restart_soccer_ = true;
+}
+
+bool SoccerDemo::handleFallen(int fallen_status)
+{
+  if (fallen_status == Stand)
+  {
+    return false;
+  }
+
+  // change to motion module
+  setModuleToDemo("action_module");
+
+  usleep(600 * 1000);
+
+  // getup motion
+  switch (fallen_status)
+  {
+    case Fallen_Forward:
+      std::cout << "Getup Motion [F]: " << std::endl;
+      playMotion(GetUpFront);
+      break;
+
+    case Fallen_Behind:
+      std::cout << "Getup Motion [B]: " << std::endl;
+      playMotion(GetUpBack);
+      break;
+
+    default:
+      break;
+  }
+
+  usleep(500 * 1000);
+
+  if (on_following_ball_ == true)
+    restart_soccer_ = true;
+
+  // reset state
+  on_following_ball_ = false;
+
+  return true;
+}
+
+void SoccerDemo::playMotion(int motion_index)
+{
+  std_msgs::Int32 motion_msg;
+  motion_msg.data = motion_index;
+
+  motion_index_pub_.publish(motion_msg);
+}
+
+}

op3_demo/src/soccer_demo_node.cpp

@@ -0,0 +1,504 @@
+/*******************************************************************************
+ * 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 <ros/ros.h>
+#include <std_msgs/String.h>
+#include <sensor_msgs/Imu.h>
+#include <boost/thread.hpp>
+
+#include "op3_demo/ball_tracker.h"
+#include "op3_demo/ball_follower.h"
+#include "robotis_math/robotis_linear_algebra.h"
+
+enum Motion_Index
+{
+  GetUpFront = 81,
+  GetUpBack = 82,
+  RightKick = 83,
+  LeftKick = 84,
+  Ceremony = 85,
+};
+
+enum Stand_Status
+{
+  Stand = 0,
+  Fallen_Forward = 1,
+  Fallen_Behind = 2,
+};
+
+enum Robot_Status
+{
+  Waited = 0,
+  TrackingAndFollowing = 1,
+  ReadyToKick = 2,
+  ReadyToCeremony = 3,
+  ReadyToGetup = 4,
+};
+
+const double FALLEN_FORWARD_LIMIT = 60;
+const double FALLEN_BEHIND_LIMIT = -60;
+const int SPIN_RATE = 30;
+
+void callbackThread();
+
+void setBodyModuleToDemo(const std::string &body_module);
+void setModuleToDemo(const std::string &module_name);
+void parseJointNameFromYaml(const std::string &path);
+bool getJointNameFromID(const int &id, std::string &joint_name);
+bool getIDFromJointName(const std::string &joint_name, int &id);
+void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
+void demoCommandCallback(const std_msgs::String::ConstPtr& msg);
+void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg);
+
+void startSoccerMode();
+void stopSoccerMode();
+
+void handleKick(int ball_position);
+bool handleFallen(int fallen_status);
+
+void playMotion(int motion_index);
+
+ros::Publisher module_control_pub;
+ros::Publisher motion_index_pub;
+ros::Subscriber buttuon_sub;
+ros::Subscriber demo_command_sub;
+ros::Subscriber imu_data_sub;
+std::map<int, std::string> id_joint_table;
+std::map<std::string, int> joint_id_table;
+
+int wait_count = 0;
+bool on_following_ball = false;
+bool restart_soccer = false;
+bool start_following = false;
+bool stop_following = false;
+bool stop_fallen_check = false;
+int robot_status = Waited;
+int stand_state = Stand;
+double present_pitch = 0;
+
+bool debug_code = false;
+
+//node main
+int main(int argc, char **argv)
+{
+  //init ros
+  ros::init(argc, argv, "soccer_demo_node");
+
+  //create ros wrapper object
+  robotis_op::BallTracker tracker;
+  robotis_op::BallFollower follower;
+
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  std::string _default_path = ros::package::getPath("op3_gui_demo") + "/config/demo_config.yaml";
+  std::string _path = nh.param<std::string>("demo_config", _default_path);
+  parseJointNameFromYaml(_path);
+
+  boost::thread queue_thread = boost::thread(boost::bind(&callbackThread));
+
+  bool result = false;
+
+  //set node loop rate
+  ros::Rate loop_rate(SPIN_RATE);
+
+  tracker.startTracking();
+
+  //node loop
+  while (ros::ok())
+  {
+    // ball tracking
+    bool is_tracked;
+    is_tracked = tracker.processTracking();
+
+    if (start_following == true)
+    {
+      tracker.startTracking();
+      follower.startFollowing();
+      start_following = false;
+
+      wait_count = 1 * SPIN_RATE;  // wait 1 sec
+    }
+
+    if (stop_following == true)
+    {
+      follower.stopFollowing();
+      stop_following = false;
+
+      wait_count = 0;
+    }
+
+    if (wait_count <= 0)
+    {
+      // ball following
+      if (on_following_ball == true)
+      {
+        if (is_tracked)
+          follower.processFollowing(tracker.getPanOfBall(), tracker.getTiltOfBall(), tracker.getBallSize());
+        else
+          follower.waitFollowing();
+      }
+
+      // check fallen states
+      switch (stand_state)
+      {
+        case Stand:
+        {
+          // check restart soccer
+          if (restart_soccer == true)
+          {
+            restart_soccer = false;
+            startSoccerMode();
+            break;
+          }
+
+          // check states for kick
+          int ball_position = follower.getBallPosition();
+          if (ball_position != robotis_op::BallFollower::NotFound)
+          {
+            follower.stopFollowing();
+            handleKick(ball_position);
+          }
+          break;
+        }
+
+          // fallen state : Fallen_Forward, Fallen_Behind
+        default:
+        {
+          follower.stopFollowing();
+          handleFallen(stand_state);
+          break;
+        }
+      }
+    }
+    else
+    {
+      wait_count -= 1;
+    }
+
+    //execute pending callbacks
+    ros::spinOnce();
+
+    //relax to fit output rate
+    loop_rate.sleep();
+  }
+
+  //exit program
+  return 0;
+}
+
+void callbackThread()
+{
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  // subscriber & publisher
+  module_control_pub = nh.advertise<robotis_controller_msgs::JointCtrlModule>("/robotis/set_joint_ctrl_modules", 0);
+  motion_index_pub = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
+  buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback);
+  demo_command_sub = nh.subscribe("/ball_tracker/command", 1, demoCommandCallback);
+  imu_data_sub = nh.subscribe("/robotis/open_cr/imu", 1, imuDataCallback);
+
+  while (nh.ok())
+  {
+    ros::spinOnce();
+
+    usleep(1000);
+  }
+}
+
+void setBodyModuleToDemo(const std::string &body_module)
+{
+  robotis_controller_msgs::JointCtrlModule control_msg;
+
+  //std::string body_module = "action_module";
+  std::string head_module = "head_control_module";
+
+  // todo : remove hard coding
+  for (int ix = 1; ix <= 20; ix++)
+  {
+    std::string joint_name;
+
+    if (getJointNameFromID(ix, joint_name) == false)
+      continue;
+
+    control_msg.joint_name.push_back(joint_name);
+    if (ix <= 18)
+      control_msg.module_name.push_back(body_module);
+    else
+      control_msg.module_name.push_back(head_module);
+
+  }
+
+  // no control
+  if (control_msg.joint_name.size() == 0)
+    return;
+
+  module_control_pub.publish(control_msg);
+  std::cout << "enable module of body : " << body_module << std::endl;
+}
+
+void setModuleToDemo(const std::string &module_name)
+{
+  robotis_controller_msgs::JointCtrlModule control_msg;
+
+  // todo : remove hard coding
+  for (int ix = 1; ix <= 20; ix++)
+  {
+    std::string joint_name;
+
+    if (getJointNameFromID(ix, joint_name) == false)
+      continue;
+
+    control_msg.joint_name.push_back(joint_name);
+    control_msg.module_name.push_back(module_name);
+  }
+
+  // no control
+  if (control_msg.joint_name.size() == 0)
+    return;
+
+  module_control_pub.publish(control_msg);
+  std::cout << "enable module : " << module_name << std::endl;
+}
+
+void parseJointNameFromYaml(const std::string &path)
+{
+  YAML::Node doc;
+  try
+  {
+    // load yaml
+    doc = YAML::LoadFile(path.c_str());
+  } catch (const std::exception& e)
+  {
+    ROS_ERROR("Fail to load id_joint table yaml.");
+    return;
+  }
+
+  // parse id_joint table
+  YAML::Node _id_sub_node = doc["id_joint"];
+  for (YAML::iterator _it = _id_sub_node.begin(); _it != _id_sub_node.end(); ++_it)
+  {
+    int _id;
+    std::string _joint_name;
+
+    _id = _it->first.as<int>();
+    _joint_name = _it->second.as<std::string>();
+
+    id_joint_table[_id] = _joint_name;
+    joint_id_table[_joint_name] = _id;
+  }
+}
+
+// joint id -> joint name
+bool getJointNameFromID(const int &id, std::string &joint_name)
+{
+  std::map<int, std::string>::iterator _iter;
+
+  _iter = id_joint_table.find(id);
+  if (_iter == id_joint_table.end())
+    return false;
+
+  joint_name = _iter->second;
+  return true;
+}
+
+// joint name -> joint id
+bool getIDFromJointName(const std::string &joint_name, int &id)
+{
+  std::map<std::string, int>::iterator _iter;
+
+  _iter = joint_id_table.find(joint_name);
+  if (_iter == joint_id_table.end())
+    return false;
+
+  id = _iter->second;
+  return true;
+}
+
+void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
+{
+  if (msg->data == "start")
+  {
+    if (on_following_ball == true)
+      stopSoccerMode();
+    else
+      startSoccerMode();
+  }
+}
+
+void demoCommandCallback(const std_msgs::String::ConstPtr &msg)
+{
+  if (msg->data == "start")
+  {
+    if (on_following_ball == true)
+      stopSoccerMode();
+    else
+      startSoccerMode();
+  }
+  else if (msg->data == "stop")
+  {
+    stopSoccerMode();
+  }
+}
+
+// check fallen states
+void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg)
+{
+  if (stop_fallen_check == true)
+    return;
+
+  Eigen::Quaterniond orientation(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);
+  Eigen::MatrixXd rpy_orientation = robotis_framework::convertQuaternionToRPY(orientation);
+  rpy_orientation *= (180 / M_PI);
+
+  double pitch = rpy_orientation.coeff(1, 0);
+
+  if (present_pitch == 0)
+    present_pitch = pitch;
+  else
+    present_pitch = present_pitch * 0.5 + pitch * 0.5;
+
+  if (present_pitch > FALLEN_FORWARD_LIMIT)
+    stand_state = Fallen_Forward;
+  else if (present_pitch < FALLEN_BEHIND_LIMIT)
+    stand_state = Fallen_Behind;
+  else
+    stand_state = Stand;
+}
+
+void startSoccerMode()
+{
+  setBodyModuleToDemo("walking_module");
+
+  usleep(10 * 1000);
+
+  ROS_INFO("Start Soccer Demo");
+  on_following_ball = true;
+  start_following = true;
+}
+
+void stopSoccerMode()
+{
+  ROS_INFO("Stop Soccer Demo");
+  on_following_ball = false;
+  stop_following = true;
+}
+
+void handleKick(int ball_position)
+{
+  usleep(500 * 1000);
+
+  // change to motion module
+  // setBodyModuleToDemo("action_module");
+  setModuleToDemo("action_module");
+
+  usleep(1000 * 1000);
+
+  if (handleFallen(stand_state) == true)
+    return;
+
+  // kick motion
+  switch (ball_position)
+  {
+    case robotis_op::BallFollower::OnRight:
+      std::cout << "Kick Motion [R]: " << ball_position << std::endl;
+      playMotion(RightKick);
+      break;
+
+    case robotis_op::BallFollower::OnLeft:
+      std::cout << "Kick Motion [L]: " << ball_position << std::endl;
+      playMotion(LeftKick);
+      break;
+
+    default:
+      break;
+  }
+
+  on_following_ball = false;
+
+  usleep(2000 * 1000);
+
+  if (handleFallen(stand_state) == true)
+    return;
+
+  // ceremony
+  std::cout << "Go Ceremony!!!" << std::endl;
+  playMotion(Ceremony);
+}
+
+bool handleFallen(int fallen_status)
+{
+  if (fallen_status == Stand)
+  {
+    return false;
+  }
+
+  // change to motion module
+  setModuleToDemo("action_module");
+
+  usleep(600 * 1000);
+
+  // getup motion
+  switch (fallen_status)
+  {
+    case Fallen_Forward:
+      std::cout << "Getup Motion [F]: " << std::endl;
+      playMotion(GetUpFront);
+      break;
+
+    case Fallen_Behind:
+      std::cout << "Getup Motion [B]: " << std::endl;
+      playMotion(GetUpBack);
+      break;
+
+    default:
+      break;
+  }
+
+  usleep(500 * 1000);
+
+  if (on_following_ball == true)
+    restart_soccer = true;
+
+  // reset state
+  //stand_state = Stand;
+  on_following_ball = false;
+
+  return true;
+}
+
+void playMotion(int motion_index)
+{
+  std_msgs::Int32 motion_msg;
+  motion_msg.data = motion_index;
+
+  motion_index_pub.publish(motion_msg);
+}

op3_demo/src/test/button_test.cpp

@@ -0,0 +1,268 @@
+/*******************************************************************************
+ * 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 "op3_demo/button_test.h"
+
+namespace robotis_op
+{
+
+ButtonTest::ButtonTest()
+    : SPIN_RATE(30),
+      led_count_(0),
+      rgb_led_count_(0)
+//      is_tracking_(false),
+//      tracking_status_(FaceTracker::Waiting)
+{
+  enable_ = false;
+
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  boost::thread queue_thread = boost::thread(boost::bind(&ButtonTest::callbackThread, this));
+  boost::thread process_thread = boost::thread(boost::bind(&ButtonTest::processThread, this));
+
+  default_mp3_path_ = ros::package::getPath("op3_demo") + "/Data/mp3/test/";
+}
+
+ButtonTest::~ButtonTest()
+{
+  // TODO Auto-generated destructor stub
+}
+
+void ButtonTest::setDemoEnable()
+{
+  // change to motion module
+//  setModuleToDemo("action_module");
+
+//  usleep(100 * 1000);
+
+//  playMotion(InitPose);
+
+//  usleep(1500 * 1000);
+
+//  setModuleToDemo("head_control_module");
+
+//  usleep(10 * 1000);
+
+  enable_ = true;
+//  face_tracker_.startTracking();
+
+  ROS_INFO("Start Button Test");
+
+}
+
+void ButtonTest::setDemoDisable()
+{
+
+//  face_tracker_.stopTracking();
+//  is_tracking_ = false;
+//  tracking_status_ = FaceTracker::Waiting;
+  enable_ = false;
+}
+
+void ButtonTest::process()
+{
+  //bool is_tracked = face_tracker_.processTracking();
+//  int tracking_status = face_tracker_.processTracking();
+
+//if(is_tracking_ != is_tracked)
+//  if(tracking_status_ != tracking_status)
+//  {
+//    switch(tracking_status)
+//    {
+//      case FaceTracker::Found:
+//        setRGBLED(0x1F, 0x1F, 0x1F);
+//        break;
+//
+//      case FaceTracker::NotFound:
+//        setRGBLED(0, 0, 0);
+//        break;
+//
+//      default:
+//        break;
+//    }
+//  }
+
+//  if(tracking_status != FaceTracker::Waiting)
+//    tracking_status_ = tracking_status;
+
+//is_tracking_ = is_tracked;
+//  std::cout << "Tracking : " << tracking_status << std::endl;
+}
+
+void ButtonTest::processThread()
+{
+  //set node loop rate
+  ros::Rate loop_rate(SPIN_RATE);
+
+  //node loop
+  while (ros::ok())
+  {
+    if (enable_ == true)
+      process();
+
+    //relax to fit output rate
+    loop_rate.sleep();
+  }
+}
+
+void ButtonTest::callbackThread()
+{
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  // subscriber & publisher
+//  module_control_pub_ = nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 0);
+//  motion_index_pub_ = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
+  rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
+  play_sound_pub_ = nh.advertise<std_msgs::String>("/play_sound_file", 0);
+
+  buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &ButtonTest::buttonHandlerCallback, this);
+//  faceCoord_sub_ = nh.subscribe("/faceCoord", 1, &ButtonTest::facePositionCallback, this);
+
+  while (nh.ok())
+  {
+    ros::spinOnce();
+
+    usleep(1 * 1000);
+  }
+}
+
+// button test
+void ButtonTest::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
+{
+  if (enable_ == false)
+    return;
+
+  if (msg->data == "mode")
+  {
+    playSound(default_mp3_path_ + "Mode button pressed.mp3");
+  }
+  else if (msg->data == "start")
+  {
+    // RGB led color test
+    playSound(default_mp3_path_ + "Start button pressed.mp3");
+    int rgb_selector[3] = { 1, 0, 0 };
+    setRGBLED((0x1F * rgb_selector[rgb_led_count_ % 3]), (0x1F * rgb_selector[(rgb_led_count_ + 1) % 3]),
+              (0x1F * rgb_selector[(rgb_led_count_ + 2) % 3]));
+    rgb_led_count_ += 1;
+  }
+  else if (msg->data == "user")
+  {
+    // Monochromatic led color test
+    playSound(default_mp3_path_ + "User button pressed.mp3");
+    setLED(0x01 << (led_count_++ % 3));
+  }
+  else if (msg->data == "mode_long")
+  {
+    playSound(default_mp3_path_ + "Mode button long pressed.mp3");
+  }
+  else if (msg->data == "start_long")
+  {
+    playSound(default_mp3_path_ + "Start button long pressed.mp3");
+  }
+  else if (msg->data == "user_long")
+  {
+    playSound(default_mp3_path_ + "User button long pressed.mp3");
+  }
+}
+
+//void ButtonTest::setModuleToDemo(const std::string &module_name)
+//{
+//  std_msgs::String control_msg;
+//  control_msg.data = module_name;
+//
+//  module_control_pub_.publish(control_msg);
+//  std::cout << "enable module : " << module_name << std::endl;
+//}
+//
+//void ButtonTest::facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg)
+//{
+//  if (enable_ == false)
+//    return;
+//
+//  // face is detected
+//  if (msg->data.size() >= 10)
+//  {
+//    // center of face
+//    face_position_.x = (msg->data[6] + msg->data[8] * 0.5) / msg->data[2] * 2 - 1;
+//    face_position_.y = (msg->data[7] + msg->data[9] * 0.5) / msg->data[3] * 2 - 1;
+//    face_position_.z = msg->data[8] * 0.5 + msg->data[9] * 0.5;
+//
+//    face_tracker_.setFacePosition(face_position_);
+//  }
+//  else
+//  {
+//    face_position_.x = 0;
+//    face_position_.y = 0;
+//    face_position_.z = 0;
+//    return;
+//  }
+//}
+//
+//void ButtonTest::playMotion(int motion_index)
+//{
+//  std_msgs::Int32 motion_msg;
+//  motion_msg.data = motion_index;
+//
+//  motion_index_pub_.publish(motion_msg);
+//}
+
+void ButtonTest::setRGBLED(int blue, int green, int red)
+{
+  int led_full_unit = 0x1F;
+  int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | (red & led_full_unit);
+  robotis_controller_msgs::SyncWriteItem syncwrite_msg;
+  syncwrite_msg.item_name = "LED_RGB";
+  syncwrite_msg.joint_name.push_back("open-cr");
+  syncwrite_msg.value.push_back(led_value);
+
+  rgb_led_pub_.publish(syncwrite_msg);
+}
+
+void ButtonTest::setLED(int led)
+{
+  robotis_controller_msgs::SyncWriteItem syncwrite_msg;
+  syncwrite_msg.item_name = "LED";
+  syncwrite_msg.joint_name.push_back("open-cr");
+  syncwrite_msg.value.push_back(led);
+
+  rgb_led_pub_.publish(syncwrite_msg);
+}
+
+void ButtonTest::playSound(const std::string &path)
+{
+  std_msgs::String sound_msg;
+  sound_msg.data = path;
+
+  play_sound_pub_.publish(sound_msg);
+}
+
+} /* namespace robotis_op */

op3_demo/src/test/mic_test.cpp

@@ -0,0 +1,295 @@
+/*******************************************************************************
+ * 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 "op3_demo/mic_test.h"
+
+namespace robotis_op
+{
+
+MicTest::MicTest()
+    : SPIN_RATE(30),
+      is_wait_(false),
+      wait_time_(-1),
+      test_status_(Ready),
+      record_pid_(-1),
+      play_pid_(-1)
+{
+  enable_ = false;
+
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  boost::thread queue_thread = boost::thread(boost::bind(&MicTest::callbackThread, this));
+  boost::thread process_thread = boost::thread(boost::bind(&MicTest::processThread, this));
+
+  recording_file_name_ = ros::package::getPath("op3_demo") + "/Data/mp3/test/mic-test.wav";
+  default_mp3_path_ = ros::package::getPath("op3_demo") + "/Data/mp3/test/";
+
+  start_time_ = ros::Time::now();
+}
+
+MicTest::~MicTest()
+{
+  // TODO Auto-generated destructor stub
+}
+
+void MicTest::setDemoEnable()
+{
+  wait_time_ = -1;
+  test_status_ = AnnounceRecording;
+  enable_ = true;
+
+  ROS_INFO("Start Mic test Demo");
+
+}
+
+void MicTest::setDemoDisable()
+{
+  finishTimer();
+
+  test_status_ = Ready;
+  enable_ = false;
+}
+
+void MicTest::process()
+{
+  // check status
+  // timer
+  if (wait_time_ > 0)
+  {
+    ros::Duration dur = ros::Time::now() - start_time_;
+
+    // check timer
+    if (dur.sec >= wait_time_)
+    {
+      finishTimer();
+    }
+  }
+  else if (wait_time_ == -1.0)
+  {
+    // handle test process
+    switch (test_status_)
+    {
+      case Ready:
+        // do nothing
+        break;
+
+      case AnnounceRecording:
+        announceTest();
+        test_status_ = MicRecording;
+        break;
+
+      case MicRecording:
+        recordSound();
+        test_status_ = PlayingSound;
+        break;
+
+      case PlayingSound:
+        playTestSound(recording_file_name_);
+        test_status_ = DeleteTempFile;
+        break;
+
+      case DeleteTempFile:
+        deleteSoundFile(recording_file_name_);
+        test_status_ = Ready;
+        break;
+
+      default:
+        break;
+    }
+  }
+
+}
+
+void MicTest::processThread()
+{
+  //set node loop rate
+  ros::Rate loop_rate(SPIN_RATE);
+
+  //node loop
+  while (ros::ok())
+  {
+    if (enable_ == true)
+      process();
+
+    //relax to fit output rate
+    loop_rate.sleep();
+  }
+}
+
+void MicTest::callbackThread()
+{
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  // subscriber & publisher
+  buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &MicTest::buttonHandlerCallback, this);
+  play_sound_pub_ = nh.advertise<std_msgs::String>("/play_sound_file", 0);
+
+  while (nh.ok())
+  {
+    ros::spinOnce();
+
+    usleep(1 * 1000);
+  }
+}
+
+void MicTest::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
+{
+  if (enable_ == false)
+    return;
+
+  if (msg->data == "start")
+  {
+    // restart mic test
+    if (test_status_ != Ready)
+      return;
+
+    test_status_ = AnnounceRecording;
+  }
+  else if(msg->data == "user")
+  {
+    is_wait_ = true;
+  }
+}
+
+void MicTest::announceTest()
+{
+  // play mic test sound
+  playSound(default_mp3_path_ + "Announce mic test.mp3");
+
+  //startTimer(3.0);
+  usleep(3.4 * 1000 * 1000);
+}
+
+void MicTest::recordSound(int recording_time)
+{
+  ROS_INFO("Start to record");
+  // arecord -D plughw:1,0 -f S16_LE -c1 -r22050 -t raw -d 5 | lame -r -s 22.05 -m m -b 64 - mic-input.mp3
+  // arecord -D plughw:1,0 -f S16_LE -c1 -r22050 -t wav -d 5 test.wav
+
+  playSound(default_mp3_path_ + "Start recording.mp3");
+
+  usleep(1.5 * 1000 * 1000);
+
+  if (record_pid_ != -1)
+    kill(record_pid_, SIGKILL);
+
+  record_pid_ = fork();
+
+  switch (record_pid_)
+  {
+    case -1:
+      fprintf(stderr, "Fork Failed!! \n");
+      ROS_WARN("Fork Failed!! \n");
+      //done_msg.data = "play_sound_fail";
+      //g_done_msg_pub.publish(done_msg);
+      break;
+
+    case 0:
+    {
+      std::stringstream ss;
+      ss << "-d " << recording_time;
+      execl("/usr/bin/arecord", "arecord", "-Dplughw:1,0", "-fS16_LE", "-c1", "-r22050", "-twav", ss.str().c_str(),
+            recording_file_name_.c_str(), (char*) 0);
+      break;
+    }
+
+    default:
+      break;
+  }
+
+  startTimer(recording_time);
+}
+
+void MicTest::recordSound()
+{
+  recordSound(5);
+}
+
+void MicTest::playTestSound(const std::string &path)
+{
+  ROS_INFO("Start to play recording sound");
+
+  playSound(default_mp3_path_ + "Start playing.mp3");
+
+  usleep(1.3 * 1000 * 1000);
+
+  if (play_pid_ != -1)
+    kill(play_pid_, SIGKILL);
+
+  play_pid_ = fork();
+
+  switch (play_pid_)
+  {
+    case -1:
+      fprintf(stderr, "Fork Failed!! \n");
+      ROS_WARN("Fork Failed!! \n");
+      break;
+
+    case 0:
+      execl("/usr/bin/aplay", "aplay", path.c_str(), (char*) 0);
+      break;
+
+    default:
+      break;
+  }
+
+  startTimer(5);
+}
+
+void MicTest::playSound(const std::string &path)
+{
+  std_msgs::String sound_msg;
+  sound_msg.data = path;
+
+  play_sound_pub_.publish(sound_msg);
+}
+
+void MicTest::deleteSoundFile(const std::string &file_path)
+{
+  remove(file_path.c_str());
+  ROS_INFO("Delete temporary file");
+}
+
+void MicTest::startTimer(double wait_time)
+{
+  start_time_ = ros::Time::now();
+  wait_time_ = wait_time;
+  //is_wait_ = true;
+}
+
+void MicTest::finishTimer()
+{
+  //is_wait_ = false;
+  wait_time_ = -1;
+}
+
+} /* namespace robotis_op */

op3_demo/src/test_node.cpp

@@ -0,0 +1,380 @@
+/*******************************************************************************
+ * 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 <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/String.h>
+
+#include "op3_demo/soccer_demo.h"
+#include "op3_demo/action_demo.h"
+#include "op3_demo/vision_demo.h"
+#include "op3_demo/button_test.h"
+#include "op3_demo/mic_test.h"
+#include "robotis_math/robotis_linear_algebra.h"
+#include "robotis_controller_msgs/SyncWriteItem.h"
+
+enum Demo_Status
+{
+  Ready = 0,
+  ButtonTest = 1,
+  MicTest = 2,
+  SoccerDemo = 3,
+  VisionDemo = 4,
+  ActionDemo = 5,
+  DemoCount = 6,
+};
+
+void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg);
+void goInitPose();
+void playSound(const std::string &path);
+void setLED(int led);
+bool checkManagerRunning(std::string& manager_name);
+
+const int SPIN_RATE = 30;
+
+ros::Publisher init_pose_pub;
+ros::Publisher play_sound_pub;
+ros::Publisher led_pub;
+
+std::string default_mp3_path = "";
+int current_status = Ready;
+int desired_status = Ready;
+bool apply_desired = false;
+
+//node main
+int main(int argc, char **argv)
+{
+  //init ros
+  ros::init(argc, argv, "self_test_node");
+
+  //create ros wrapper object
+  robotis_op::OPDemo *current_demo = NULL;
+  robotis_op::SoccerDemo *soccer_demo = new robotis_op::SoccerDemo();
+  robotis_op::ActionDemo *action_demo = new robotis_op::ActionDemo();
+  robotis_op::VisionDemo *vision_demo = new robotis_op::VisionDemo();
+  robotis_op::ButtonTest *button_test = new robotis_op::ButtonTest();
+  robotis_op::MicTest *mic_test = new robotis_op::MicTest();
+
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  init_pose_pub = nh.advertise<std_msgs::String>("/robotis/base/ini_pose", 0);
+  play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
+  led_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
+  ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback);
+
+  default_mp3_path = ros::package::getPath("op3_demo") + "/Data/mp3/";
+
+  ros::start();
+
+  //set node loop rate
+  ros::Rate loop_rate(SPIN_RATE);
+
+  // wait for starting of manager
+  std::string manager_name = "/op3_manager";
+  while (ros::ok())
+  {
+    ros::Duration(1.0).sleep();
+
+    if (checkManagerRunning(manager_name) == true)
+    {
+      break;
+      ROS_INFO("Succeed to connect");
+    }
+    ROS_WARN("Waiting for op3 manager");
+  }
+
+  // init procedure
+  playSound(default_mp3_path + "test/Self test ready mode.mp3");
+  setLED(0x01 | 0x02 | 0x04);
+
+  //node loop
+  while (ros::ok())
+  {
+    // process
+    if (apply_desired == true)
+    {
+      switch (desired_status)
+      {
+        case Ready:
+        {
+
+          if (current_demo != NULL)
+            current_demo->setDemoDisable();
+
+          current_demo = NULL;
+
+          goInitPose();
+
+          ROS_INFO("[Go to Demo READY!]");
+          break;
+        }
+
+        case SoccerDemo:
+        {
+          if (current_demo != NULL)
+            current_demo->setDemoDisable();
+
+          current_demo = soccer_demo;
+          current_demo->setDemoEnable();
+
+          ROS_INFO("[Start] Soccer Demo");
+          break;
+        }
+
+        case VisionDemo:
+        {
+          if (current_demo != NULL)
+            current_demo->setDemoDisable();
+
+          current_demo = vision_demo;
+          current_demo->setDemoEnable();
+          ROS_INFO("[Start] Vision Demo");
+          break;
+        }
+        case ActionDemo:
+        {
+          if (current_demo != NULL)
+            current_demo->setDemoDisable();
+
+          current_demo = action_demo;
+          current_demo->setDemoEnable();
+          ROS_INFO("[Start] Action Demo");
+          break;
+        }
+        case ButtonTest:
+        {
+          if (current_demo != NULL)
+            current_demo->setDemoDisable();
+
+          current_demo = button_test;
+          current_demo->setDemoEnable();
+          ROS_INFO("[Start] Button Test");
+          break;
+        }
+        case MicTest:
+        {
+          if (current_demo != NULL)
+            current_demo->setDemoDisable();
+
+          current_demo = mic_test;
+          current_demo->setDemoEnable();
+          ROS_INFO("[Start] Mic Test");
+          break;
+        }
+
+
+        default:
+        {
+          break;
+        }
+      }
+
+      apply_desired = false;
+      current_status = desired_status;
+    }
+    else
+    {
+      if (current_status != desired_status)
+      {
+//        // sound out desired status
+//        switch (desired_status)
+//        {
+//          case SoccerDemo:
+//            playSound(default_path + "Autonomous soccer mode.mp3");
+//            break;
+//
+//          case VisionDemo:
+//            playSound(default_path + "Vision processing mode.mp3");
+//            break;
+//
+//          case ActionDemo:
+//            playSound(default_path + "Interactive motion mode.mp3");
+//            break;
+//
+//          default:
+//            break;
+//        }
+      }
+    }
+
+    //execute pending callbacks
+    ros::spinOnce();
+
+    //relax to fit output rate
+    loop_rate.sleep();
+  }
+
+  //exit program
+  return 0;
+}
+
+void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
+{
+  // in the middle of playing demo
+  if (current_status != Ready)
+  {
+    if (msg->data == "mode_long")
+    {
+      // go to mode selection status
+      desired_status = Ready;
+      apply_desired = true;
+
+      playSound(default_mp3_path + "test/Self test ready mode.mp3");
+      setLED(0x01 | 0x02 | 0x04);
+    }
+    else if (msg->data == "user_long")
+    {
+      // it's using in op3_manager
+      // torque on and going to init pose
+    }
+  }
+  // ready to start demo
+  else
+  {
+    if (msg->data == "start")
+    {
+      // select current demo
+      desired_status = (desired_status == Ready) ? desired_status + 1 : desired_status;
+      apply_desired = true;
+
+      // sound out desired status
+      switch (desired_status)
+      {
+        case SoccerDemo:
+          playSound(default_mp3_path + "Start soccer demonstration.mp3");
+          break;
+
+        case VisionDemo:
+          playSound(default_mp3_path + "Start vision processing demonstration.mp3");
+          break;
+
+        case ActionDemo:
+          playSound(default_mp3_path + "Start motion demonstration.mp3");
+          break;
+
+        case ButtonTest:
+          playSound(default_mp3_path + "test/Start button test mode.mp3");
+          break;
+
+        case MicTest:
+          playSound(default_mp3_path + "test/Start mic test mode.mp3");
+          break;
+
+        default:
+          break;
+      }
+
+      ROS_INFO("= Start Demo Mode : %d", desired_status);
+    }
+    else if (msg->data == "mode")
+    {
+      // change to next demo
+      desired_status = (desired_status + 1) % DemoCount;
+      desired_status = (desired_status == Ready) ? desired_status + 1 : desired_status;
+
+      // sound out desired status and changign LED
+      switch (desired_status)
+      {
+        case SoccerDemo:
+          playSound(default_mp3_path + "Autonomous soccer mode.mp3");
+          setLED(0x01);
+          break;
+
+        case VisionDemo:
+          playSound(default_mp3_path + "Vision processing mode.mp3");
+          setLED(0x02);
+          break;
+
+        case ActionDemo:
+          playSound(default_mp3_path + "Interactive motion mode.mp3");
+          setLED(0x04);
+          break;
+
+        case ButtonTest:
+          playSound(default_mp3_path + "test/Button test mode.mp3");
+          setLED(0x01 | 0x02);
+          break;
+
+        case MicTest:
+          playSound(default_mp3_path + "test/Mic test mode.mp3");
+          setLED(0x01 | 0x04);
+          break;
+
+        default:
+          break;
+      }
+
+      ROS_INFO("= Demo Mode : %d", desired_status);
+    }
+  }
+}
+
+void goInitPose()
+{
+  std_msgs::String init_msg;
+  init_msg.data = "ini_pose";
+
+  init_pose_pub.publish(init_msg);
+}
+
+void playSound(const std::string &path)
+{
+  std_msgs::String sound_msg;
+  sound_msg.data = path;
+
+  play_sound_pub.publish(sound_msg);
+}
+
+void setLED(int led)
+{
+  robotis_controller_msgs::SyncWriteItem syncwrite_msg;
+  syncwrite_msg.item_name = "LED";
+  syncwrite_msg.joint_name.push_back("open-cr");
+  syncwrite_msg.value.push_back(led);
+
+  led_pub.publish(syncwrite_msg);
+}
+
+bool checkManagerRunning(std::string& manager_name)
+{
+  std::vector<std::string> node_list;
+  ros::master::getNodes(node_list);
+
+  for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); node_list_idx++)
+  {
+    if (node_list[node_list_idx] == manager_name)
+      return true;
+  }
+
+  ROS_ERROR("Can't find op3_manager");
+  return false;
+}

op3_demo/src/vision/face_tracker.cpp

@@ -0,0 +1,208 @@
+/*******************************************************************************
+ * 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 "op3_demo/face_tracker.h"
+
+namespace robotis_op
+{
+
+FaceTracker::FaceTracker()
+    : nh_(ros::this_node::getName()),
+      FOV_WIDTH(26.4 * M_PI / 180),
+      FOV_HEIGHT(21.6 * M_PI / 180),
+      NOT_FOUND_THRESHOLD(50),
+      use_head_scan_(false),
+      count_not_found_(0),
+      on_tracking_(false)
+{
+  head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
+  head_scan_pub_ = nh_.advertise<std_msgs::String>("/robotis/head_control/scan_command", 0);
+
+  face_position_sub_ = nh_.subscribe("/face_position", 1, &FaceTracker::facePositionCallback, this);
+  face_tracking_command_sub_ = nh_.subscribe("/face_tracker/command", 1, &FaceTracker::faceTrackerCommandCallback,
+                                             this);
+}
+
+FaceTracker::~FaceTracker()
+{
+
+}
+
+void FaceTracker::facePositionCallback(const geometry_msgs::Point::ConstPtr &msg)
+{
+  if (msg->z < 0)
+    return;
+
+  face_position_ = *msg;
+}
+
+void FaceTracker::faceTrackerCommandCallback(const std_msgs::String::ConstPtr &msg)
+{
+  if (msg->data == "start")
+  {
+    startTracking();
+  }
+  else if (msg->data == "stop")
+  {
+    stopTracking();
+  }
+  else if (msg->data == "toggle_start")
+  {
+    if (on_tracking_ == false)
+      startTracking();
+    else
+      stopTracking();
+  }
+}
+
+void FaceTracker::startTracking()
+{
+  on_tracking_ = true;
+  ROS_INFO("Start Face tracking");
+}
+
+void FaceTracker::stopTracking()
+{
+  on_tracking_ = false;
+  ROS_INFO("Stop Face tracking");
+}
+
+void FaceTracker::setUsingHeadScan(bool use_scan)
+{
+  use_head_scan_ = use_scan;
+}
+
+void FaceTracker::setFacePosition(geometry_msgs::Point &face_position)
+{
+  if (face_position.z > 0)
+  {
+    face_position_ = face_position;
+  }
+}
+
+int FaceTracker::processTracking()
+{
+  if (on_tracking_ == false)
+  {
+    face_position_.z = 0;
+    count_not_found_ = 0;
+    //return false;
+    return Waiting;
+  }
+
+  // check ball position
+  if (face_position_.z <= 0)
+  {
+    count_not_found_++;
+
+    if (count_not_found_ == NOT_FOUND_THRESHOLD)
+    {
+      scanFace();
+      //count_not_found_ = 0;
+      return NotFound;
+    }
+    else if (count_not_found_ > NOT_FOUND_THRESHOLD)
+    {
+      return NotFound;
+    }
+    else
+    {
+      return Waiting;
+    }
+
+    //return false;
+  }
+
+  // if face is detected
+  double x_error = -atan(face_position_.x * tan(FOV_WIDTH));
+  double y_error = -atan(face_position_.y * tan(FOV_HEIGHT));
+
+  face_position_.z = 0;
+  count_not_found_ = 0;
+
+  double p_gain = 0.7, d_gain = 0.45;
+  double x_error_diff = x_error - current_face_pan_;
+  double y_error_diff = y_error - current_face_tilt_;
+  double x_error_target = x_error * p_gain + x_error_diff * d_gain;
+  double y_error_target = y_error * p_gain + y_error_diff * d_gain;
+
+  // move head joint
+  publishHeadJoint(x_error_target, y_error_target);
+
+  current_face_pan_ = x_error;
+  current_face_tilt_ = y_error;
+
+  // return true;
+  return Found;
+}
+
+void FaceTracker::publishHeadJoint(double pan, double tilt)
+{
+  double min_angle = 1 * M_PI / 180;
+  if (fabs(pan) < min_angle && fabs(tilt) < min_angle)
+  {
+    dismissed_count_ += 1;
+    return;
+  }
+  std::cout << "Target angle[" << dismissed_count_ << "] : " << pan << " | " << tilt << std::endl;
+
+  dismissed_count_ = 0;
+
+  sensor_msgs::JointState head_angle_msg;
+
+  head_angle_msg.name.push_back("head_pan");
+  head_angle_msg.name.push_back("head_tilt");
+
+  head_angle_msg.position.push_back(pan);
+  head_angle_msg.position.push_back(tilt);
+
+  head_joint_pub_.publish(head_angle_msg);
+}
+
+void FaceTracker::scanFace()
+{
+  if (use_head_scan_ == false)
+    return;
+
+  // check head control module enabled
+  // ...
+
+  // send message to head control module
+  std_msgs::String scan_msg;
+  scan_msg.data = "scan";
+
+  head_scan_pub_.publish(scan_msg);
+  // ROS_INFO("Scan the ball");
+}
+
+}
+

op3_demo/src/vision/vision_demo.cpp

@@ -0,0 +1,247 @@
+/*******************************************************************************
+ * 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 "op3_demo/vision_demo.h"
+
+namespace robotis_op
+{
+
+VisionDemo::VisionDemo()
+    : SPIN_RATE(30),
+      is_tracking_(false),
+      tracking_status_(FaceTracker::Waiting)
+{
+  enable_ = false;
+
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  boost::thread queue_thread = boost::thread(boost::bind(&VisionDemo::callbackThread, this));
+  boost::thread process_thread = boost::thread(boost::bind(&VisionDemo::processThread, this));
+}
+
+VisionDemo::~VisionDemo()
+{
+  // TODO Auto-generated destructor stub
+}
+
+void VisionDemo::setDemoEnable()
+{
+  // change to motion module
+  setModuleToDemo("action_module");
+
+  usleep(100 * 1000);
+
+  playMotion(InitPose);
+
+  usleep(1500 * 1000);
+
+  setModuleToDemo("head_control_module");
+
+  usleep(10 * 1000);
+
+  enable_ = true;
+  face_tracker_.startTracking();
+
+  ROS_INFO("Start Vision Demo");
+
+}
+
+void VisionDemo::setDemoDisable()
+{
+
+  face_tracker_.stopTracking();
+  is_tracking_ = false;
+  tracking_status_ = FaceTracker::Waiting;
+  enable_ = false;
+}
+
+void VisionDemo::process()
+{
+  //bool is_tracked = face_tracker_.processTracking();
+  int tracking_status = face_tracker_.processTracking();
+
+  //if(is_tracking_ != is_tracked)
+  if(tracking_status_ != tracking_status)
+  {
+//    if(is_tracked == true)
+//      setRGBLED(0x1F, 0, 0);
+//    else
+//      setRGBLED(0x1F, 0x1F, 0);
+    switch(tracking_status)
+    {
+      case FaceTracker::Found:
+        setRGBLED(0x1F, 0x1F, 0x1F);
+        break;
+
+      case FaceTracker::NotFound:
+        setRGBLED(0, 0, 0);
+        break;
+
+      default:
+        break;
+    }
+  }
+
+  if(tracking_status != FaceTracker::Waiting)
+    tracking_status_ = tracking_status;
+
+  //is_tracking_ = is_tracked;
+  std::cout << "Tracking : " << tracking_status << std::endl;
+}
+
+void VisionDemo::processThread()
+{
+  //set node loop rate
+  ros::Rate loop_rate(SPIN_RATE);
+
+  //node loop
+  while (ros::ok())
+  {
+    if (enable_ == true)
+      process();
+
+    //relax to fit output rate
+    loop_rate.sleep();
+  }
+}
+
+void VisionDemo::callbackThread()
+{
+  ros::NodeHandle nh(ros::this_node::getName());
+
+  // subscriber & publisher
+  module_control_pub_ = nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 0);
+  motion_index_pub_ = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
+  rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
+
+  buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &VisionDemo::buttonHandlerCallback, this);
+  faceCoord_sub_ = nh.subscribe("/faceCoord", 1, &VisionDemo::facePositionCallback, this);
+
+  while (nh.ok())
+  {
+    ros::spinOnce();
+
+    usleep(1 * 1000);
+  }
+}
+
+void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg)
+{
+  if (enable_ == false)
+    return;
+
+  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")
+  {
+
+  }
+}
+
+void VisionDemo::setModuleToDemo(const std::string &module_name)
+{
+  std_msgs::String control_msg;
+  control_msg.data = module_name;
+
+  module_control_pub_.publish(control_msg);
+  std::cout << "enable module : " << module_name << std::endl;
+}
+
+void VisionDemo::facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg)
+{
+  if (enable_ == false)
+    return;
+
+  // face is detected
+  if (msg->data.size() >= 10)
+  {
+    // center of face
+    face_position_.x = (msg->data[6] + msg->data[8] * 0.5) / msg->data[2] * 2 - 1;
+    face_position_.y = (msg->data[7] + msg->data[9] * 0.5) / msg->data[3] * 2 - 1;
+    face_position_.z = msg->data[8] * 0.5 + msg->data[9] * 0.5;
+
+    face_tracker_.setFacePosition(face_position_);
+  }
+  else
+  {
+    face_position_.x = 0;
+    face_position_.y = 0;
+    face_position_.z = 0;
+    return;
+  }
+}
+
+void VisionDemo::playMotion(int motion_index)
+{
+  std_msgs::Int32 motion_msg;
+  motion_msg.data = motion_index;
+
+  motion_index_pub_.publish(motion_msg);
+}
+
+void VisionDemo::setRGBLED(int blue, int green, int red)
+{
+  int led_full_unit = 0x1F;
+  int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | (red & led_full_unit);
+  robotis_controller_msgs::SyncWriteItem syncwrite_msg;
+  syncwrite_msg.item_name = "LED_RGB";
+  syncwrite_msg.joint_name.push_back("open-cr");
+  syncwrite_msg.value.push_back(led_value);
+
+  rgb_led_pub_.publish(syncwrite_msg);
+}
+
+} /* namespace robotis_op */

robotis_op3_demo/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robotis_op3_demo)
+find_package(catkin REQUIRED)
+catkin_metapackage()

robotis_op3_demo/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package>
+  <name>robotis_op3_demo</name>
+  <version>0.1.0</version>
+  <description>
+    ROS packages for the robotis_op3_demo (meta package)
+  </description>
+
+  <license>BSD</license>
+  <author email="kmjung@robotis.com">Kayman</author>
+  <maintainer email="pyo@robotis.com">Pyo</maintainer>
+
+  <!-- <url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues</url> -->
+  <!-- <url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo</url> -->
+  <!-- <url type="website">http://wiki.ros.org/robotis_op3_demo</url> -->
+  
+  <buildtool_depend>catkin</buildtool_depend>    
+  <run_depend>ball_detector</run_depend>
+  <run_depend>op3_demo</run_depend>
+
+  <export>
+    <metapackage/>
+  </export>
+</package>