From c9f9254e527fb477ef3a483e0758caf66dee058a Mon Sep 17 00:00:00 2001
From: Kayman <kmjung@robotis.com>
Date: Mon, 15 Oct 2018 17:54:23 +0900
Subject: [PATCH] changed a method that judge the ball position.

---
 op3_demo/include/op3_demo/ball_follower.h |  4 +++-
 op3_demo/launch/demo.launch               |  2 +-
 op3_demo/src/soccer/ball_follower.cpp     | 18 +++++++++++++-----
 op3_demo/src/soccer/soccer_demo.cpp       |  2 ++
 4 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/op3_demo/include/op3_demo/ball_follower.h b/op3_demo/include/op3_demo/ball_follower.h
index fbcc710..4e2c05f 100644
--- a/op3_demo/include/op3_demo/ball_follower.h
+++ b/op3_demo/include/op3_demo/ball_follower.h
@@ -20,6 +20,7 @@
 #define BALL_FOLLOWER_H_
 
 #include <math.h>
+#include <numeric>
 #include <ros/ros.h>
 #include <ros/package.h>
 #include <std_msgs/String.h>
@@ -104,7 +105,7 @@ class BallFollower
   ros::Publisher head_scan_pub_;
   ros::Publisher set_walking_command_pub_;
   ros::Publisher set_walking_param_pub_;
-  ;
+
   ros::Publisher motion_index_pub_;
   ros::ServiceClient get_walking_param_client_;
 
@@ -120,6 +121,7 @@ class BallFollower
   int count_not_found_;
   int count_to_kick_;
   int accum_ball_position_;
+  std::vector<int> ball_position_queue_;
   bool on_tracking_;
   int approach_ball_position_;
   double current_pan_, current_tilt_;
diff --git a/op3_demo/launch/demo.launch b/op3_demo/launch/demo.launch
index d6d26b4..0bd83a9 100644
--- a/op3_demo/launch/demo.launch
+++ b/op3_demo/launch/demo.launch
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<launch>    
+<launch>
     <!-- robotis op3 manager -->
     <include file="$(find op3_manager)/launch/op3_manager.launch"/>
 
diff --git a/op3_demo/src/soccer/ball_follower.cpp b/op3_demo/src/soccer/ball_follower.cpp
index fa89672..0b0826a 100644
--- a/op3_demo/src/soccer/ball_follower.cpp
+++ b/op3_demo/src/soccer/ball_follower.cpp
@@ -60,7 +60,6 @@ BallFollower::BallFollower()
       "/robotis/walking/get_params");
 
   prev_time_ = ros::Time::now();
-
 }
 
 BallFollower::~BallFollower()
@@ -218,12 +217,21 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
 
     ROS_INFO("In range [%d | %d]", count_to_kick_, accum_ball_position_);
 
+    // ball queue
+    if(ball_position_queue_.size() >= 5)
+      ball_position_queue_.erase(ball_position_queue_.begin());
+
+    ball_position_queue_.push_back((ball_x_angle > 0) ? 1 : -1);
+
+
     if (count_to_kick_ > 20)
     {
       setWalkingCommand("stop");
       on_tracking_ = false;
 
       // check direction of the ball
+       accum_ball_position_ = std::accumulate(ball_position_queue_.begin(), ball_position_queue_.end(), 0);
+
       if (accum_ball_position_ > 0)
       {
         ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : left");  // left
@@ -239,10 +247,10 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_
     }
     else if (count_to_kick_ > 15)
     {
-      if (ball_x_angle > 0)
-        accum_ball_position_ += 1;
-      else
-        accum_ball_position_ -= 1;
+//      if (ball_x_angle > 0)
+//        accum_ball_position_ += 1;
+//      else
+//        accum_ball_position_ -= 1;
 
       // send message
       setWalkingParam(IN_PLACE_FB_STEP, 0, 0);
diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp
index 42ed395..638cfdd 100644
--- a/op3_demo/src/soccer/soccer_demo.cpp
+++ b/op3_demo/src/soccer/soccer_demo.cpp
@@ -553,11 +553,13 @@ void SoccerDemo::handleKick()
   {
   case robotis_op::BallFollower::OnRight:
     std::cout << "Kick Motion [R]: " << ball_position << std::endl;
+    sendDebugTopic("Kick the ball using Right foot");
     playMotion(is_grass_ ? RightKick + ForGrass : RightKick);
     break;
 
   case robotis_op::BallFollower::OnLeft:
     std::cout << "Kick Motion [L]: " << ball_position << std::endl;
+    sendDebugTopic("Kick the ball using Left foot");
     playMotion(is_grass_ ? LeftKick + ForGrass : LeftKick);
     break;