From 58a62c0721c4d1bd7404475a5423679d1fc4520f Mon Sep 17 00:00:00 2001
From: Kayman <kmjung@robotis.com>
Date: Fri, 7 Sep 2018 10:42:05 +0900
Subject: [PATCH] added a process thread for ball tracking

---
 op3_demo/include/op3_demo/soccer_demo.h |  4 +-
 op3_demo/src/soccer/soccer_demo.cpp     | 52 +++++++++++++++++++++----
 2 files changed, 47 insertions(+), 9 deletions(-)

diff --git a/op3_demo/include/op3_demo/soccer_demo.h b/op3_demo/include/op3_demo/soccer_demo.h
index a771eda..f5f6df4 100644
--- a/op3_demo/include/op3_demo/soccer_demo.h
+++ b/op3_demo/include/op3_demo/soccer_demo.h
@@ -72,6 +72,7 @@ class SoccerDemo : public OPDemo
 
   void processThread();
   void callbackThread();
+  void trackingThread();
 
   void setBodyModuleToDemo(const std::string &body_module, bool with_head_control = true);
   void setModuleToDemo(const std::string &module_name);
@@ -114,7 +115,8 @@ class SoccerDemo : public OPDemo
 
   bool is_grass_;
   int wait_count_;
-    bool on_following_ball_;
+  bool on_following_ball_;
+  bool on_tracking_ball_;
   bool restart_soccer_;
   bool start_following_;
   bool stop_following_;
diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp
index 0565152..fd50c70 100644
--- a/op3_demo/src/soccer/soccer_demo.cpp
+++ b/op3_demo/src/soccer/soccer_demo.cpp
@@ -28,6 +28,7 @@ SoccerDemo::SoccerDemo()
     DEBUG_PRINT(false),
     wait_count_(0),
     on_following_ball_(false),
+    on_tracking_ball_(false),
     restart_soccer_(false),
     start_following_(false),
     stop_following_(false),
@@ -48,6 +49,7 @@ SoccerDemo::SoccerDemo()
 
   boost::thread queue_thread = boost::thread(boost::bind(&SoccerDemo::callbackThread, this));
   boost::thread process_thread = boost::thread(boost::bind(&SoccerDemo::processThread, this));
+  boost::thread tracking_thread = boost::thread(boost::bind(&SoccerDemo::trackingThread, this));
 
   is_grass_ = nh.param<bool>("grass_demo", false);
 }
@@ -73,6 +75,7 @@ void SoccerDemo::setDemoDisable()
   enable_ = false;
   wait_count_ = 0;
   on_following_ball_ = false;
+  on_tracking_ball_ = false;
   restart_soccer_ = false;
   start_following_ = false;
   stop_following_ = false;
@@ -87,9 +90,9 @@ void SoccerDemo::process()
     return;
 
   // ball tracking
-  int tracking_status;
+  int tracking_status = tracking_status_;
 
-  tracking_status = ball_tracker_.processTracking();
+  //tracking_status = ball_tracker_.processTracking();
 
   // check to start
   if (start_following_ == true)
@@ -120,22 +123,22 @@ void SoccerDemo::process()
       {
       case BallTracker::Found:
         ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), 0.0);
-        if(tracking_status_ != tracking_status)
-          setRGBLED(0x1F, 0x1F, 0x1F);
+//        if(tracking_status_ != tracking_status)
+//          setRGBLED(0x1F, 0x1F, 0x1F);
         break;
 
       case BallTracker::NotFound:
         ball_follower_.waitFollowing();
-        if(tracking_status_ != tracking_status)
-          setRGBLED(0, 0, 0);
+//        if(tracking_status_ != tracking_status)
+//          setRGBLED(0, 0, 0);
         break;
 
       default:
         break;
       }
 
-      if(tracking_status != tracking_status_)
-        tracking_status_ = tracking_status;
+//      if(tracking_status != tracking_status_)
+//        tracking_status_ = tracking_status;
     }
 
     // check fallen states
@@ -219,6 +222,37 @@ void SoccerDemo::callbackThread()
   }
 }
 
+void SoccerDemo::trackingThread()
+{
+  if(enable_ == false || on_tracking_ball_ == false)
+    return;
+
+  // ball tracking
+  int tracking_status;
+
+  tracking_status = ball_tracker_.processTracking();
+
+  // set led
+  switch(tracking_status)
+  {
+  case BallTracker::Found:
+    if(tracking_status_ != tracking_status)
+      setRGBLED(0x1F, 0x1F, 0x1F);
+    break;
+
+  case BallTracker::NotFound:
+    if(tracking_status_ != tracking_status)
+      setRGBLED(0, 0, 0);
+    break;
+
+  default:
+    break;
+  }
+
+  if(tracking_status != tracking_status_)
+    tracking_status_ = tracking_status;
+}
+
 void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_head_control)
 {
   robotis_controller_msgs::JointCtrlModule control_msg;
@@ -436,6 +470,7 @@ void SoccerDemo::startSoccerMode()
 
   ROS_INFO("Start Soccer Demo");
   on_following_ball_ = true;
+  on_tracking_ball_ = true;
   start_following_ = true;
 }
 
@@ -443,6 +478,7 @@ void SoccerDemo::stopSoccerMode()
 {
   ROS_INFO("Stop Soccer Demo");
   on_following_ball_ = false;
+  on_tracking_ball_ = false;
   stop_following_ = true;
 }