WorkerNode Class Reference

Implements the worker side of the batch distributed adaptive dynamic domain informed rrt* planner. More...

#include <RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h>

+ Inheritance diagram for WorkerNode:

Public Types

using ConfigType = Tree::ConfigType
 Type of a configuration. More...
 
- Public Types inherited from ManagedIceObject
using PeriodicTaskPtr = IceUtil::Handle< SimplePeriodicTask< std::function< void(void)> >>
 

Public Member Functions

std::string getDefaultName () const override
 
Update getUpdate (Ice::Long updateId, const Ice::Current &=Ice::emptyCurrent) const override
 
void ice_postUnmarshal () override
 noop More...
 
void killWorker (const Ice::Current &=Ice::emptyCurrent) override
 Signals the worker thread to exit. More...
 
void onConnectComponent () override
 Gets a proxy to the topic. More...
 
void onDisconnectComponent () override
 noop More...
 
void onExitComponent () override
 Kills the worker thread (if it is still running) and joins it. More...
 
void onInitComponent () override
 Initializes the worker and starts the worker thread. More...
 
void pauseWorker (bool pauseFlag, const Ice::Current &=Ice::emptyCurrent) override
 
void updateTree (const Update &u, const Ice::Current &=Ice::emptyCurrent) override
 Receives an update from other workers and adds it to the queue of pending updates. More...
 
 WorkerNode (const CSpaceBasePtr &cspace, const VectorXf &startCfg, const VectorXf &goalCfg, Ice::Float DCDStepSize, AdaptiveDynamicDomainParameters addParams, const ManagerNodeBasePrx &manager, Ice::Long workerId, Ice::Long batchSize, Ice::Long nodeCountDeltaForGoalConnectionTries, const std::string &topicPrefix, const Ice::FloatSeq &rotationMatrix)
 ctor More...
 
 ~WorkerNode () override
 dtor. More...
 
- Public Member Functions inherited from ManagedIceObject
void enableProfiler (bool enable)
 setProfiler allows setting ManagedIceObject::profiler to a new instance (if the new instance is actually not a null pointer) More...
 
std::string generateSubObjectName (const std::string &subObjectName)
 Generates a unique name for a sub object from a general name. More...
 
ArmarXManagerPtr getArmarXManager () const
 Returns the ArmarX manager used to add and remove components. More...
 
ManagedIceObjectConnectivity getConnectivity () const
 Retrieve connectivity of the object (topcis as well as proxies) More...
 
IceManagerPtr getIceManager () const
 Returns the IceManager. More...
 
VariantBasePtr getMetaInfo (const std::string &id)
 
StringVariantBaseMap getMetaInfoMap () const
 
std::string getName () const
 Retrieve name of object. More...
 
Ice::ObjectAdapterPtr getObjectAdapter () const
 Returns object's Ice adapter. More...
 
ArmarXObjectSchedulerPtr getObjectScheduler () const
 
PeriodicTaskPtr getPeriodicTask (const std::string &name)
 
Profiler::ProfilerPtr getProfiler () const
 getProfiler returns an instance of armarx::Profiler More...
 
template<class ProxyTarg , class... Args>
void getProxy (const char *name, IceInternal::ProxyHandle< ProxyTarg > &proxy, Args &&...args)
 
template<class ProxyType >
ProxyType getProxy (const std::string &name, bool addToDependencies=false, const std::string &endpoints="", bool throwOnProxyError=true)
 Retrieves a proxy object. More...
 
template<class ProxyTarg , class... Args>
void getProxy (const std::string &name, IceInternal::ProxyHandle< ProxyTarg > &proxy, Args &&...args)
 
template<class ProxyTarg , class... Args>
void getProxy (IceInternal::ProxyHandle< ProxyTarg > &proxy, const char *name, Args &&...args)
 
template<class ProxyTarg , class... Args>
void getProxy (IceInternal::ProxyHandle< ProxyTarg > &proxy, const std::string &name, Args &&...args)
 Assigns a proxy to proxy. More...
 
Ice::ObjectPrx getProxy (long timeoutMs=0, bool waitForScheduler=true) const
 Returns the proxy of this object (optionally it waits for the proxy) More...
 
template<class Prx >
Prx getProxy (long timeoutMs=0, bool waitForScheduler=true) const
 
template<class ProxyType >
void getProxy (ProxyType &proxy, const char *name, bool addToDependencies=false, const std::string &endpoints="", bool throwOnProxyError=true)
 Overload to allow using string literals as name (solve ambiguous overload). More...
 
template<class Prx >
void getProxy (Prx &prx, long timeoutMs=0, bool waitForScheduler=true) const
 
int getState () const
 Retrieve current state of the ManagedIceObject. More...
 
template<class TopicProxyType >
TopicProxyType getTopic (const std::string &name)
 Returns a proxy of the specified topic. More...
 
template<class TopicProxyType >
void getTopic (TopicProxyType &topicProxy, const std::string &name)
 Assigns a proxy of the specified topic to topicProxy. More...
 
std::vector< std::string > getUnresolvedDependencies () const
 returns the names of all unresolved dependencies More...
 
 ManagedIceObject (ManagedIceObject const &other)
 
void offeringTopic (const std::string &name)
 Registers a topic for retrival after initialization. More...
 
void preambleGetTopic (std::string const &name)
 
void setMetaInfo (const std::string &id, const VariantBasePtr &value)
 Allows to set meta information that can be queried live via Ice interface on the ArmarXManager. More...
 
void startPeriodicTask (const std::string &uniqueName, std::function< void(void)> f, int periodMs, bool assureMeanInterval=false, bool forceSystemTime=true)
 
bool stopPeriodicTask (const std::string &name)
 
bool unsubscribeFromTopic (const std::string &name)
 Unsubscribe from a topic. More...
 
bool usingProxy (const std::string &name, const std::string &endpoints="")
 Registers a proxy for retrieval after initialization and adds it to the dependency list. More...
 
void usingTopic (const std::string &name, bool orderedPublishing=false)
 Registers a proxy for subscription after initialization. More...
 
void waitForObjectScheduler ()
 Waits until the ObjectScheduler could resolve all dependencies. More...
 
void waitForProxy (std::string const &name, bool addToDependencies)
 
- Public Member Functions inherited from Logging
SpamFilterDataPtr deactivateSpam (float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
 disables the logging for the current line for the given amount of seconds. More...
 
MessageTypeT getEffectiveLoggingLevel () const
 
 Logging ()
 
void setLocalMinimumLoggingLevel (MessageTypeT level)
 With setLocalMinimumLoggingLevel the minimum verbosity-level of log-messages can be set. More...
 
void setTag (const LogTag &tag)
 
void setTag (const std::string &tagName)
 
virtual ~Logging ()
 

Static Public Member Functions

static float distance (const ConfigType &from, const ConfigType &to)
 Calculates the euclidian distance between from and to. More...
 
- Static Public Member Functions inherited from ManagedIceObject
static std::string generateSubObjectName (const std::string &superObjectName, const std::string &subObjectName)
 Generates a unique name for a sub object from a general name and unique name. More...
 
static std::string GetObjectStateAsString (int state)
 

Protected Member Functions

void addAndRewireConfig (const ConfigType &cfgReached, const NodeId &nodeNearestId)
 Adds a configuration to the tree. More...
 
void applyPendingUpdates ()
 Applies all pending updates to the tree. More...
 
void doBatch ()
 Executes a batch. More...
 
void doBatchIteration ()
 Executes one iteration of a batch. More...
 
void findBestParent (const NodeId &nodeNearestId, const ConfigType &cfgReached, NodeId &outNodeBestId, float &outCostReachedToNodeBest, std::vector< std::pair< NodeId, float >> &outKNearestIdsAndDistances, std::map< NodeId, bool > &outIsCollisionFreeCache)
 Searches the optimal parent for a configuration. More...
 
std::size_t getDimensionality ()
 
std::size_t getK ()
 
Update getRemoteUpdate (Ice::Long workerNodeId, Ice::Long updateSubId)
 
void initTree ()
 initializes the tree by getting the current tree from the manager More...
 
bool isCollisionFree (const ConfigType &cfg)
 
bool isPathCollisionFree (const ConfigType &from, const ConfigType &to)
 
void rewire (const NodeId &nodeId, const std::vector< std::pair< NodeId, float >> &kNearestIdsAndDistances, const std::map< NodeId, bool > &isCollisionFreeCache)
 The rewire operation of the rrt* algorithm. More...
 
ConfigType steer (const ConfigType &from, const ConfigType &to)
 Does a linear interpolation from from to to and checks the path per DCD. More...
 
 WorkerNode ()=default
 Only used when transmitting through ice. More...
 
void workerTask ()
 The worker task. More...
 
- Protected Member Functions inherited from ManagedIceObject
template<class PluginT , class... ParamsT>
PluginT * addPlugin (const std::string prefix="", ParamsT &&...params)
 
template<class PluginT , class... ParamsT>
void addPlugin (PluginT *&targ, const std::string prefix="", ParamsT &&...params)
 
template<class PluginT , class... ParamsT>
void addPlugin (std::experimental::observer_ptr< PluginT > &targ, const std::string prefix="", ParamsT &&...params)
 
Ice::CommunicatorPtr getCommunicator () const
 
std::unique_ptr< ManagedIceObjectPlugin > & getPluginPointer (std::type_info const &type, std::string const &prefix)
 
 ManagedIceObject ()
 Protected default constructor. More...
 
virtual void postOnConnectComponent ()
 
virtual void postOnDisconnectComponent ()
 
virtual void postOnExitComponent ()
 
virtual void postOnInitComponent ()
 
virtual void preOnConnectComponent ()
 
virtual void preOnDisconnectComponent ()
 
virtual void preOnExitComponent ()
 
virtual void preOnInitComponent ()
 
bool removeProxyDependency (const std::string &name)
 This function removes the dependency of this object on the in parameter name specified object. More...
 
void setName (std::string name)
 Override name of well-known object. More...
 
void terminate ()
 Initiates termination of this IceManagedObject. More...
 
 ~ManagedIceObject () override
 
- Protected Member Functions inherited from Logging
bool checkLogLevel (MessageTypeT level) const
 
const LogSenderPtrgetLogSender () const
 Retrieve log sender. More...
 
LogSenderPtr loghelper (const char *file, int line, const char *function) const
 

Protected Attributes

TreeUpdateInterfacePrx globalWorkers
 Proxy used to write to the update topic. More...
 
std::atomic_bool killRequest
 Wheether the node should shut down. More...
 
std::size_t onePastLastGoalConnect
 The index one past the last node that was considered to be connected to the goal configuration. More...
 
std::unique_ptr< InformedSamplersampler
 Stores the sampler. More...
 
Tree tree
 The RRT tree. More...
 
std::mutex updateMutex
 Mutex protecting the update structures. More...
 
std::string updateTopicName
 The update topic's name. More...
 
std::string workerIdSring
 A string representation of the worker id. More...
 
std::atomic_bool workerPaused
 workerPaused is true if the worker is currently on hold and does not perform any calculations. More...
 
std::thread workerThread
 Thread executing the worker task. More...
 
- Protected Attributes inherited from Logging
MessageTypeT minimumLoggingLevel
 
SpamFilterDataPtr spamFilter
 
LogTag tag
 

Friends

class GenericFactory< WorkerNodeBase, WorkerNode >
 
class ManagedIceObject
 required for static factory methode create More...
 

Additional Inherited Members

- Static Public Attributes inherited from ManagedIceObject
static const ManagedIceObjectPtr NullPtr
 A nullptr to be used when a const ref to a nullptr is required. More...
 

Detailed Description

Implements the worker side of the batch distributed adaptive dynamic domain informed rrt* planner.

This algorithm has two variants for selecting nodes for rewiring. (see doi> 10.1177/0278364911406761) One using k-Nearest-Neighbours with:

  • k(#nodes)= kRRT * log(#nodes)
  • kRRT > 2^(dimensions+1) * e (1 + 1/dimensions)

One using Nearest-Neighbours with a radius:

  • r(#nodes)= min{γRRT( log(#nodes) / #nodes )^(1/dimensions) , η}
  • γRRT =(2 ( 1 + 1/dimensions))^(1/d) *(μ(Xfree) / ζd)^(1/dimensions)
  • η = maximal step size during 1 extend (+inf in this implementation)
  • μ(Xfree) = Lebesgue measure of the obstacle-free space
  • ζd = volume of the d-dimensional unit ball in Euclidean space

since μ(Xfree) can only be estimated the k-NN version is used.

Definition at line 73 of file WorkerNode.h.

Member Typedef Documentation

◆ ConfigType

Type of a configuration.

Definition at line 81 of file WorkerNode.h.

Constructor & Destructor Documentation

◆ WorkerNode() [1/2]

WorkerNode ( const CSpaceBasePtr &  cspace,
const VectorXf &  startCfg,
const VectorXf &  goalCfg,
Ice::Float  DCDStepSize,
AdaptiveDynamicDomainParameters  addParams,
const ManagerNodeBasePrx &  manager,
Ice::Long  workerId,
Ice::Long  batchSize,
Ice::Long  nodeCountDeltaForGoalConnectionTries,
const std::string &  topicPrefix,
const Ice::FloatSeq &  rotationMatrix 
)
inline

ctor

Parameters
cspaceThe palnning cspace.
startCfgThe start configuration.
goalCfgThe goal configuration.
DCDStepSizeThe dcd step size.
addParamsThe adaptive dynamic domain parameters.
managerA proxy to the manager node.
workerIdThe worker's id.
batchSizeThe batch size.
nodeCountDeltaForGoalConnectionTriesNumber of nodes created (by a worker) before a connect to the goal node is tried (by this worker).
topicPrefixThe used topic prefix. (for the topic name to distribute updates)
rotationMatrixThe rotation matrix for the informed ellipsoid.

Definition at line 99 of file WorkerNode.h.

◆ ~WorkerNode()

~WorkerNode ( )
inlineoverride

dtor.

Asserts the worker thread was joined.

Definition at line 128 of file WorkerNode.h.

◆ WorkerNode() [2/2]

WorkerNode ( )
protecteddefault

Only used when transmitting through ice.

Member Function Documentation

◆ addAndRewireConfig()

void addAndRewireConfig ( const ConfigType cfgReached,
const NodeId &  nodeNearestId 
)
protected

Adds a configuration to the tree.

The configurations best parent is searched and rewiring is performed fot it.

Parameters
cfgReachedThe configuration.
nodeNearestIdThe nearest node's id.

Definition at line 272 of file WorkerNode.cpp.

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

◆ applyPendingUpdates()

void applyPendingUpdates ( )
protected

Applies all pending updates to the tree.

Definition at line 204 of file WorkerNode.cpp.

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

◆ distance()

static float distance ( const ConfigType from,
const ConfigType to 
)
inlinestatic

Calculates the euclidian distance between from and to.

Parameters
fromFirst vector.
toSecond vector.
Returns
The euclidian distance between from and to.

Definition at line 203 of file WorkerNode.h.

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

◆ doBatch()

void doBatch ( )
protected

Executes a batch.

Definition at line 131 of file WorkerNode.cpp.

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

◆ doBatchIteration()

void doBatchIteration ( )
protected

Executes one iteration of a batch.

Definition at line 218 of file WorkerNode.cpp.

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

◆ findBestParent()

void findBestParent ( const NodeId &  nodeNearestId,
const ConfigType cfgReached,
NodeId &  outNodeBestId,
float outCostReachedToNodeBest,
std::vector< std::pair< NodeId, float >> &  outKNearestIdsAndDistances,
std::map< NodeId, bool > &  outIsCollisionFreeCache 
)
protected

Searches the optimal parent for a configuration.

Used by the rrt* component.

Parameters
nodeNearestIdThe id of the nearest node.
cfgReachedThe reached configuration.
outNodeBestIdOutput parameter: The id of the optimal parent.
outCostReachedToNodeBestOutput parameter: The distance of reached and the optimal parent.
outKNearestIdsAndDistancesOutput parameter: The k nearest nodes and their distance to reached.
outIsCollisionFreeCacheOutput parameter: Cache storing collision checks for paths between nodes from knearest and reached.

Definition at line 339 of file WorkerNode.cpp.

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

◆ getDefaultName()

std::string getDefaultName ( ) const
inlineoverridevirtual
Returns
The component's default name.

Implements ManagedIceObject.

Definition at line 156 of file WorkerNode.h.

◆ getDimensionality()

std::size_t getDimensionality ( )
inlineprotected
Returns
The cspace's dimensionality

Definition at line 267 of file WorkerNode.h.

+ Here is the caller graph for this function:

◆ getK()

std::size_t getK ( )
protected
Returns
Number of neighbours currently considered as parent.

Definition at line 409 of file WorkerNode.cpp.

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

◆ getRemoteUpdate()

Update getRemoteUpdate ( Ice::Long  workerNodeId,
Ice::Long  updateSubId 
)
protected
Parameters
workerNodeIdThe update's worker id.
updateSubIdThe update's sub id.
Returns
The update fetched from the manager node.

Definition at line 425 of file WorkerNode.cpp.

+ Here is the caller graph for this function:

◆ getUpdate()

Update getUpdate ( Ice::Long  updateId,
const Ice::Current &  = Ice::emptyCurrent 
) const
override
Parameters
updateIdThe updates id.
Returns
This workers update with the given id.

Definition at line 417 of file WorkerNode.cpp.

+ Here is the call graph for this function:

◆ ice_postUnmarshal()

void ice_postUnmarshal ( )
inlineoverride

noop

Definition at line 193 of file WorkerNode.h.

◆ initTree()

void initTree ( )
inlineprotected

initializes the tree by getting the current tree from the manager

Definition at line 233 of file WorkerNode.h.

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

◆ isCollisionFree()

bool isCollisionFree ( const ConfigType cfg)
protected
Parameters
cfgThe configuration.
Returns
Whether the given configuration is collision free.

Definition at line 402 of file WorkerNode.cpp.

+ Here is the caller graph for this function:

◆ isPathCollisionFree()

bool isPathCollisionFree ( const ConfigType from,
const ConfigType to 
)
protected
Parameters
fromThe start configuration.
toThe goal configuration.
Returns
Whether the path from from to to is colliswion free.

Definition at line 393 of file WorkerNode.cpp.

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

◆ killWorker()

void killWorker ( const Ice::Current &  = Ice::emptyCurrent)
inlineoverride

Signals the worker thread to exit.

Definition at line 165 of file WorkerNode.h.

+ Here is the caller graph for this function:

◆ onConnectComponent()

void onConnectComponent ( )
overridevirtual

Gets a proxy to the topic.

Implements ManagedIceObject.

Definition at line 54 of file WorkerNode.cpp.

◆ onDisconnectComponent()

void onDisconnectComponent ( )
inlineoverridevirtual

noop

Reimplemented from ManagedIceObject.

Definition at line 148 of file WorkerNode.h.

◆ onExitComponent()

void onExitComponent ( )
overridevirtual

Kills the worker thread (if it is still running) and joins it.

Reimplemented from ManagedIceObject.

Definition at line 59 of file WorkerNode.cpp.

+ Here is the call graph for this function:

◆ onInitComponent()

void onInitComponent ( )
overridevirtual

Initializes the worker and starts the worker thread.

Implements ManagedIceObject.

Definition at line 37 of file WorkerNode.cpp.

+ Here is the call graph for this function:

◆ pauseWorker()

void pauseWorker ( bool  pauseFlag,
const Ice::Current &  = Ice::emptyCurrent 
)
inlineoverride

Definition at line 170 of file WorkerNode.h.

◆ rewire()

void rewire ( const NodeId &  nodeId,
const std::vector< std::pair< NodeId, float >> &  kNearestIdsAndDistances,
const std::map< NodeId, bool > &  isCollisionFreeCache 
)
protected

The rewire operation of the rrt* algorithm.

It optimizes path lengths.

Parameters
nodeIdThe root node's id
kNearestIdsAndDistancesThe k nearest nodes and their distance to the root node.
isCollisionFreeCacheCache storing collision checks for paths between nodes from knearest and the root node..

Definition at line 302 of file WorkerNode.cpp.

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

◆ steer()

WorkerNode::ConfigType steer ( const ConfigType from,
const ConfigType to 
)
protected

Does a linear interpolation from from to to and checks the path per DCD.

Parameters
fromstarting point for the interpolation. (assumed to be collision free)
toend point for the interpolation.
Returns
The point furthest away from from (but collision free reachable from from) on the line from->to. If from->to is collision free the return value is identical to to (no floating point error). If the first step on the path from->to has a collision the return value is identical to from (no floating point error).

Definition at line 382 of file WorkerNode.cpp.

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

◆ updateTree()

void updateTree ( const Update &  u,
const Ice::Current &  = Ice::emptyCurrent 
)
override

Receives an update from other workers and adds it to the queue of pending updates.

Parameters
uthe update.

Definition at line 66 of file WorkerNode.cpp.

+ Here is the call graph for this function:

◆ workerTask()

void workerTask ( )
protected

The worker task.

It performs all planning.

Definition at line 72 of file WorkerNode.cpp.

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

Friends And Related Function Documentation

◆ GenericFactory< WorkerNodeBase, WorkerNode >

friend class GenericFactory< WorkerNodeBase, WorkerNode >
friend

Definition at line 210 of file WorkerNode.h.

◆ ManagedIceObject

friend class ManagedIceObject
friend

required for static factory methode create

Definition at line 333 of file WorkerNode.h.

Member Data Documentation

◆ globalWorkers

TreeUpdateInterfacePrx globalWorkers
protected

Proxy used to write to the update topic.

Definition at line 309 of file WorkerNode.h.

◆ killRequest

std::atomic_bool killRequest
protected

Wheether the node should shut down.

Definition at line 285 of file WorkerNode.h.

◆ onePastLastGoalConnect

std::size_t onePastLastGoalConnect
protected

The index one past the last node that was considered to be connected to the goal configuration.

Definition at line 319 of file WorkerNode.h.

◆ sampler

std::unique_ptr<InformedSampler> sampler
protected

Stores the sampler.

A pointer is used, since the sampler ctor requires space information and in case of default construction the information is not available. (the default ctor is used when transmitting this object per ice.) The pointer gets set during construction or post unmarshalling.

Definition at line 328 of file WorkerNode.h.

◆ tree

Tree tree
protected

The RRT tree.

Definition at line 280 of file WorkerNode.h.

◆ updateMutex

std::mutex updateMutex
mutableprotected

Mutex protecting the update structures.

Definition at line 295 of file WorkerNode.h.

◆ updateTopicName

std::string updateTopicName
protected

The update topic's name.

Definition at line 305 of file WorkerNode.h.

◆ workerIdSring

std::string workerIdSring
protected

A string representation of the worker id.

Used for output.

Definition at line 314 of file WorkerNode.h.

◆ workerPaused

std::atomic_bool workerPaused
protected

workerPaused is true if the worker is currently on hold and does not perform any calculations.

Definition at line 290 of file WorkerNode.h.

◆ workerThread

std::thread workerThread
protected

Thread executing the worker task.

Definition at line 300 of file WorkerNode.h.


The documentation for this class was generated from the following files: