laser/updater.h

Go to the documentation of this file.

#ifndef ED_LASER_LASER_UPDATER_H_
#define ED_LASER_LASER_UPDATER_H_
 
#include "ed/laser/entity_update.h"
 
#include <ed/convex_hull.h>
#include <ed/init_data.h>
#include <ed/world_model.h>
 
#include <geolib/sensors/LaserRangeFinder.h>
 
#include <map>
#include <string>
 
typedef std::vector<unsigned int> ScanSegment;
 
class LaserUpdater
{
 
public:
    void configure(tue::Configuration& config);
 
    void update(const ed::WorldModel& world, std::vector<double>& sensor_ranges,
                const geo::Pose3D& sensor_pose, const double timestamp, ed::UpdateRequest& req);
 
    void configureLaserModel(uint num_beams, double angle_min, double angle_max, double range_min, double range_max)
    {
        lrf_model_.setNumBeams(num_beams);
        lrf_model_.setAngleLimits(angle_min, angle_max);
        lrf_model_.setRangeLimits(range_min, range_max);
    }
 
    inline void setLaserFrame(const std::string& frame_id) { lrf_frame_ = frame_id; }
 
    unsigned int getNumBeams() { return lrf_model_.getNumBeams(); }
 
    void renderWorld(const geo::Pose3D& sensor_pose, const ed::WorldModel& world, std::vector<double>& model_ranges);
 
    // parameters
    uint min_segment_size_pixels_; // in nr of laser points
    double world_association_distance_; // in m
    double segment_depth_threshold_; 
    double min_cluster_size_;
    double max_cluster_size_;
    bool fit_entities_;
    uint max_gap_size_;
 
private:
    void associate(std::vector<double>& sensor_ranges, const std::vector<double>& model_ranges);
 
    std::vector<ScanSegment> segment(const std::vector<double>& sensor_ranges);
 
    EntityUpdate segmentToConvexHull(const ScanSegment& segment, const geo::Pose3D& sensor_pose, const std::vector<double>& sensor_ranges);
 
    // PARAMETERS
    geo::LaserRangeFinder lrf_model_;
    std::string lrf_frame_;
 
    std::map<ed::UUID, geo::Pose3D> pose_cache;
 
};
 
#endif