CommandTerrassementPickUp.h

Go to the documentation of this file.

//  ConscienceRobotManager
//
//  Created by Hazar Zaoui on 06/10/2020.
//  Copyright © 2020 IliesZaoui. All rights reserved.
//
 
#ifndef ConscienceCommandTerrassementPickUp_h
#define ConscienceCommandTerrassementPickUp_h
 
#include "./Command.h"
#include "Util/conscience_util.h"
#include "./Result/CommandExecutionResultNone.h"
#include "CoreObjects/CscMember.h"
 
#include "Util/CscCartography.h"
#include "./CommandDivideTarget.h"
#include "./CommandLocateTargetMinus.h"
#include "./CommandPlayStateOutputs.h"
#include "./Bridging/EnvironmentInteractionCommands/environmentEntitiesCommands.h"
#include "./CommandEntityToObject.h"
#include "./Command.h"
#include "./Result/CommandExecutionResultSequence.h"
#include "CoreObjects/CscSequence.h"
#include "CoreObjects/CscState.h"
 
#include <string>
 
using std::vector; using std::string;
 
using namespace conscience_utils::Cartography;
using conscience_utils::logging::CscLogger;
 
namespace conscience_core::bridging::commands {
    
    
    class CSC_DLL_IMPORTEXPORT CommandTerrassementPickUp : public CscCommand {
 
    public:
        inline static const CommandTypeId COMMAND_ID = "TERRASSEMENT_PICK_UP";
        CommandTerrassementPickUp(ptr<CscEntityReflexion> entityReflexion, double entityLat, double entityLon, float yaw, float targetPointX, float targetPointY, float targetPointZ);
    protected:
        const CscCommandExecutionResultSequence * doExecute(CscEnvironmentSimulator &environmentSimulator) override;
        std::unique_ptr<CscLogger> logger;
        ptr<CscEntityReflexion> getEntityReflexion() const override {
            return entityReflexion;
        }
    private:
        ptr<CscEntityReflexion> entityReflexion;
        double entityLat;
        double entityLon;
        float yaw;
        float targetPointX;
        float targetPointY;
        float targetPointZ;
 
        float getAngleBetweenTwoPoint(CscPoint3d* basePoint, CscPoint3d* objectivePoint);
        float calculateDeltaWithCap(CscPoint3d * firstPoint, CscPoint3d * secondPoint,float angle = 0.0);
        CscPoint3d* convertReferentialToReality(float distance, float delta, float angleDirection);
 
        double l(double phi,double e);
        
        CscPoint3d getTargetPositionFinal (const CscMember &targetOriginRelativeMember, CscPoint3d &targetPoint, CscPoint3d &distanceFromTarget) {
            float x = targetPoint.getX();
            float y = targetPoint.getY();
            float z = targetPoint.getZ();
            
            x-= distanceFromTarget.getX();
            y-= distanceFromTarget.getY();
            z-= distanceFromTarget.getZ();
            
            return CscPoint3d(x, y, z);
        }
    };
 
COMMAND_REGISTER(CommandTerrassementPickUp, ptr<CscEntityReflexion>, double, double, float, float, float, float)
 
}
 
#endif