Repository split from ROBOTIS-OP3

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 */