CscEntityPositionManager.h

Go to the documentation of this file.

#ifndef CscEntityPositionManager_h
#define CscEntityPositionManager_h
 
#include <filesystem>
#include <map>
#include <shared_mutex>
#include <string>
#include <vector>
 
#include <Util/conscience_log.h>
#include <Axiomes/Csc3dTypes.h>
using namespace conscience_utils::logging;
using namespace conscience_core::axiomes;
 
using std::string, std::shared_mutex, std::vector, std::map, std::pair, std::shared_mutex, std::unique_lock, std::shared_lock;
namespace fs = std::filesystem;
 
class CscEnvironmentSimulator;
using namespace conscience_utils;
 
namespace conscience_core::lidar {
 
enum CscEntityPositionSource {
    Entity = 0,
    LidarLocalization = 1,
    Simulator = 2,
    CoreInstruction = 3
};
 
// TODO : read position from simulator if no position read from other sources during X millis
 
class CscEntityPositionManager {
private:
    struct PositionEntry {
        const CscPoint3dOriented *position;
        unsigned long long timestampMillis;
        float certitude;
        CscEntityPositionSource source;
 
        ~PositionEntry();
    };
    bool isGpsValid = false;
    const string entitySerialNumber;
    CscEnvironmentSimulator &globalEnvironmentSimulator;
 
    vector<const PositionEntry *> positionsHistory;
    mutable shared_mutex positionsHistoryMutex;
 
    std::unique_ptr<CscLogger> logger;
 
    void persistPositionToEnvironment(const PositionEntry *positionEntry);
 
    bool areConsistent(const PositionEntry *entry1, const PositionEntry *entry2, double improbablePositionDistanceCm) const;
 
    typedef std::function<void(const CscPoint3dOriented *position, const string serialNumber, CscEntityPositionSource source)> PositionUpdateListener;
 
    map<const string, PositionUpdateListener> positionListeners;
 
    void emptyPositionHistory();
 
    shared_mutex realEntityAccelerationMutex;
    optional<CscPoint3d> realEntityAcceleration;
 
public:
    CscEntityPositionManager(const string &entitySerialNumber, CscEnvironmentSimulator &globalEnvironmentSimulator);
    virtual ~CscEntityPositionManager();
 
    void addPositionUpdateListener(const string &listenerId, PositionUpdateListener listener);
 
    void removePositionUpdateListener(const string &listenerId);
 
    const CscPoint3dOriented *getLastPlausiblePosition(double improbablePositionDistanceCm = 50) const;
 
    const CscPoint3dOriented *getLastPosition() const;
 
    const CscPoint3dOriented *getPositionFromSimulator() const;
 
    pair<CscPoint3d, CscQuaternion> getPositionAndOrientationQuaternionFromSimulator() const;
 
    void registerPosition(const CscPoint3dOriented *position, float certitude, CscEntityPositionSource source, double improbablePositionDistanceCm = 50, bool forceRegisterPosition = false);
 
    const string getEntitySerialNumber() const {
        return entitySerialNumber;
    }
 
    CscEnvironmentSimulator &getEnvironmentSimulator() const;
 
    void setGpsValid(bool isValid);
    bool getGpsValid() const;
 
    map<const string, PositionUpdateListener> getPositionListeners() const;
 
 
    optional<CscPoint3d> getRealEntityAcceleration();
    void setRealEntityAcceleration(optional<CscPoint3d> acceleration);
};
 
}
 
#endif