HLR  0.0.1
LidarInterpreter.hpp
1 #ifndef LIDAR_INTERPRETER_HPP
2 #define LIDAR_INTERPRETER_HPP
3 
4 #include <cmath>
5 #include <cstdint>
6 #include "ISensorInterpreter.hpp"
7 #include "Vision/ICupDetector.hpp"
8 #ifdef TESTING
9 #include "Vision/mock/MockCupDetector.hpp"
10 #else
11 #include "Vision/CupDetector.hpp"
12 #endif
13 #include <pcl/point_cloud.h>
14 #include <pcl/point_types.h>
15 
16 namespace HLR
17 {
18 namespace PointCloud
19 {
25 {
26 public:
38  std::vector<Object> get_objects(
39  const HLR::Vision::CupDetection& detected_cups,
40  const pcl::PointCloud<pcl::PointXYZ>& point_cloud) override;
41 
42 private:
43  struct Cup
44  {
45  Object::Point centerpoint;
46  double upper_radius;
47  double lower_radius;
48  double height;
49  std::uint32_t id;
50  };
51 
52  using PointCloudWithId =
53  std::pair<pcl::PointCloud<pcl::PointXYZ>, std::uint32_t>;
54 
55  struct CupMeasurements
56  {
57  double upper_radius;
58  double lower_radius;
59  double height;
60  CupMeasurements() = default;
61 
62  CupMeasurements(double upper_radius, double lower_radius, double height)
63  : upper_radius(upper_radius)
64  , lower_radius(lower_radius)
65  , height(height)
66  {}
67  };
68 
69  static Object convert_cup_to_polyhedron(
70  const Cup& cup_in_robot_arm_space,
71  const pcl::PointCloud<pcl::PointXYZ>& point_cloud);
72  static Cup translate_lidar_to_robot_arm(const Cup& cup_in_lidar_space);
73  static Cup get_centerpoint_from_contour(
74  const PointCloudWithId& point_cloud_with_id);
75  PointCloudWithId find_contour(
76  std::pair<double, double> angle_and_distance_cup,
77  const pcl::PointCloud<pcl::PointXYZ>& newest_cloud);
78  static std::pair<double, double> translate_kinect_to_lidar(
79  const std::shared_ptr<cv::Mat>& distance_frame,
80  const cv::Point& location_cup);
81  bool is_angle_in_angle_range(double input,
82  double min_angle,
83  double max_angle);
84  std::vector<PointCloudWithId> pointcloud_database;
85  unsigned long highest_pointcloud_id = 0;
86  std::unique_ptr<HLR::Vision::ICupDetector> cup_detector
87 #ifdef TESTING
88  = std::make_unique<HLR::Vision::MockCupDetector>();
89 #else
90  // = std::make_unique<HLR::Vision::CupDetector>();
91  ;
92 #endif
93 
94  static constexpr double distance_kinect_lidar = 0.084; // meters
95  static constexpr unsigned number_of_cylinder_faces = 24;
99  static const CupMeasurements
100  measured_cup_values; // = CupMeasurements(0.079, 0.03, 0.0225);
101  static constexpr double lidar_height = 0.032;
102  static const CGAL::Simple_cartesian<double>::Vector_3
103  lidar_to_robot_arm_translation;
104 };
105 
106 } // namespace PointCloud
107 } // namespace HLR
108 
109 #endif /*LIDAR_INTERPRETER_HPP */
A class for mapping the ROS::Object defined in msg/Object.msg.
Definition: Object.hpp:24
CGAL::Simple_cartesian< double >::Point_3 Point
Data type specifying a point in 3D space.
Definition: Object.hpp:29
Collection of detected cups Collection of detected cups where the location of the cups is described i...
Definition: ICupDetector.hpp:17
An abstract class responsible for reading and interpreting the data from a device interpreting the wo...
Definition: ISensorInterpreter.hpp:18
std::vector< Object > get_objects(const HLR::Vision::CupDetection &detected_cups, const pcl::PointCloud< pcl::PointXYZ > &point_cloud) override
Construct the objects in the given CupDetection.
A class responsible for reading and interpreting the LiDAR data.
Definition: LidarInterpreter.hpp:24