Merge branch 'develop' of https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo into develop

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>
@@ -55,6 +56,10 @@ class BallFollower
   void waitFollowing();
   void startFollowing();
   void stopFollowing();
+  void clearBallPosition()
+  {
+    approach_ball_position_ = NotFound;
+  }
 
   int getBallPosition()
   {
@@ -100,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_;
 
@@ -116,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/include/op3_demo/soccer_demo.h

@@ -98,6 +98,8 @@ class SoccerDemo : public OPDemo
   void setRGBLED(int blue, int green, int red);
   bool isActionRunning();
 
+  void sendDebugTopic(const std::string &msgs);
+
   BallTracker ball_tracker_;
   BallFollower ball_follower_;
 
@@ -108,6 +110,8 @@ class SoccerDemo : public OPDemo
   ros::Subscriber demo_command_sub_;
   ros::Subscriber imu_data_sub_;
 
+  ros::Publisher test_pub_;
+
   ros::ServiceClient is_running_client_;
   ros::ServiceClient set_joint_module_client_;
 

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

@@ -143,7 +143,7 @@ void SoccerDemo::process()
       }
 
       // check states for kick
-      int ball_position = ball_follower_.getBallPosition();
+//      int ball_position = ball_follower_.getBallPosition();
       bool in_range = ball_follower_.isBallInRange();
 
       if(in_range == true)
@@ -204,6 +204,8 @@ void SoccerDemo::callbackThread()
   is_running_client_ = nh.serviceClient<op3_action_module_msgs::IsRunning>("/robotis/action/is_running");
   set_joint_module_client_ = nh.serviceClient<robotis_controller_msgs::SetJointModule>("/robotis/set_present_joint_ctrl_modules");
 
+  test_pub_ = nh.advertise<std_msgs::String>("/debug_text", 0);
+
   while (nh.ok())
   {
     ros::spinOnce();
@@ -514,6 +516,8 @@ void SoccerDemo::handleKick(int ball_position)
 
   on_following_ball_ = false;
   restart_soccer_ = true;
+  tracking_status_ = BallTracker::NotFound;
+  ball_follower_.clearBallPosition();
 
   usleep(2000 * 1000);
 
@@ -540,6 +544,8 @@ void SoccerDemo::handleKick()
   {
     on_following_ball_ = false;
     restart_soccer_ = true;
+    tracking_status_ = BallTracker::NotFound;
+    ball_follower_.clearBallPosition();
     return;
   }
 
@@ -547,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;
 
@@ -561,6 +569,8 @@ void SoccerDemo::handleKick()
 
   on_following_ball_ = false;
   restart_soccer_ = true;
+  tracking_status_ = BallTracker::NotFound;
+  ball_follower_.clearBallPosition();
 
   usleep(2000 * 1000);
 
@@ -653,4 +663,12 @@ bool SoccerDemo::isActionRunning()
   return false;
 }
 
+void SoccerDemo::sendDebugTopic(const std::string &msgs)
+{
+  std_msgs::String debug_msg;
+  debug_msg.data = msgs;
+
+  test_pub_.publish(debug_msg);
+}
+
 }