VENUS ROBOTIX

#include <opencv2/highgui/highgui.hpp>

#include "image_processing.h"

hsv_threshold threshold;

RTC::ReturnCode_t BallDetection::onActivated(RTC::UniqueId ec_id)

{

  threshold.hue_lower = m_hue_lower;

  threshold.hue_upper = m_hue_upper;

  threshold.saturation = m_saturation;

  threshold.value = m_value;

  return RTC::RTC_OK;

}

RTC::ReturnCode_t BallDetection::onDeactivated(RTC::UniqueId ec_id)

{

  cv::destroyAllWindows();

  return RTC::RTC_OK;

}

RTC::ReturnCode_t BallDetection::onExecute(RTC::UniqueId ec_id)

{

  if (m_cameraIn.isNew()){

      m_cameraIn.read();

      if(m_camera.bpp == 24)

      {

          cv::Mat receive(m_camera.height, m_camera.width, CV_8UC3, &m_camera.pixels[0]);

          cv::Mat binary = extract_color(receive, threshold);

          cv::vector<cv::Vec3f> circles;

          cv::HoughCircles(binary, circles, CV_HOUGH_GRADIENT, 1, binary.rows/8, 200, 10, 0, 0);

          if(circles.size() > 0)

          {

              cv::Point2f center(circles[0][0], circles[0][1]);

              m_point3d.data.z = m_ball_radius / circles[0][2] * m_f_per_s;

              m_point3d.data.y = (center.x - m_camera.width / 2) * m_point3d.data.z / m_f_per_s;

              m_point3d.data.x = (center.y - m_camera.height / 2) * m_point3d.data.z / m_f_per_s;

              std::cout << "Z: "  << m_point3d.data.z

                        << " X: " << m_point3d.data.x

                        << " Y: " << m_point3d.data.y << std::endl;

              cv::circle(receive, center, 3, cv::Scalar(0, 255, 0), -1, 8, 0);

              cv::circle(receive, center, circles[0][2], cv::Scalar(0, 0, 255), 3, 8, 0);

              setTimestamp(m_point3d);

              m_point3dOut.write();

          }

          cv::imshow("Capture", receive);

          cv::waitKey(1);

      }

  }

  return RTC::RTC_OK;

}