decreased the count of copying image.

added command to face_tracker_node.

ball_detector/include/ball_detector/ball_detector.h

@@ -83,13 +83,16 @@ class BallDetector
   void printConfig();
   void saveConfig();
   void setInputImage(const cv::Mat & inIm);
+  void setInputImage(const cv::Mat & inIm, cv::Mat &in_filter_img);
   void getOutputImage(cv::Mat & outIm);
   void filterImage();
+  void filterImage(const cv::Mat &in_filter_img, cv::Mat &out_filter_img);
   void makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, int range);
   void makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &mask_img);
   void inRangeHsv(const cv::Mat &input_img, const HsvFilter &filter_value, cv::Mat &output_img);
   void mophology(const cv::Mat &intput_img, cv::Mat &output_img, int ellipse_size);
   void houghDetection(const unsigned int imgEncoding);
+  void houghDetection2(const cv::Mat &input_hough);
   void drawOutputImage();
 
   //ros node handle

ball_detector/src/ball_detector.cpp

@@ -125,14 +125,25 @@ void BallDetector::process()
 
   if (cv_img_ptr_sub_ != NULL)
   {
-    //sets input image
-    setInputImage(cv_img_ptr_sub_->image);
+    cv::Mat img_hsv, img_filtered;
 
-    // test image filtering
-    filterImage();
+    // set input image
+    setInputImage(cv_img_ptr_sub_->image, img_hsv);
+
+    // image filtering
+    filterImage(img_hsv, img_filtered);
 
     //detect circles
-    houghDetection(this->img_encoding_);
+    houghDetection2(img_filtered);
+
+//    // set input image
+//    setInputImage(cv_img_ptr_sub_->image);
+
+//    // image filtering
+//    filterImage();
+
+//    //detect circles
+//    houghDetection(this->img_encoding_);
   }
 }
 
@@ -184,8 +195,8 @@ void BallDetector::publishCircles()
   // left(-1), right(+1)
   for (int idx = 0; idx < circles_.size(); idx++)
   {
-    circles_msg_.circles[idx].x = circles_[idx][0] / in_image_.cols * 2 - 1;    // x (-1 ~ 1)
-    circles_msg_.circles[idx].y = circles_[idx][1] / in_image_.rows * 2 - 1;    // y (-1 ~ 1)
+    circles_msg_.circles[idx].x = circles_[idx][0] / out_image_.cols * 2 - 1;    // x (-1 ~ 1)
+    circles_msg_.circles[idx].y = circles_[idx][1] / out_image_.rows * 2 - 1;    // y (-1 ~ 1)
     circles_msg_.circles[idx].z = circles_[idx][2];    // radius
   }
 
@@ -482,6 +493,14 @@ void BallDetector::setInputImage(const cv::Mat & inIm)
     out_image_ = in_image_.clone();
 }
 
+void BallDetector::setInputImage(const cv::Mat & inIm, cv::Mat &in_filter_img)
+{
+  cv::cvtColor(inIm, in_filter_img, cv::COLOR_RGB2HSV);
+
+  if (params_config_.debug == false)
+    out_image_ = inIm.clone();
+}
+
 void BallDetector::getOutputImage(cv::Mat & outIm)
 {
   this->drawOutputImage();
@@ -522,6 +541,42 @@ void BallDetector::filterImage()
   cv::cvtColor(img_filtered, in_image_, cv::COLOR_GRAY2RGB);
 }
 
+void BallDetector::filterImage(const cv::Mat &in_filter_img, cv::Mat &out_filter_img)
+{
+  if (!in_filter_img.data)
+    return;
+
+  inRangeHsv(in_filter_img, params_config_.filter_threshold, out_filter_img);
+
+  // mophology : open and close
+  mophology(out_filter_img, out_filter_img, params_config_.ellipse_size);
+
+  if (params_config_.use_second_filter == true)
+  {
+    // mask
+    cv::Mat img_mask;
+
+    // check hsv range
+    cv::Mat img_filtered2;
+    inRangeHsv(in_filter_img, params_config_.filter2_threshold, img_filtered2);
+
+    makeFilterMaskFromBall(out_filter_img, img_mask);
+    cv::bitwise_and(img_filtered2, img_mask, img_filtered2);
+
+    // or
+    cv::bitwise_or(out_filter_img, img_filtered2, out_filter_img);
+  }
+
+  mophology(out_filter_img, out_filter_img, params_config_.ellipse_size);
+
+//  cv::cvtColor(img_filtered, in_image_, cv::COLOR_GRAY2RGB);
+
+  //draws results to output Image
+  if (params_config_.debug == true)
+    cv::cvtColor(out_filter_img, out_image_, cv::COLOR_GRAY2RGB);
+//    out_image_ = in_image_.clone();
+}
+
 void BallDetector::makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, int range)
 {
   // source_img.
@@ -707,6 +762,68 @@ void BallDetector::houghDetection(const unsigned int imgEncoding)
   }
 }
 
+void BallDetector::houghDetection2(const cv::Mat &input_hough)
+{
+//  cv::Mat gray_image;
+  std::vector<cv::Vec3f> circles_current;
+  std::vector<cv::Vec3f> prev_circles = circles_;
+
+  //clear previous circles
+  circles_.clear();
+
+  // If input image is RGB, convert it to gray
+//  if (imgEncoding == IMG_RGB8)
+//    cv::cvtColor(input_hough, gray_image, CV_RGB2GRAY);
+
+
+  //Reduce the noise so we avoid false circle detection
+  cv::GaussianBlur(input_hough, input_hough,
+                   cv::Size(params_config_.gaussian_blur_size, params_config_.gaussian_blur_size),
+                   params_config_.gaussian_blur_sigma);
+
+  double hough_accum_th = params_config_.hough_accum_th;
+
+
+  //Apply the Hough Transform to find the circles
+  cv::HoughCircles(input_hough, circles_current, CV_HOUGH_GRADIENT, params_config_.hough_accum_resolution,
+                   params_config_.min_circle_dist, params_config_.canny_edge_th, hough_accum_th,
+                   params_config_.min_radius, params_config_.max_radius);
+
+  if (circles_current.size() == 0)
+    not_found_count_ += 1;
+  else
+    not_found_count_ = 0;
+
+  double alpha = 0.2;
+
+  for (int ix = 0; ix < circles_current.size(); ix++)
+  {
+    cv::Point2d center = cv::Point(circles_current[ix][0], circles_current[ix][1]);
+    double radius = circles_current[ix][2];
+
+    for (int prev_ix = 0; prev_ix < prev_circles.size(); prev_ix++)
+    {
+      cv::Point2d prev_center = cv::Point(prev_circles[prev_ix][0], prev_circles[prev_ix][1]);
+      double prev_radius = prev_circles[prev_ix][2];
+
+      cv::Point2d diff = center - prev_center;
+      double radius_th = std::max(radius, prev_radius) * 0.75;
+
+      if (sqrt(diff.dot(diff)) < radius_th)
+      {
+        if (abs(radius - prev_radius) < radius_th)
+        {
+          circles_current[ix] = circles_current[ix] * alpha + prev_circles[prev_ix] * (1 - alpha);
+        }
+
+        break;
+      }
+    }
+
+    circles_.push_back(circles_current[ix]);
+  }
+}
+
 void BallDetector::drawOutputImage()
 {
   cv::Point center_position;
@@ -714,8 +831,8 @@ void BallDetector::drawOutputImage()
   size_t ii;
 
   //draws results to output Image
-  if (params_config_.debug == true)
-    out_image_ = in_image_.clone();
+//  if (params_config_.debug == true)
+//    out_image_ = in_image_.clone();
 
   for (ii = 0; ii < circles_.size(); ii++)
   {

ball_detector/src/ball_detector_node.cpp

@@ -39,6 +39,7 @@ int main(int argc, char **argv)
       detector.process();
       detector.publishImage();
       detector.publishCircles();
+
     }
 
     //execute pending callbacks

op3_demo/include/op3_demo/face_tracker.h

@@ -24,6 +24,7 @@
 
 #include <ros/ros.h>
 #include <ros/package.h>
+#include <std_msgs/Bool.h>
 #include <std_msgs/String.h>
 #include <std_msgs/Int32.h>
 #include <sensor_msgs/JointState.h>

op3_demo/include/op3_demo/vision_demo.h

@@ -64,7 +64,8 @@ class VisionDemo : public OPDemo
 
   ros::Publisher module_control_pub_;
   ros::Publisher motion_index_pub_;
-  ros::Publisher rgb_led_pub_;
+  ros::Publisher rgb_led_pub_;  
+  ros::Publisher face_tracking_command_pub_;
 
   ros::Subscriber buttuon_sub_;
   ros::Subscriber faceCoord_sub_;

op3_demo/launch/face_detection_op3.launch

@@ -17,5 +17,6 @@
         <param name="displayed_Image" type="int" value="0" />
         <!-- <param name="publish" type="int" value="2" /> -->
         <param name="publish" type="int" value="3" />
+		<param name="start_condition" type="bool" value="false" />
   </node>
 </launch>

op3_demo/src/test_node.cpp

@@ -193,6 +193,10 @@ int main(int argc, char **argv)
 
     //relax to fit output rate
     loop_rate.sleep();
+
+    // for debug
+    if (checkManagerRunning(manager_name) == false)
+      return 0;
   }
 
   //exit program

op3_demo/src/vision/face_tracker.cpp

@@ -34,8 +34,7 @@ FaceTracker::FaceTracker()
   head_scan_pub_ = nh_.advertise<std_msgs::String>("/robotis/head_control/scan_command", 0);
 
   face_position_sub_ = nh_.subscribe("/face_position", 1, &FaceTracker::facePositionCallback, this);
-  face_tracking_command_sub_ = nh_.subscribe("/face_tracker/command", 1, &FaceTracker::faceTrackerCommandCallback,
-                                             this);
+  //face_tracking_command_sub_ = nh_.subscribe("/robotis/demo_command", 1, &FaceTracker::faceTrackerCommandCallback, this);
 }
 
 FaceTracker::~FaceTracker()
@@ -73,12 +72,14 @@ void FaceTracker::faceTrackerCommandCallback(const std_msgs::String::ConstPtr &m
 void FaceTracker::startTracking()
 {
   on_tracking_ = true;
+
   ROS_INFO("Start Face tracking");
 }
 
 void FaceTracker::stopTracking()
 {
   on_tracking_ = false;
+
   ROS_INFO("Stop Face tracking");
 }
 

op3_demo/src/vision/vision_demo.cpp

@@ -55,6 +55,11 @@ void VisionDemo::setDemoEnable()
   usleep(20 * 1000);
 
   enable_ = true;
+
+  std_msgs::Bool command;
+  command.data = enable_;
+  face_tracking_command_pub_.publish(command);
+
   face_tracker_.startTracking();
 
   ROS_INFO("Start Vision Demo");
@@ -68,6 +73,10 @@ void VisionDemo::setDemoDisable()
   is_tracking_ = false;
   tracking_status_ = FaceTracker::Waiting;
   enable_ = false;
+
+  std_msgs::Bool command;
+  command.data = enable_;
+  face_tracking_command_pub_.publish(command);
 }
 
 void VisionDemo::process()
@@ -95,7 +104,7 @@ void VisionDemo::process()
     tracking_status_ = tracking_status;
 
   //is_tracking_ = is_tracked;
-  std::cout << "Tracking : " << tracking_status << std::endl;
+  //std::cout << "Tracking : " << tracking_status << std::endl;
 }
 
 void VisionDemo::processThread()
@@ -122,6 +131,7 @@ void VisionDemo::callbackThread()
   module_control_pub_ = nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 0);
   motion_index_pub_ = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
   rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
+  face_tracking_command_pub_ = nh.advertise<std_msgs::Bool>("/face_tracking/command", 0);
 
   buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &VisionDemo::buttonHandlerCallback, this);
   faceCoord_sub_ = nh.subscribe("/faceCoord", 1, &VisionDemo::facePositionCallback, this);