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          42 : MockCupDetector::MockCupDetector()
      10             : {
      11          39 :     boost::system::error_code ec;
      12          78 :     auto path = boost::filesystem::read_symlink("/proc/self/exe", ec);
      13             : 
      14          39 :     if (!ec)
      15             :     {
      16          39 :         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          78 :     cv::FileStorage frgb;
      25          39 :     frgb.open((path / "rgb.yml").native(), cv::FileStorage::READ);
      26          78 :     cv::FileStorage fir;
      27          39 :     fir.open((path / "ir.yml").native(), cv::FileStorage::READ);
      28          78 :     cv::FileStorage fdepth;
      29          39 :     fdepth.open((path / "depth.yml").native(), cv::FileStorage::READ);
      30             : 
      31          39 :     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          72 :     cv::Mat d, i, c;
      39          36 :     frgb["Rgb"] >> c;
      40          36 :     fir["IR"] >> i;
      41          36 :     fdepth["DEPTH"] >> d;
      42          36 :     depth = std::make_shared<cv::Mat>(d);
      43          36 :     ir = std::make_shared<cv::Mat>(i);
      44          36 :     color = std::make_shared<cv::Mat>(c);
      45             : 
      46             :     // Create a fake detection with hardcoded cups
      47          36 :     detection.frame = depth;
      48          36 :     detection.cups.emplace_back(cv::Point(237, 209));
      49          36 :     detection.cups.emplace_back(cv::Point(331, 208));
      50          36 : }
      51             : 
      52           1 : std::shared_ptr<cv::Mat> MockCupDetector::get_detected_cups_image()
      53             : {
      54           1 :     return color;
      55             : }
      56             : 
      57           3 : std::shared_ptr<cv::Mat> MockCupDetector::get_latest_depth_frame()
      58             : {
      59           3 :     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           4 : CupDetection MockCupDetector::detect_cups()
      73             : {
      74           4 :     return detection;
      75             : }
      76             : 
      77             : } // namespace Vision
      78           3 : } // namespace HLR