CscMoveForwardInGroup.h

Go to the documentation of this file.

//
//  CscMoveForwardInGroup.hpp
//  ConscienceRobotManager
//
//  Created by Max Lhermitte on 15/02/2023.
//  Copyright © 2023 IliesZaoui. All rights reserved.
//
 
#ifndef CscMoveForwardInGroup_hpp
#define CscMoveForwardInGroup_hpp
 
#include "../../Simulator/CscEnvironmentSimulator.h"
#include <Lidar/CscEntityPositionManager.h>
#include "Axiomes/Csc3dTypes.h"
#include "CscCommon.h"
#include "CscMoveForwardInGroupTarget.h"
#include "CscMoveForwardInGroupTargetObject.h"
#include <Lidar/CscEntityPositionManager.h>
 
#include <mutex>
 
namespace conscience_core::ai {
class CscEntityReflexion;
}
using namespace conscience_core::lidar;
 
namespace conscience_core::ai::multi_entity {
 
 
class CSC_DLL_IMPORTEXPORT CscMoveForwardInGroup {
public:
    bool hasAlreadySentSequenceStop = false;
    struct CSC_DLL_IMPORTEXPORT FollowParameters {
    private:
        std::optional<float> angle;
        std::optional<float> distance;
        std::optional<float> height;
 
    public:
        FollowParameters(std::optional<float> angle = 0.0f,
                         std::optional<float> distance = 0.0f,
                         std::optional<float> height = 0.0f);
 
        float getAngle() const;
        float getDistance() const;
        float getHeight() const;
    };
 
    struct CSC_DLL_IMPORTEXPORT SlaveMoveParameters {
    private:
        const string serialNumber;
        CscPoint3d *position;
        CscPoint3d *positionTarget;
        const FollowParameters *followParams;
        CscEntityPositionManager *posManager;
 
    public:
        SlaveMoveParameters(const string &serialNumber, const FollowParameters *followParams, CscEntityPositionManager *posManager);
        ~SlaveMoveParameters();
 
        const string &getSerialNumber() const;
        CscPoint3d *getPosition() const;
        CscPoint3d *getPositionTarget() const;
        const FollowParameters *getFollowParams() const;
        CscEntityPositionManager *getPosManager() const;
        void setPosition(CscPoint3d *position);
        void setPositionTarget(CscPoint3d *positionTarget);
    };
 
    struct CSC_DLL_IMPORTEXPORT FollowState {
        public:
            FollowState(CscPoint3d* targetOnImage, CscPoint3dOriented* robotPositionAndRotation);
            ~FollowState();
 
            bool hasSimilarityWith(const FollowState &other) const;
 
        private :
            CscPoint3d* targetOnImage;
            CscPoint3dOriented* robotPositionAndRotation;
    };
 
    CscMoveForwardInGroup(
        ptr<CscMoveForwardInGroupTarget> masterReference,
        ptr<CscEntityReflexion> entitySlaveReflexion,
        const FollowParameters *followParameters,
        const CscEnvironmentSimulator &environmentSimulator);
 
    ~CscMoveForwardInGroup();
 
    void moveForwardInGroupLoopTick();
    const string &getFollowTargetWorldId() const;
    void addSlave(ptr<CscEntityReflexion> slave, const CscMoveForwardInGroup::FollowParameters *params);
    void removeSlave(const string &serialNumberSlaveToRemove);
 
    vector<SlaveMoveParameters *> getSlaveMoveParameters();
    vector<string> getSlaveSerialNumbers();
 
private:
    unique_ptr<CscLogger> logger = CscLogger::getForCategory("CscMoveForwardInGroup");
    const CscEnvironmentSimulator &globalEnvironment;
    ptr<CscMoveForwardInGroupTarget> masterReference;
    ptr<CscEntityReflexion> entitySlaveReflexion;
    string followTargetWorldId;
    CscPoint3d *masterPosition;
    CscPoint3d *masterOrientation;
    vector<SlaveMoveParameters *> slavesMoveParameters;
    recursive_mutex slaveMutex;
 
    FollowState* lastFollowState = nullptr;
 
    bool updateMasterPosition();
    void updateSlavesPositionsTarget();
 
    bool isDetectionUpdated();
    
    CscPoint3d* computeTargetPositionFromRover(const CscPoint3dOriented *positionAndRotationFromSimulator, const CscPoint3d *tagetPosition);
    CscPoint3d* computeTargetPositionFromDrone(const CscPoint3dOriented *positionAndRotationFromSimulator, const CscPoint3d *tagetPosition, FieldOfView fov, float followDistance);
 
    CscPoint3dOriented* getEntitySlavePositionAndRotation();
};
 
}
 
#endif