Line data    Source code

       1             : #include "Vision/mock/MockCupDetector.hpp"
       2             : #include <exception>
       3             : #include <boost/filesystem.hpp>
       4             : 
       5             : namespace HLR
       6             : {
       7             : namespace Vision
       8             : {
       9          11 : MockCupDetector::MockCupDetector()
      10             : {
      11           8 :     boost::system::error_code ec;
      12          16 :     auto path = boost::filesystem::read_symlink("/proc/self/exe", ec);
      13             : 
      14           8 :     if (!ec)
      15             :     {
      16           8 :         path.remove_filename();
      17             :     }
      18             :     else
      19             :     {
      20           0 :         path.clear();
      21             :     }
      22             : 
      23             :     // Read all the yml files to extract the saved mat's.
      24          16 :     cv::FileStorage frgb;
      25           8 :     frgb.open((path / "rgb.yml").native(), cv::FileStorage::READ);
      26          16 :     cv::FileStorage fir;
      27           8 :     fir.open((path / "ir.yml").native(), cv::FileStorage::READ);
      28          16 :     cv::FileStorage fdepth;
      29           8 :     fdepth.open((path / "depth.yml").native(), cv::FileStorage::READ);
      30             : 
      31           8 :     if (!frgb.isOpened() || !fir.isOpened() || !fdepth.isOpened())
      32             :     {
      33             :         throw std::runtime_error(
      34           3 :             "Could not open yml files for Vision MockCupDetector");
      35             :     }
      36             : 
      37             :     // Put the different mats in their respective member value
      38          10 :     cv::Mat d, i, c;
      39           5 :     frgb["Rgb"] >> c;
      40           5 :     fir["IR"] >> i;
      41           5 :     fdepth["DEPTH"] >> d;
      42           5 :     depth = std::make_shared<cv::Mat>(d);
      43           5 :     ir = std::make_shared<cv::Mat>(i);
      44           5 :     color = std::make_shared<cv::Mat>(c);
      45             : 
      46             :     // Create a fake detection with hardcoded cups
      47           5 :     detection.frame = depth;
      48           5 :     detection.cups.emplace_back(cv::Point(237, 209));
      49           5 :     detection.cups.emplace_back(cv::Point(331, 208));
      50           5 : }
      51             : 
      52           1 : std::shared_ptr<cv::Mat> MockCupDetector::get_detected_cups_image()
      53             : {
      54           1 :     return color;
      55             : }
      56             : 
      57           1 : std::shared_ptr<cv::Mat> MockCupDetector::get_latest_depth_frame()
      58             : {
      59           1 :     return detection.frame;
      60             : }
      61             : 
      62           1 : std::shared_ptr<cv::Mat> MockCupDetector::get_latest_ir_frame()
      63             : {
      64           1 :     return ir;
      65             : }
      66             : 
      67           1 : std::shared_ptr<cv::Mat> MockCupDetector::get_latest_color_frame()
      68             : {
      69           1 :     return color;
      70             : }
      71             : 
      72           2 : CupDetection MockCupDetector::detect_cups()
      73             : {
      74           2 :     return detection;
      75             : }
      76             : 
      77             : } // namespace Vision
      78           3 : } // namespace HLR