CscMrptAutonomousExploration.h

Go to the documentation of this file.

//
// Created by Louis Grignon on 08/08/2025.
//
 
#ifndef CSCMRPTAUTONOMOUSEXPLORATION_H
#define CSCMRPTAUTONOMOUSEXPLORATION_H
 
#include "Bridging/EnvironmentInteractionCommands/environmentEntitiesCommands.h"
 
#include <Lidar/CscSlamEngine.h>
#include <CoreObjects/CscEntityModel.h>
 
using namespace conscience_core::lidar;
using namespace conscience_core::bridging::commands::environment_entities;
 
namespace conscience_core::ai {
class CscEntityReflexion;
 
class CSC_DLL_IMPORTEXPORT CscMrptAutonomousExploration {
public:
    CscMrptAutonomousExploration(
        const ptr<CscEntityReflexion> &entityReflexion,
        CscSlamEngine *slamEngine,
        CscEnvironmentSimulator &environmentSimulator,
        const CscEntityModel *entityModel,
        double minX, double minY, double maxX, double maxY);
    virtual ~CscMrptAutonomousExploration();
 
    void update(double nowSec);
 
    bool isStarted() const;
    void stop();
    void restart();
 
private:
    CscEnvironmentSimulator &environmentSimulator;
    CscSlamEngine *slamEngine = nullptr;
    const CscEntityModel *entityModel = nullptr;
 
    ptr<CscEntityReflexion> entityReflexion = nullptr;
    bool started = true;
 
    bool moveRobotToPosition2d(const TPoint2D &target);
    vector<TPoint2D> detectFrontiers(const COccupancyGridMap2D &map);
 
    // Bounds
    const double minX, minY, maxX, maxY;
 
    // robot bbox
    const CscSize3d robotDimensionsConscienceRefFrame;
    const double robotRadiusMeters;
    const double robotReachedKnownFrontierDistanceMeters;
    const double mobileDistanceThresholdMeters;
    const CscPoint2d robotDimensionsMrpt;
 
    optional<TPoint2D> adjustedTarget = {};
    optional<TPoint2D> immobilityCheckRefPosition = {};
    std::chrono::steady_clock::time_point lastMoveTime = std::chrono::steady_clock::now();
 
    // Planner state
    vector<TPoint2D> frontiers;
    double lastPlanTime = -1;
    size_t noFrontiersTickCount = 0;
    size_t retryOldFrontiersCount = 0;
    vector<TPose2D> lastPath;
    vector<TPoint2D> frontiersBeforeMovingToFindNewFrontiers;
 
    size_t cannotMoveToFrontiersTickCount = 0;
 
    std::thread *moveThread = nullptr;
    ptr<CommandSmallMoveToBestDirection> currentMoveToRestoreExplorationCommand = nullptr;
 
    void startMoveToRestoreExplorationAsync();
    void stopAllSequences();
    void stopRobotWheels();
    void moveRobotWheels(double leftPercent, double rightPercent);
 
    void saveExplorationDebugFiles(const ptr<COccupancyGridMap2D> &map, const TPose2D &pose, optional<TPoint2D> selectedTarget, const string &suffix = "");
};
}
 
#endif // CSCMRPTAUTONOMOUSEXPLORATION_H