From ebaf7f9e2193a56a31b58c1593b19e847f061116 Mon Sep 17 00:00:00 2001
From: Kayman <kmjung@robotis.com>
Date: Fri, 7 Sep 2018 17:36:15 +0900
Subject: [PATCH] added that a head goes init when OP3 stop tracking the ball.

---
 op3_demo/include/op3_demo/ball_tracker.h |  2 ++
 op3_demo/src/soccer/ball_tracker.cpp     | 18 +++++++++++++++++-
 op3_demo/src/soccer/soccer_demo.cpp      |  4 ++--
 3 files changed, 21 insertions(+), 3 deletions(-)

diff --git a/op3_demo/include/op3_demo/ball_tracker.h b/op3_demo/include/op3_demo/ball_tracker.h
index e5fe9ff..383c809 100644
--- a/op3_demo/include/op3_demo/ball_tracker.h
+++ b/op3_demo/include/op3_demo/ball_tracker.h
@@ -56,6 +56,7 @@ public:
   void stopTracking();
 
   void setUsingHeadScan(bool use_scan);
+  void goInit();
 
   double getPanOfBall()
   {
@@ -89,6 +90,7 @@ protected:
 
   //image publisher/subscriber
   ros::Publisher module_control_pub_;
+  ros::Publisher head_joint_offset_pub_;
   ros::Publisher head_joint_pub_;
   ros::Publisher head_scan_pub_;
 
diff --git a/op3_demo/src/soccer/ball_tracker.cpp b/op3_demo/src/soccer/ball_tracker.cpp
index e90bb54..a1706ad 100644
--- a/op3_demo/src/soccer/ball_tracker.cpp
+++ b/op3_demo/src/soccer/ball_tracker.cpp
@@ -45,7 +45,8 @@ BallTracker::BallTracker()
 
   ROS_INFO_STREAM("Ball tracking Gain : " << p_gain_ << ", " << i_gain_ << ", " << d_gain_);
 
-  head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
+  head_joint_offset_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
+  head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states", 0);
   head_scan_pub_ = nh_.advertise<std_msgs::String>("/robotis/head_control/scan_command", 0);
   //  error_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("/ball_tracker/errors", 0);
 
@@ -97,6 +98,8 @@ void BallTracker::startTracking()
 
 void BallTracker::stopTracking()
 {
+  goInit();
+
   on_tracking_ = false;
   ROS_INFO_COND(DEBUG_PRINT, "Stop Ball tracking");
 
@@ -246,6 +249,19 @@ void BallTracker::publishHeadJoint(double pan, double tilt)
   head_angle_msg.position.push_back(pan);
   head_angle_msg.position.push_back(tilt);
 
+  head_joint_offset_pub_.publish(head_angle_msg);
+}
+
+void BallTracker::goInit()
+{
+  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(0.0);
+  head_angle_msg.position.push_back(0.0);
+
   head_joint_pub_.publish(head_angle_msg);
 }
 
diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp
index 6f8d99e..cf5db3c 100644
--- a/op3_demo/src/soccer/soccer_demo.cpp
+++ b/op3_demo/src/soccer/soccer_demo.cpp
@@ -497,12 +497,12 @@ void SoccerDemo::stopSoccerMode()
 
 void SoccerDemo::handleKick(int ball_position)
 {
-  usleep(1000 * 1000);
+  usleep(1500 * 1000);
 
   // change to motion module
   setModuleToDemo("action_module");
 
-  usleep(1500 * 1000);
+  //usleep(1500 * 1000);
 
   if (handleFallen(stand_state_) == true || enable_ == false)
     return;