ObjectInterpreter.hpp

#ifndef OBJECTINTERPRETER_HPP
#define OBJECTINTERPRETER_HPP

#include <chrono>
#include <vector>
#include <boost/circular_buffer.hpp>
#include "LidarInterpreter.hpp"
#include "Object.hpp"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace HLR
{
namespace PointCloud
{
class ObjectInterpreter
{
public:
    ObjectInterpreter();
    std::vector<Object> get_current_objects(
        const HLR::Vision::CupDetection& detected_cups,
        const pcl::PointCloud<pcl::PointXYZ>& point_cloud);

    std::vector<Object> get_changed_objects(
        const HLR::Vision::CupDetection& detected_cups,
        const pcl::PointCloud<pcl::PointXYZ>& point_cloud);

    std::vector<Object> determine_speed(std::vector<Object>& objects,
                                        long milliseconds_since_epoch = -1);

private:
    void add_only_changed_objects(const Object& object,
                                  std::vector<Object>& changed_objects) const;
    void merge_objects(const HLR::Vision::CupDetection& detected_cups,
                       const pcl::PointCloud<pcl::PointXYZ>& point_cloud);
    // TEMPORARY FIX: keep the id's of the last cups, delete the rest.
    void tolerance_based_object_detection(double cup_tolerance);

    // milliseconds since epoch
    long last_measure_time;
    std::vector<Object> isolate_objects(
        const pcl::PointCloud<pcl::PointXYZ>& newest_cloud);
    std::vector<Object> current_objects;
    boost::circular_buffer<std::vector<Object>> history =
        boost::circular_buffer<std::vector<Object>>(5);
    LidarInterpreter lidar_interpreter;
};
} // namespace PointCloud
} // namespace HLR
#endif /* OBJECTINTERPRETER_HPP */