Line data    Source code

       1             : #include "ObjectInterpreter.hpp"
       2             : #include <boost/filesystem.hpp>
       3             : #include <boost/filesystem/fstream.hpp>
       4             : 
       5             : namespace HLR
       6             : {
       7             : namespace PointCloud
       8             : {
       9             : namespace
      10             : {
      11             : std::optional<CGAL::Surface_mesh<CGAL::Simple_cartesian<double>::Point_3>>
      12             : get_static_polyhedron();
      13             : }
      14             : 
      15           0 : std::vector<Object> ObjectInterpreter::get_current_objects(
      16             :     const HLR::Vision::CupDetection& detected_cups,
      17             :     const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
      18             : {
      19           0 :     merge_objects(detected_cups, point_cloud);
      20           0 :     return current_objects;
      21             : }
      22             : 
      23           0 : std::vector<Object> ObjectInterpreter::get_changed_objects(
      24             :     const HLR::Vision::CupDetection& detected_cups,
      25             :     const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
      26             : {
      27           0 :     merge_objects(detected_cups, point_cloud);
      28           0 :     std::vector<Object> changed_objects;
      29           0 :     if (!history.empty())
      30             :     {
      31           0 :         for (const auto& i : current_objects)
      32             :         {
      33           0 :             bool is_new = true;
      34           0 :             for (const auto& j : history[0])
      35             :             {
      36           0 :                 if (i.id == j.id)
      37             :                 {
      38           0 :                     is_new = false;
      39           0 :                     if (i != j)
      40             :                     {
      41           0 :                         changed_objects.push_back(i);
      42             :                     }
      43             :                 }
      44             :             }
      45           0 :             if (is_new)
      46             :             {
      47           0 :                 changed_objects.push_back(i);
      48             :             }
      49             :         }
      50           0 :         return changed_objects;
      51             :     }
      52           0 :     return current_objects;
      53             : }
      54             : 
      55           0 : void ObjectInterpreter::merge_objects(
      56             :     const HLR::Vision::CupDetection& detected_cups,
      57             :     const pcl::PointCloud<pcl::PointXYZ>& point_cloud)
      58             : {
      59           0 :     static auto polyhedron = get_static_polyhedron().value_or(
      60           0 :         CGAL::Surface_mesh<CGAL::Simple_cartesian<double>::Point_3>{});
      61             : 
      62           0 :     history.push_back(std::move(current_objects));
      63             : 
      64           0 :     Object object;
      65           0 :     object.id = 0;
      66           0 :     object.polyhedron = polyhedron;
      67           0 :     object.speed = std::nullopt;
      68           0 :     object.is_cup = false;
      69           0 :     object.is_deleted = false;
      70           0 :     lidar_interpreter.get_objects(detected_cups, point_cloud);
      71           0 :     current_objects.push_back(object);
      72           0 : }
      73             : namespace
      74             : {
      75             : std::optional<CGAL::Surface_mesh<CGAL::Simple_cartesian<double>::Point_3>>
      76           0 : get_static_polyhedron() try
      77             : {
      78           0 :     boost::system::error_code ec;
      79           0 :     auto path = boost::filesystem::read_symlink("/proc/self/exe", ec);
      80             : 
      81           0 :     if (!ec)
      82             :     {
      83           0 :         path.remove_filename();
      84           0 :         path /= "poly.off";
      85             :     }
      86             :     else
      87             :     {
      88           0 :         path = "poly.off";
      89             :     }
      90             : 
      91           0 :     boost::filesystem::ifstream poly_stream{path};
      92             : 
      93           0 :     CGAL::Surface_mesh<CGAL::Simple_cartesian<double>::Point_3> polyhedron;
      94           0 :     poly_stream >> polyhedron;
      95             : 
      96           0 :     return polyhedron;
      97             : }
      98           0 : catch (...)
      99             : {
     100           0 :     return std::nullopt;
     101             : }
     102             : } // namespace
     103             : 
     104             : } // namespace PointCloud
     105           3 : } // namespace HLR