Program Listing for File RTABMapSLAM.hpp

↰ Return to documentation for file (include/depthai/rtabmap/RTABMapSLAM.hpp)

#pragma once

#include "depthai/pipeline/DeviceNode.hpp"
#include "depthai/pipeline/Subnode.hpp"
#include "depthai/pipeline/ThreadedHostNode.hpp"
#include "depthai/pipeline/datatype/Buffer.hpp"
#include "depthai/pipeline/datatype/IMUData.hpp"
#include "depthai/pipeline/datatype/ImgFrame.hpp"
#include "depthai/pipeline/datatype/MapData.hpp"
#include "depthai/pipeline/datatype/MessageGroup.hpp"
#include "depthai/pipeline/datatype/PointCloudData.hpp"
#include "depthai/pipeline/datatype/TrackedFeatures.hpp"
#include "depthai/pipeline/datatype/TransformData.hpp"
#include "depthai/pipeline/node/Sync.hpp"
#include "depthai/utility/Pimpl.hpp"

namespace dai {
namespace node {

class RTABMapSLAM : public dai::NodeCRTP<dai::node::ThreadedHostNode, RTABMapSLAM> {
   public:
    constexpr static const char* NAME = "RTABMapSLAM";

    RTABMapSLAM();
    ~RTABMapSLAM() override;
    Subnode<node::Sync> sync{*this, "sync"};
    InputMap& inputs = sync->inputs;
    std::string rectInputName = "rect";
    std::string depthInputName = "depth";
    std::string featuresInputName = "features";

    Input& rect = inputs[rectInputName];
    Input& depth = inputs[depthInputName];
    Input features{*this, {featuresInputName, DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{DatatypeEnum::TrackedFeatures, true}}}}};
    Input odom{*this, {"odom", DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{dai::DatatypeEnum::TransformData, true}}}}};

    Output transform{*this, {"transform", DEFAULT_GROUP, {{{dai::DatatypeEnum::TransformData, true}}}}};
    Output odomCorrection{*this, {"odomCorrection", DEFAULT_GROUP, {{{dai::DatatypeEnum::TransformData, true}}}}};
    Output obstaclePCL{*this, {"obstaclePCL", DEFAULT_GROUP, {{{dai::DatatypeEnum::PointCloudData, true}}}}};
    Output groundPCL{*this, {"groundPCL", DEFAULT_GROUP, {{{dai::DatatypeEnum::PointCloudData, true}}}}};
    Output occupancyGridMap{*this, {"occupancyGridMap", DEFAULT_GROUP, {{{dai::DatatypeEnum::MapData, true}}}}};

    Output passthroughRect{*this, {"passthroughRect", DEFAULT_GROUP, {{{dai::DatatypeEnum::ImgFrame, true}}}}};
    Output passthroughDepth{*this, {"passthroughDepth", DEFAULT_GROUP, {{{dai::DatatypeEnum::ImgFrame, true}}}}};
    Output passthroughFeatures{*this, {"passthroughFeatures", DEFAULT_GROUP, {{{dai::DatatypeEnum::TrackedFeatures, true}}}}};
    Output passthroughOdom{*this, {"passthroughOdom", DEFAULT_GROUP, {{{dai::DatatypeEnum::TransformData, true}}}}};

    void setParams(const std::map<std::string, std::string>& params);

    void setDatabasePath(const std::string& path) {
        databasePath = path;
    }

    void setLoadDatabaseOnStart(bool load) {
        loadDatabaseOnStart = load;
    }

    void setSaveDatabaseOnClose(bool save) {
        saveDatabaseOnClose = save;
    }

    void saveDatabase();
    void setSaveDatabasePeriodically(bool save) {
        saveDatabasePeriodically = save;
    }
    void setSaveDatabasePeriod(double interval) {
        databaseSaveInterval = interval;
    }
    void setPublishObstacleCloud(bool publish);
    void setPublishGroundCloud(bool publish);
    void setPublishGrid(bool publish);
    void setFreq(float f) {
        freq = f;
    }
    void setAlphaScaling(float alpha) {
        alphaScaling = alpha;
    }
    void setUseFeatures(bool use);
    void setLocalTransform(std::shared_ptr<TransformData> transform);
    std::shared_ptr<TransformData> getLocalTransform();
    void triggerNewMap();

    void buildInternal() override;

   private:
    // pimpl
    class Impl;
    Pimpl<Impl> pimplRtabmap;
    void run() override;
    Input inSync{*this, {"inSync", DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{dai::DatatypeEnum::MessageGroup, true}}}}};
    void syncCB(std::shared_ptr<dai::ADatatype> data);
    void odomPoseCB(std::shared_ptr<dai::ADatatype> data);
    void imuCB(std::shared_ptr<dai::ADatatype> msg);
    void initialize(dai::Pipeline& pipeline, int instanceNum, int width, int height);
    float alphaScaling = -1.0;
    bool useFeatures = false;
    bool initialized = false;
    std::map<std::string, std::string> rtabParams;
    std::string databasePath = "";
    double databaseSaveInterval = 30.0;
    bool loadDatabaseOnStart = false;
    bool saveDatabaseOnClose = false;
    bool saveDatabasePeriodically = false;
    bool publishObstacleCloud = true;
    bool publishGroundCloud = true;
    bool publishGrid = true;
    float freq = 1.0f;
};
}  // namespace node
}  // namespace dai