MRPT  2.0.4
List of all members | Classes | Public Types | Public Member Functions | Static Public Member Functions | Public Attributes | Protected Attributes | Private Member Functions | Private Attributes
mrpt::nav::CHolonomicND Class Reference

Detailed Description

An implementation of the holonomic reactive navigation method "Nearness-Diagram".

The algorithm "Nearness-Diagram" was proposed in:

Nearness diagram (ND) navigation: collision avoidance in troublesome scenarios, IEEE Transactions on Robotics and Automation, Minguez, J. and Montano, L., vol. 20, no. 1, pp. 45-59, 2004.

These are the optional parameters of the method which can be set by means of a configuration file passed to the constructor or to CHolonomicND::initialize() or directly in CHolonomicND::options

# Section name can be changed via setConfigFileSectionName()
[ND_CONFIG]
factorWeights=1.0 0.5 2.0 0.4
// 1: Free space
// 2: Dist. in sectors
// 3: Closer to target (euclidean)
// 4: Hysteresis
WIDE_GAP_SIZE_PERCENT = 0.25
MAX_SECTOR_DIST_FOR_D2_PERCENT = 0.25
RISK_EVALUATION_SECTORS_PERCENT = 0.25
RISK_EVALUATION_DISTANCE = 0.15 // In normalized ps-meters [0,1]
TARGET_SLOW_APPROACHING_DISTANCE = 0.60 // For stopping gradually
TOO_CLOSE_OBSTACLE = 0.02 // In normalized ps-meters
See also
CAbstractHolonomicReactiveMethod,CReactiveNavigationSystem

Definition at line 52 of file CHolonomicND.h.

#include <mrpt/nav/holonomic/CHolonomicND.h>

Inheritance diagram for mrpt::nav::CHolonomicND:

Classes

struct  TGap
 The structure used to store a detected gap in obstacles. More...
 
struct  TOptions
 Algorithm options. More...
 

Public Types

enum  TSituations { SITUATION_TARGET_DIRECTLY = 1, SITUATION_SMALL_GAP, SITUATION_WIDE_GAP, SITUATION_NO_WAY_FOUND }
 The set of posible situations for each trajectory. More...
 
using TGapArray = std::vector< TGap >
 

Public Member Functions

 CHolonomicND (const mrpt::config::CConfigFileBase *INI_FILE=nullptr)
 Initialize the parameters of the navigator, from some configuration file, or default values if set to nullptr. More...
 
void navigate (const NavInput &ni, NavOutput &no) override
 Invokes the holonomic navigation algorithm itself. More...
 
void initialize (const mrpt::config::CConfigFileBase &INI_FILE) override
 Initialize the parameters of the navigator. More...
 
void saveConfigFile (mrpt::config::CConfigFileBase &c) const override
 saves all available parameters, in a forma loadable by initialize() More...
 
double getTargetApproachSlowDownDistance () const override
 Returns the actual value of this parameter [m], as set via the children class options structure. More...
 
void setTargetApproachSlowDownDistance (const double dist) override
 Sets the actual value of this parameter [m]. More...
 
void setConfigFileSectionName (const std::string &sectName)
 Defines the name of the section used in initialize() More...
 
std::string getConfigFileSectionName () const
 Gets the name of the section used in initialize() More...
 
void setAssociatedPTG (mrpt::nav::CParameterizedTrajectoryGenerator *ptg)
 Optionally, sets the associated PTG, just in case a derived class requires this info (not required for methods where the robot kinematics are totally abstracted) More...
 
mrpt::nav::CParameterizedTrajectoryGeneratorgetAssociatedPTG () const
 Returns the pointer set by setAssociatedPTG() More...
 
void enableApproachTargetSlowDown (bool enable)
 
virtual mxArraywriteToMatlab () const
 Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class. More...
 
RTTI classes and functions for polymorphic hierarchies
mrpt::rtti::CObject::Ptr duplicateGetSmartPtr () const
 Makes a deep copy of the object and returns a smart pointer to it. More...
 

Static Public Member Functions

static CAbstractHolonomicReactiveMethod::Ptr Factory (const std::string &className) noexcept
 
static CAbstractHolonomicReactiveMethodCreate (const std::string &className) noexcept
 Class factory from class name, e.g. More...
 

Public Attributes

TOptions options
 Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options.loadFromConfigFile(), etc.) More...
 

Protected Member Functions

CSerializable virtual methods
uint8_t serializeGetVersion () const override
 Must return the current versioning number of the object. More...
 
void serializeTo (mrpt::serialization::CArchive &out) const override
 Pure virtual method for writing (serializing) to an abstract archive. More...
 
void serializeFrom (mrpt::serialization::CArchive &in, uint8_t serial_version) override
 Pure virtual method for reading (deserializing) from an abstract archive. More...
 
CSerializable virtual methods
virtual void serializeTo (CSchemeArchiveBase &out) const
 Virtual method for writing (serializing) to an abstract schema based archive. More...
 
virtual void serializeFrom (CSchemeArchiveBase &in)
 Virtual method for reading (deserializing) from an abstract schema based archive. More...
 

Protected Attributes

mrpt::nav::CParameterizedTrajectoryGeneratorm_associatedPTG
 If applicable, this will contain the argument of the most recent call to setAssociatedPTG() More...
 
bool m_enableApproachTargetSlowDown
 Whether to decrease speed when approaching target. More...
 

Private Member Functions

unsigned int direction2sector (const double a, const unsigned int N)
 
void gapsEstimator (const std::vector< double > &obstacles, const mrpt::math::TPoint2D &in_target, TGapArray &gaps)
 Find gaps in the obtacles. More...
 
void searchBestGap (const std::vector< double > &in_obstacles, const double in_maxObsRange, const TGapArray &in_gaps, const mrpt::math::TPoint2D &in_target, unsigned int &out_selDirection, double &out_selEvaluation, TSituations &out_situation, double &out_riskEvaluation, CLogFileRecord_ND &log)
 Search the best gap. More...
 
void calcRepresentativeSectorForGap (TGap &gap, const mrpt::math::TPoint2D &target, const std::vector< double > &obstacles)
 Fills in the representative sector field in the gap structure: More...
 
void evaluateGaps (const std::vector< double > &in_obstacles, const double in_maxObsRange, const TGapArray &in_gaps, const unsigned int TargetSector, const float TargetDist, std::vector< double > &out_gaps_evaluation)
 Evaluate each gap: More...
 

Private Attributes

unsigned int m_last_selected_sector
 

RTTI stuff

using Ptr = std::shared_ptr< mrpt::nav ::CHolonomicND >
 
using ConstPtr = std::shared_ptr< const mrpt::nav ::CHolonomicND >
 
using UniquePtr = std::unique_ptr< mrpt::nav ::CHolonomicND >
 
using ConstUniquePtr = std::unique_ptr< const mrpt::nav ::CHolonomicND >
 
static const mrpt::rtti::TRuntimeClassId runtimeClassId
 
static constexpr const char * className = "mrpt::nav" "::" "CHolonomicND"
 
static const mrpt::rtti::TRuntimeClassId_GetBaseClass ()
 
static constexpr auto getClassName ()
 
static const mrpt::rtti::TRuntimeClassIdGetRuntimeClassIdStatic ()
 
static std::shared_ptr< CObjectCreateObject ()
 
template<typename... Args>
static Ptr Create (Args &&... args)
 
template<typename Alloc , typename... Args>
static Ptr CreateAlloc (const Alloc &alloc, Args &&... args)
 
template<typename... Args>
static UniquePtr CreateUnique (Args &&... args)
 
virtual const mrpt::rtti::TRuntimeClassIdGetRuntimeClass () const override
 Returns information about the class of an object in runtime. More...
 
virtual mrpt::rtti::CObjectclone () const override
 Returns a deep copy (clone) of the object, indepently of its class. More...
 

Member Typedef Documentation

◆ ConstPtr

Definition at line 54 of file CHolonomicND.h.

◆ ConstUniquePtr

using mrpt::nav::CHolonomicND::ConstUniquePtr = std::unique_ptr<const mrpt::nav :: CHolonomicND >

Definition at line 54 of file CHolonomicND.h.

◆ Ptr

A type for the associated smart pointer

Definition at line 54 of file CHolonomicND.h.

◆ TGapArray

Definition at line 73 of file CHolonomicND.h.

◆ UniquePtr

using mrpt::nav::CHolonomicND::UniquePtr = std::unique_ptr< mrpt::nav :: CHolonomicND >

Definition at line 54 of file CHolonomicND.h.

Member Enumeration Documentation

◆ TSituations

The set of posible situations for each trajectory.

(mrpt::typemeta::TEnumType works with this enum)

Enumerator
SITUATION_TARGET_DIRECTLY 
SITUATION_SMALL_GAP 
SITUATION_WIDE_GAP 
SITUATION_NO_WAY_FOUND 

Definition at line 77 of file CHolonomicND.h.

Constructor & Destructor Documentation

◆ CHolonomicND()

mrpt::nav::CHolonomicND::CHolonomicND ( const mrpt::config::CConfigFileBase INI_FILE = nullptr)

Initialize the parameters of the navigator, from some configuration file, or default values if set to nullptr.

Member Function Documentation

◆ _GetBaseClass()

static const mrpt::rtti::TRuntimeClassId* mrpt::nav::CHolonomicND::_GetBaseClass ( )
staticprotected

◆ calcRepresentativeSectorForGap()

void CHolonomicND::calcRepresentativeSectorForGap ( TGap gap,
const mrpt::math::TPoint2D target,
const std::vector< double > &  obstacles 
)
private

Fills in the representative sector field in the gap structure:

Definition at line 445 of file CHolonomicND.cpp.

References mrpt::abs_diff(), dir, mrpt::nav::CHolonomicND::TGap::end, mrpt::nav::CHolonomicND::TGap::ini, mrpt::keep_max(), mrpt::keep_min(), mrpt::nav::CHolonomicND::TGap::representative_sector, mrpt::round(), mrpt::math::TPoint2D_data< T >::x, and mrpt::math::TPoint2D_data< T >::y.

Here is the call graph for this function:

◆ clone()

virtual mrpt::rtti::CObject* mrpt::nav::CHolonomicND::clone ( ) const
overridevirtual

Returns a deep copy (clone) of the object, indepently of its class.

Implements mrpt::rtti::CObject.

◆ Create() [1/2]

template<typename... Args>
static Ptr mrpt::nav::CHolonomicND::Create ( Args &&...  args)
inlinestatic

Definition at line 54 of file CHolonomicND.h.

◆ Create() [2/2]

static CAbstractHolonomicReactiveMethod* mrpt::nav::CAbstractHolonomicReactiveMethod::Create ( const std::string &  className)
staticnoexceptinherited

Class factory from class name, e.g.

"CHolonomicVFF", etc.

Exceptions
std::logic_errorOn invalid or missing parameters.

◆ CreateAlloc()

template<typename Alloc , typename... Args>
static Ptr mrpt::nav::CHolonomicND::CreateAlloc ( const Alloc &  alloc,
Args &&...  args 
)
inlinestatic

Definition at line 54 of file CHolonomicND.h.

◆ CreateObject()

static std::shared_ptr<CObject> mrpt::nav::CHolonomicND::CreateObject ( )
static

◆ CreateUnique()

template<typename... Args>
static UniquePtr mrpt::nav::CHolonomicND::CreateUnique ( Args &&...  args)
inlinestatic

Definition at line 54 of file CHolonomicND.h.

◆ direction2sector()

unsigned int CHolonomicND::direction2sector ( const double  a,
const unsigned int  N 
)
private

Definition at line 608 of file CHolonomicND.cpp.

References M_PI, mrpt::round(), and mrpt::math::wrapToPi().

Here is the call graph for this function:

◆ duplicateGetSmartPtr()

mrpt::rtti::CObject::Ptr CObject::duplicateGetSmartPtr ( ) const
inlineinherited

Makes a deep copy of the object and returns a smart pointer to it.

Definition at line 204 of file CObject.h.

References mrpt::rtti::CObject::clone().

Referenced by mrpt::obs::CRawlog::insert().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ enableApproachTargetSlowDown()

void mrpt::nav::CAbstractHolonomicReactiveMethod::enableApproachTargetSlowDown ( bool  enable)
inlineinherited

Definition at line 119 of file CAbstractHolonomicReactiveMethod.h.

References mrpt::nav::CAbstractHolonomicReactiveMethod::m_enableApproachTargetSlowDown.

Referenced by mrpt::nav::CAbstractPTGBasedReactive::build_movement_candidate().

Here is the caller graph for this function:

◆ evaluateGaps()

void CHolonomicND::evaluateGaps ( const std::vector< double > &  in_obstacles,
const double  in_maxObsRange,
const TGapArray in_gaps,
const unsigned int  TargetSector,
const float  TargetDist,
std::vector< double > &  out_gaps_evaluation 
)
private

Evaluate each gap:

Definition at line 508 of file CHolonomicND.cpp.

References mrpt::abs_diff(), ASSERT_, mrpt::nav::CHolonomicND::TGap::end, mrpt::containers::end(), mrpt::nav::CHolonomicND::TGap::ini, mrpt::min3(), mrpt::math::minimumDistanceFromPointToSegment(), mrpt::nav::CHolonomicND::TGap::representative_sector, mrpt::square(), and mrpt::math::sum().

Here is the call graph for this function:

◆ Factory()

CAbstractHolonomicReactiveMethod::Ptr CAbstractHolonomicReactiveMethod::Factory ( const std::string &  className)
staticnoexceptinherited

Definition at line 51 of file CAbstractHolonomicReactiveMethod.cpp.

References mrpt::rtti::TRuntimeClassId::createObject(), mrpt::rtti::findRegisteredClass(), mrpt::ptr_cast< CAST_TO >::from(), and mrpt::rtti::registerAllPendingClasses().

Referenced by mrpt::nav::CAbstractPTGBasedReactive::setHolonomicMethod().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ gapsEstimator()

void CHolonomicND::gapsEstimator ( const std::vector< double > &  obstacles,
const mrpt::math::TPoint2D in_target,
TGapArray gaps 
)
private

Find gaps in the obtacles.

Definition at line 131 of file CHolonomicND.cpp.

References ASSERT_, mrpt::nav::CHolonomicND::TGap::end, mrpt::containers::end(), mrpt::nav::CHolonomicND::TGap::ini, mrpt::keep_max(), mrpt::keep_min(), mrpt::nav::CHolonomicND::TGap::maxDistance, mrpt::nav::CHolonomicND::TGap::minDistance, and mrpt::math::TPoint2D_< T >::norm().

Here is the call graph for this function:

◆ getAssociatedPTG()

mrpt::nav::CParameterizedTrajectoryGenerator * CAbstractHolonomicReactiveMethod::getAssociatedPTG ( ) const
inherited

◆ getClassName()

static constexpr auto mrpt::nav::CHolonomicND::getClassName ( )
inlinestatic

Definition at line 54 of file CHolonomicND.h.

◆ getConfigFileSectionName()

std::string CAbstractHolonomicReactiveMethod::getConfigFileSectionName ( ) const
inherited

Gets the name of the section used in initialize()

Definition at line 35 of file CAbstractHolonomicReactiveMethod.cpp.

References mrpt::nav::CAbstractHolonomicReactiveMethod::m_cfgSectionName.

◆ GetRuntimeClass()

virtual const mrpt::rtti::TRuntimeClassId* mrpt::nav::CHolonomicND::GetRuntimeClass ( ) const
overridevirtual

Returns information about the class of an object in runtime.

Reimplemented from mrpt::nav::CAbstractHolonomicReactiveMethod.

◆ GetRuntimeClassIdStatic()

static const mrpt::rtti::TRuntimeClassId& mrpt::nav::CHolonomicND::GetRuntimeClassIdStatic ( )
static

◆ getTargetApproachSlowDownDistance()

double mrpt::nav::CHolonomicND::getTargetApproachSlowDownDistance ( ) const
inlineoverridevirtual

Returns the actual value of this parameter [m], as set via the children class options structure.

See also
setTargetApproachSlowDownDistance()

Implements mrpt::nav::CAbstractHolonomicReactiveMethod.

Definition at line 115 of file CHolonomicND.h.

References options, and mrpt::nav::CHolonomicND::TOptions::TARGET_SLOW_APPROACHING_DISTANCE.

◆ initialize()

void CHolonomicND::initialize ( const mrpt::config::CConfigFileBase INI_FILE)
overridevirtual

Initialize the parameters of the navigator.

Implements mrpt::nav::CAbstractHolonomicReactiveMethod.

Definition at line 37 of file CHolonomicND.cpp.

◆ navigate()

void CHolonomicND::navigate ( const NavInput ni,
NavOutput no 
)
overridevirtual

◆ saveConfigFile()

void CHolonomicND::saveConfigFile ( mrpt::config::CConfigFileBase c) const
overridevirtual

saves all available parameters, in a forma loadable by initialize()

Implements mrpt::nav::CAbstractHolonomicReactiveMethod.

Definition at line 41 of file CHolonomicND.cpp.

◆ searchBestGap()

void CHolonomicND::searchBestGap ( const std::vector< double > &  in_obstacles,
const double  in_maxObsRange,
const TGapArray in_gaps,
const mrpt::math::TPoint2D in_target,
unsigned int &  out_selDirection,
double &  out_selEvaluation,
TSituations out_situation,
double &  out_riskEvaluation,
CLogFileRecord_ND log 
)
private

Search the best gap.

Definition at line 294 of file CHolonomicND.cpp.

References mrpt::nav::CHolonomicND::TGap::end, mrpt::nav::CLogFileRecord_ND::gaps_eval, mrpt::nav::CHolonomicND::TGap::ini, mrpt::math::TPoint2D_< T >::norm(), mrpt::round(), mrpt::math::TPoint2D_data< T >::x, and mrpt::math::TPoint2D_data< T >::y.

Here is the call graph for this function:

◆ serializeFrom() [1/2]

void CHolonomicND::serializeFrom ( mrpt::serialization::CArchive in,
uint8_t  serial_version 
)
overrideprotectedvirtual

Pure virtual method for reading (deserializing) from an abstract archive.

Users don't call this method directly. Instead, use stream >> object;.

Parameters
inThe input binary stream where the object data must read from.
versionThe version of the object stored in the stream: use this version number in your code to know how to read the incoming data.
Exceptions
std::exceptionOn any I/O error

Implements mrpt::serialization::CSerializable.

Definition at line 731 of file CHolonomicND.cpp.

References mrpt::nav::CHolonomicND::TOptions::factorWeights, m_last_selected_sector, mrpt::nav::CHolonomicND::TOptions::MAX_SECTOR_DIST_FOR_D2_PERCENT, MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION, options, mrpt::nav::CHolonomicND::TOptions::RISK_EVALUATION_DISTANCE, mrpt::nav::CHolonomicND::TOptions::RISK_EVALUATION_SECTORS_PERCENT, mrpt::nav::CHolonomicND::TOptions::TARGET_SLOW_APPROACHING_DISTANCE, mrpt::nav::CHolonomicND::TOptions::TOO_CLOSE_OBSTACLE, and mrpt::nav::CHolonomicND::TOptions::WIDE_GAP_SIZE_PERCENT.

◆ serializeFrom() [2/2]

virtual void mrpt::serialization::CSerializable::serializeFrom ( CSchemeArchiveBase in)
inlineprotectedvirtualinherited

Virtual method for reading (deserializing) from an abstract schema based archive.

Definition at line 74 of file CSerializable.h.

References mrpt::serialization::CSerializable::GetRuntimeClass(), and THROW_EXCEPTION.

Here is the call graph for this function:

◆ serializeGetVersion()

uint8_t CHolonomicND::serializeGetVersion ( ) const
overrideprotectedvirtual

Must return the current versioning number of the object.

Start in zero for new classes, and increments each time there is a change in the stored format.

Implements mrpt::serialization::CSerializable.

Definition at line 719 of file CHolonomicND.cpp.

◆ serializeTo() [1/2]

void CHolonomicND::serializeTo ( mrpt::serialization::CArchive out) const
overrideprotectedvirtual

◆ serializeTo() [2/2]

virtual void mrpt::serialization::CSerializable::serializeTo ( CSchemeArchiveBase out) const
inlineprotectedvirtualinherited

Virtual method for writing (serializing) to an abstract schema based archive.

Definition at line 64 of file CSerializable.h.

References mrpt::serialization::CSerializable::GetRuntimeClass(), and THROW_EXCEPTION.

Here is the call graph for this function:

◆ setAssociatedPTG()

void CAbstractHolonomicReactiveMethod::setAssociatedPTG ( mrpt::nav::CParameterizedTrajectoryGenerator ptg)
inherited

Optionally, sets the associated PTG, just in case a derived class requires this info (not required for methods where the robot kinematics are totally abstracted)

Definition at line 40 of file CAbstractHolonomicReactiveMethod.cpp.

References mrpt::nav::CAbstractHolonomicReactiveMethod::m_associatedPTG.

◆ setConfigFileSectionName()

void CAbstractHolonomicReactiveMethod::setConfigFileSectionName ( const std::string &  sectName)
inherited

Defines the name of the section used in initialize()

Defines the name of the section (Default: "FULL_EVAL_CONFIG")

Definition at line 30 of file CAbstractHolonomicReactiveMethod.cpp.

References mrpt::nav::CAbstractHolonomicReactiveMethod::m_cfgSectionName.

◆ setTargetApproachSlowDownDistance()

void mrpt::nav::CHolonomicND::setTargetApproachSlowDownDistance ( const double  dist)
inlineoverridevirtual

Sets the actual value of this parameter [m].

See also
getTargetApproachSlowDownDistance()

Implements mrpt::nav::CAbstractHolonomicReactiveMethod.

Definition at line 119 of file CHolonomicND.h.

References options, and mrpt::nav::CHolonomicND::TOptions::TARGET_SLOW_APPROACHING_DISTANCE.

◆ writeToMatlab()

virtual mxArray* mrpt::serialization::CSerializable::writeToMatlab ( ) const
inlinevirtualinherited

Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class.

Returns
A new mxArray (caller is responsible of memory freeing) or nullptr is class does not support conversion to MATLAB.

Definition at line 90 of file CSerializable.h.

Member Data Documentation

◆ className

constexpr const char* mrpt::nav::CHolonomicND::className = "mrpt::nav" "::" "CHolonomicND"
static

Definition at line 54 of file CHolonomicND.h.

◆ m_associatedPTG

mrpt::nav::CParameterizedTrajectoryGenerator* mrpt::nav::CAbstractHolonomicReactiveMethod::m_associatedPTG
protectedinherited

If applicable, this will contain the argument of the most recent call to setAssociatedPTG()

Definition at line 127 of file CAbstractHolonomicReactiveMethod.h.

Referenced by mrpt::nav::CAbstractHolonomicReactiveMethod::getAssociatedPTG(), and mrpt::nav::CAbstractHolonomicReactiveMethod::setAssociatedPTG().

◆ m_enableApproachTargetSlowDown

bool mrpt::nav::CAbstractHolonomicReactiveMethod::m_enableApproachTargetSlowDown
protectedinherited

Whether to decrease speed when approaching target.

Definition at line 129 of file CAbstractHolonomicReactiveMethod.h.

Referenced by mrpt::nav::CAbstractHolonomicReactiveMethod::enableApproachTargetSlowDown().

◆ m_last_selected_sector

unsigned int mrpt::nav::CHolonomicND::m_last_selected_sector
private

Definition at line 125 of file CHolonomicND.h.

Referenced by serializeFrom(), and serializeTo().

◆ options

TOptions mrpt::nav::CHolonomicND::options

Parameters of the algorithm (can be set manually or loaded from CHolonomicND::initialize or options.loadFromConfigFile(), etc.)

Definition at line 113 of file CHolonomicND.h.

Referenced by getTargetApproachSlowDownDistance(), serializeFrom(), serializeTo(), and setTargetApproachSlowDownDistance().

◆ runtimeClassId

const mrpt::rtti::TRuntimeClassId mrpt::nav::CHolonomicND::runtimeClassId
staticprotected

Definition at line 54 of file CHolonomicND.h.




Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020