Merge branch 'feature_improvement_soccer' into develop

op3_demo/include/op3_demo/ball_follower.h

@@ -43,8 +43,9 @@ class BallFollower
   enum
   {
     NotFound = 0,
-    OnRight = 1,
-    OnLeft = 2,
+    OutOfRange = 1,
+    OnRight = 2,
+    OnLeft = 3,
   };
 
   BallFollower();

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

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

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

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;
@@ -86,11 +89,6 @@ void SoccerDemo::process()
   if(enable_ == false)
     return;
 
-  // ball tracking
-  int tracking_status;
-
-  tracking_status = ball_tracker_.processTracking();
-
   // check to start
   if (start_following_ == true)
   {
@@ -116,26 +114,19 @@ void SoccerDemo::process()
     // ball following
     if (on_following_ball_ == true)
     {
-      switch(tracking_status)
+      switch(tracking_status_)
       {
       case BallTracker::Found:
         ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), 0.0);
-        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);
         break;
 
       default:
         break;
       }
-
-      if(tracking_status != tracking_status_)
-        tracking_status_ = tracking_status;
     }
 
     // check fallen states
@@ -219,6 +210,50 @@ void SoccerDemo::callbackThread()
   }
 }
 
+void SoccerDemo::trackingThread()
+{
+
+  //set node loop rate
+  ros::Rate loop_rate(SPIN_RATE);
+
+  ball_tracker_.startTracking();
+
+  //node loop
+  while (ros::ok())
+  {
+
+    if(enable_ == true && on_tracking_ball_ == true)
+    {
+      // 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;
+    }
+    //relax to fit output rate
+    loop_rate.sleep();
+  }
+}
+
 void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_head_control)
 {
   robotis_controller_msgs::JointCtrlModule control_msg;
@@ -436,6 +471,7 @@ void SoccerDemo::startSoccerMode()
 
   ROS_INFO("Start Soccer Demo");
   on_following_ball_ = true;
+  on_tracking_ball_ = true;
   start_following_ = true;
 }
 
@@ -443,17 +479,18 @@ void SoccerDemo::stopSoccerMode()
 {
   ROS_INFO("Stop Soccer Demo");
   on_following_ball_ = false;
+  on_tracking_ball_ = false;
   stop_following_ = true;
 }
 
 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;