changed a method that judge the ball position.

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

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

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

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;