CscLidarEngine.h

Go to the documentation of this file.

//
//  CscLidarEngine.hpp
//  ConscienceRobotManager
//
//  Created by Iliès Zaoui on 14/07/2019.
//  Copyright © 2020 Conscience Robotics. All rights reserved.
//
 
#ifndef CscLidarEngine_h
#define CscLidarEngine_h
 
#include <mutex>
 
#include <CscCommon.h>
#include <Axiomes/Csc3dTypes.h>
#include <Axiomes/CscPoint3dIterable.h>
#include <queue>
#include "CscRealEntity3dLidarValues.h"
 
using namespace conscience_core::axiomes;
using std::deque, std::mutex;
 
namespace conscience_core::lidar {
 
struct OdometryData {
public:
    double deltaPositionX;
    double deltaPositionY;
    double deltaPositionZ;
    double deltaPitch;
    double deltaYaw;
    double deltaRoll;
    optional<uint64_t> timeStamp;
    string toString() const;
 
    OdometryData(double deltaPositionX = 0, double deltaPositionY = 0, double deltaPositionZ = 0, double deltaPitch = 0, double deltaYaw = 0, double deltaRoll = 0, optional<uint64_t> timeStamp = {});
};
 
inline CscQuaternion extractNavigationQuaternionFromEntityQuaternion(const CscQuaternion &entityQuaternion) {
    const CscQuaternion inverseMeshBaseRoll = CscQuaternion::fromAngleAxis(M_PI * 0.5, Axis3d::AXIS_3D_X);
 
    CscQuaternion qNav = entityQuaternion * inverseMeshBaseRoll;
    qNav.normalize();
 
    return qNav;
}
 
inline double extractNavigationYawFromEntityQuaternion(const CscQuaternion &entityQuaternion) {
    const CscPoint3d forward = extractNavigationQuaternionFromEntityQuaternion(entityQuaternion) * CscPoint3d(1.0, 0.0, 0.0);
    const double robotYaw = std::atan2(forward.getZ(), forward.getX());
    return robotYaw;
}
 
inline vector<CscPoint3d> transformPointsLidarLocalToWorld(
    const vector<CscPoint3d> &pointCloud,
    const CscPoint3d &entityPosition, const CscQuaternion &entityRotationQuaternion) {
    vector<CscPoint3d> worldCoordinates;
    worldCoordinates.reserve(pointCloud.size());
 
    const CscQuaternion qNav =
        extractNavigationQuaternionFromEntityQuaternion(entityRotationQuaternion);
 
    for (const CscPoint3d &point : pointCloud) {
        const CscPoint3d rotated = qNav * point;
 
        worldCoordinates.emplace_back(
            entityPosition.getX() + rotated.getX(),
            entityPosition.getY() + rotated.getY(),
            entityPosition.getZ() + rotated.getZ());
    }
 
    return worldCoordinates;
}
 
inline CscPoint3d transformPointLidarLocalToWorld(
    const CscPoint3d &localPoint,
    const CscPoint3d &entityPosition,
    const CscQuaternion &entityRotationQuaternion) {
    vector<CscPoint3d> localCoordinates = {localPoint};
    return transformPointsLidarLocalToWorld(localCoordinates, entityPosition, entityRotationQuaternion).at(0);
}
 
inline CscPoint3d transformPointWorldToLidarLocal(
    const CscPoint3d &worldPoint,
    const CscPoint3d &entityPosition,
    const CscQuaternion &entityRotationQuaternion) {
    const CscQuaternion qNav = extractNavigationQuaternionFromEntityQuaternion(entityRotationQuaternion);
 
    const CscPoint3d delta(
        worldPoint.getX() - entityPosition.getX(),
        worldPoint.getY() - entityPosition.getY(),
        worldPoint.getZ() - entityPosition.getZ());
 
    return qNav.inverse() * delta;
}
class CscLidarEngine;
 
class CscPointCloud3d {
public:
    std::vector<CscPoint3d> points;
 
    double minY = 0.0f;
    double maxY = 0.0f;
    double avgY = 0.0f;
 
    bool is3d = true;
};
 
class LidarValuesSnapshot {
public:
 
    CscPointCloud3d pointCloud;
 
    std::vector<CscPoint3d> getPointCloudWorldCoordinates(const CscPoint3d &entityPosition, const CscQuaternion &robotOrientation) const;
 
    unsigned long long timestamp = 0;
 
    optional<OdometryData> odometry = {};
 
 
    LidarValuesSnapshot() = default;
 
    LidarValuesSnapshot(CscPointCloud3d pointCloud,
                        unsigned long long timestamp,
                        std::optional<OdometryData> odometry);
 
    // extract 2D slice around targetY (cm) with tolerance (cm)
    void getSliceAtY(std::vector<CscPoint3d> &out,
                     double targetY,
                     double tolerance) const;
    void getSliceAtY(std::map<double, double> &out, double targetY, double tolerance) const;
 
    bool is3d() const;
};
 
class LidarValues {
public:
    unsigned long long getUpdateTimeMillis() const;
 
    LidarValuesSnapshot getValuesSnapshot() const;
 
    bool has3dValues() const;
 
private:
    void setFor2D(const map<double, double> &newValues, const optional<OdometryData> &odometry, unsigned long long timestamp);
    void setFor3DRangeImageEncoded(uint16_t rows, uint16_t cols, uint32_t frameId, bool isKeyFrame, const uint8_t *data, uint32_t itemCount, const LidarImuData &imu, const optional<OdometryData> &odometry, LidarExtrinsics lidarExtrinsics, unsigned long long timestamp);
    void setFor3DPointCloud(
        const CscPointCloud3d *pointCloud,
        const optional<OdometryData> &odometry,
        unsigned long long timestamp);
 
    map<double, double> lidarValues;
    unsigned long long timestamp = 0;
    optional<OdometryData> odometry = {};
 
    CscRealEntity3dLidarValues *values3dRangeImage = nullptr;
    mutable /* mutable because we want to set cache in getValuesSnapshot */ const CscPointCloud3d *values3dPointCloud = nullptr;
 
    mutable mutex valuesMutex;
 
    friend class CscLidarEngine;
};
 
typedef uint64_t CscLidarMode;
enum CscLidarModeList : CscLidarMode {
    cscLidarModePaused = 1ULL << 0,
    cscLocalisation = 1ULL << 2,
 
    cscSlam = 1ULL << 4,
};
 
class CscLidarEngine {
public:
    static bool isSlamAutoEnabled();
 
    CscLidarEngine();
    virtual void startEngine();
    virtual void stopEngine();
    const LidarValues &getCurrentValues();
 
    void integrate2DValuesAtTime(const map<double, double> &newValues, unsigned long long timestamp, optional<OdometryData> odometry = {});
 
    void integrate3DRangeImageAtTime(uint16_t rows, uint16_t cols, uint32_t frameId, bool isKeyFrame, const uint8_t *data, uint32_t itemCount, const LidarImuData &imu, const optional<OdometryData> &odometry, LidarExtrinsics lidarExtrinsics, unsigned long long timestamp);
 
    void integrate3DPointCloudAtTime(
        const CscPointCloud3d *pointCloud,
        unsigned long long timestamp,
        optional<OdometryData> odometry = {});
 
    virtual ~CscLidarEngine();
    void activateLidarMode(CscLidarMode newLidarMode);
    void addLidarMode(CscLidarMode newLidarMode);
    void removeLidarMode(CscLidarMode lidarModeToRemove);
    bool isLidarModeRunning(CscLidarMode lidarModeSearched);
    bool isLidarModeActive(CscLidarMode lidarModeSearched);
    bool isLidarEngineActivated();
    void stopLidarEngine();
 
    unsigned long long getLastLidarValuesTime() const;
    unsigned long long getLidarValuesTimeToRetrieve() const;
    unsigned long long getLidarValuesTimeBetweenUpdate() const;
 
    void setOnNewValuesListener(const optional<function<void(const LidarValuesSnapshot &)>> &onNewValues = {});
    optional<function<void()>> getOnActivatedListener() const;
    void setOnActivatedListener(const optional<function<void()>> &onActivated = {});
 
    static vector<CscPoint3d *> toWorldPoints(const map<double, double> &lidarValues, const CscQuaternion &entityOrientation, const CscPoint3d &entityPosition);
 
private:
    mutex cartographyMutex;
    mutex lidarReadingsMutex;
 
    CscLidarMode lidarMode = cscLidarModePaused;
 
    unsigned long long lidarValuesTimeToRetrieveInMillis = 0;
    unsigned long long lidarValuesTimeBetweenUpdateInMillis = 0;
 
protected:
    mutex lidarValuesMutex;
    LidarValues values;
    optional<function<void(const LidarValuesSnapshot &)>> onNewValues = {};
    optional<function<void()>> onActivated = {};
 
    void get2dValues(map<double, double> **lidarPoints, optional<OdometryData> **odometry, unsigned long long &timestamp);
};
 
}
 
#endif /* CscLidarEngine */