140 lines
4.1 KiB
C++
140 lines
4.1 KiB
C++
/* Copyright 2019-2020 The MathWorks, Inc. */
|
|
|
|
#ifndef OCTOMAPCODEGEN_H_
|
|
#define OCTOMAPCODEGEN_H_
|
|
|
|
#ifdef BUILDING_LIBMWOCTOMAPCODEGEN
|
|
#include "octomapcodegen/octomapcodegen_util.hpp"
|
|
#else
|
|
#include "octomapcodegen_util.hpp"
|
|
#endif
|
|
|
|
#include <memory>
|
|
#include <fstream>
|
|
#include <string>
|
|
|
|
// Forward declaration
|
|
namespace octomap {
|
|
class OcTree;
|
|
}
|
|
|
|
namespace nav {
|
|
|
|
class octomapcodegen{
|
|
|
|
public:
|
|
|
|
octomapcodegen(real64_T resolution);
|
|
~octomapcodegen();
|
|
|
|
real64_T octomapGetResolution();
|
|
|
|
void octomapSetResolution(real64_T resolution);
|
|
|
|
uint32_T octomapGetNumLeafNodes();
|
|
|
|
void octomapGetMapDimensions(real64_T* dimensions);
|
|
|
|
void octomapUpdateNodeBoolean(real64_T* xyz, boolean_T occupied, boolean_T lazyEval);
|
|
|
|
void octomapUpdateNodeDouble(real64_T* xyz, real64_T probUpdate, boolean_T lazyEval);
|
|
|
|
void octomapSetNodeValue(real64_T* xyz, real64_T prob, boolean_T lazyEval);
|
|
|
|
void octomapGetOccupancy(real64_T* xyz, uint32_T nRows, real64_T* occupancyValues);
|
|
|
|
void octomapUpdateInternalOccupancy();
|
|
|
|
void octomapPrune();
|
|
|
|
void octomapToMaxLikelihood();
|
|
|
|
void octomapExtractVisualizationData(real64_T maxDepth, real64_T* outData);
|
|
|
|
uint32_T octomapMmemoryUsage();
|
|
|
|
void octomapInflate(real64_T radius, real64_T occThreshold);
|
|
|
|
void octomapSetClampingThreshold(real64_T clampingThresMin, real64_T clampingThresMax);
|
|
|
|
boolean_T octomapReadBinary(const char* filename);
|
|
|
|
void octomapWriteBinary(const char* filename);
|
|
|
|
boolean_T octomapRead(const char* filename);
|
|
|
|
void octomapWrite(const char* filename);
|
|
|
|
real64_T octomapGetProbHit();
|
|
|
|
real64_T octomapGetProbMiss();
|
|
|
|
uint32_T octomapGetSizeSerializationData();
|
|
|
|
void octomapSerialization(char* data);
|
|
|
|
boolean_T octomapDeserialization(char* data, uint32_T n);
|
|
|
|
void octomapDeserializationFullROSMsgData(real64_T resolution, char* data, uint32_T n);
|
|
|
|
void octomapDeserializationBinaryROSMsgData(real64_T resolution, char* data, uint32_T n);
|
|
|
|
void octomapGetRayIntersection(
|
|
real64_T* ptStart,
|
|
real64_T* ptDirections,
|
|
uint32_T nPtDirection,
|
|
real64_T occupiedThreshold,
|
|
boolean_T ignoreUnknownCells,
|
|
real64_T maxRange,
|
|
real64_T* outData);
|
|
|
|
bool insertPointCloudImpl_real64(real64_T* origin,
|
|
real64_T* points,
|
|
uint32_T nPoints,
|
|
real64_T maxRange,
|
|
boolean_T lazyEval,
|
|
boolean_T discretize);
|
|
|
|
bool insertPointCloudImpl_real32(real64_T* origin,
|
|
real32_T* points,
|
|
uint32_T nPoints,
|
|
real64_T maxRange,
|
|
boolean_T lazyEval,
|
|
boolean_T discretize);
|
|
|
|
private:
|
|
std::unique_ptr<octomap::OcTree> _map; ///< Octomap Map Object
|
|
|
|
/// Used to read from Char array
|
|
class OneShotReadBuf : public std::streambuf {
|
|
public:
|
|
OneShotReadBuf(char* s, uint32_T n) {
|
|
size_t n1 = static_cast<size_t>(n);
|
|
setg(s, s, s + n1);
|
|
}
|
|
};
|
|
|
|
// read from istream (binary)
|
|
bool readBinaryImpl(std::istream& buf);
|
|
|
|
// write to ostream (binary)
|
|
void writeBinaryImpl(std::ostream& buf);
|
|
|
|
// write to ostream
|
|
void writeImpl(std::ostream& buf);
|
|
|
|
// read from istream
|
|
bool readImpl(std::istream& buf);
|
|
|
|
template <typename TfloatPoint>
|
|
bool insertPointCloudImpl(double const* origin,
|
|
TfloatPoint const* points,
|
|
uint32_T rows,
|
|
double maxRange,
|
|
bool lazyEval,
|
|
bool discretize);
|
|
};
|
|
} // namespace nav
|
|
|
|
#endif /* OCTOMAPCODEGEN_H_ */
|