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.

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

void	killWorker (const Ice::Current &=Ice::emptyCurrent) override
	Signals the worker thread to exit.

void	onConnectComponent () override
	Gets a proxy to the topic.

void	onDisconnectComponent () override
	noop

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

void	onInitComponent () override
	Initializes the worker and starts the worker thread.

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.

	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

	~WorkerNode () override
	dtor.

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)

std::string	generateSubObjectName (const std::string &subObjectName)
	Generates a unique name for a sub object from a general name.

ArmarXManagerPtr	getArmarXManager () const
	Returns the ArmarX manager used to add and remove components.

ManagedIceObjectConnectivity	getConnectivity () const
	Retrieve connectivity of the object (topcis as well as proxies)

IceManagerPtr	getIceManager () const
	Returns the IceManager.

VariantBasePtr	getMetaInfo (const std::string &id)

StringVariantBaseMap	getMetaInfoMap () const

std::string	getName () const
	Retrieve name of object.

Ice::ObjectAdapterPtr	getObjectAdapter () const
	Returns object's Ice adapter.

ArmarXObjectSchedulerPtr	getObjectScheduler () const

PeriodicTaskPtr	getPeriodicTask (const std::string &name)

Profiler::ProfilerPtr	getProfiler () const
	getProfiler returns an instance of armarx::Profiler

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.

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`.

Ice::ObjectPrx	getProxy (long timeoutMs=0, bool waitForScheduler=true) const
	Returns the proxy of this object (optionally it waits for the proxy)

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).

template<class Prx>
void	getProxy (Prx &prx, long timeoutMs=0, bool waitForScheduler=true) const

int	getState () const
	Retrieve current state of the ManagedIceObject.

template<class TopicProxyType>
TopicProxyType	getTopic (const std::string &name)
	Returns a proxy of the specified topic.

template<class TopicProxyType>
void	getTopic (TopicProxyType &topicProxy, const std::string &name)
	Assigns a proxy of the specified topic to `topicProxy`.

std::vector< std::string >	getUnresolvedDependencies () const
	returns the names of all unresolved dependencies

	ManagedIceObject (ManagedIceObject const &other)

void	offeringTopic (const std::string &name)
	Registers a topic for retrival after initialization.

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.

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.

bool	usingProxy (const std::string &name, const std::string &endpoints="")
	Registers a proxy for retrieval after initialization and adds it to the dependency list.

void	usingTopic (const std::string &name, bool orderedPublishing=false)
	Registers a proxy for subscription after initialization.

void	waitForObjectScheduler ()
	Waits until the ObjectScheduler could resolve all dependencies.

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.

MessageTypeT	getEffectiveLoggingLevel () const

	Logging ()

void	setLocalMinimumLoggingLevel (MessageTypeT level)
	With setLocalMinimumLoggingLevel the minimum verbosity-level of log-messages can be set.

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.

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.

static std::string	GetObjectStateAsString (int state)

Protected Member Functions
void	addAndRewireConfig (const ConfigType &cfgReached, const NodeId &nodeNearestId)
	Adds a configuration to the tree.

void	applyPendingUpdates ()
	Applies all pending updates to the tree.

void	doBatch ()
	Executes a batch.

void	doBatchIteration ()
	Executes one iteration of a batch.

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.

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

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.

ConfigType	steer (const ConfigType &from, const ConfigType &to)
	Does a linear interpolation from from to to and checks the path per DCD.

	WorkerNode ()=default
	Only used when transmitting through ice.

void	workerTask ()
	The worker task.

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.

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.

void	setName (std::string name)
	Override name of well-known object.

void	terminate ()
	Initiates termination of this IceManagedObject.

	~ManagedIceObject () override

Protected Member Functions inherited from Logging
bool	checkLogLevel (MessageTypeT level) const

const LogSenderPtr &	getLogSender () const
	Retrieve log sender.

LogSenderPtr	loghelper (const char file, int line, const char function) const

Protected Attributes
TreeUpdateInterfacePrx	globalWorkers
	Proxy used to write to the update topic.

std::atomic_bool	killRequest
	Wheether the node should shut down.

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

std::unique_ptr< InformedSampler >	sampler
	Stores the sampler.

Tree	tree
	The RRT tree.

std::mutex	updateMutex
	Mutex protecting the update structures.

std::string	updateTopicName
	The update topic's name.

std::string	workerIdSring
	A string representation of the worker id.

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

std::thread	workerThread
	Thread executing the worker task.

Protected Attributes inherited from Logging
MessageTypeT	minimumLoggingLevel

SpamFilterDataPtr	spamFilter

LogTag	tag

Friends
class	GenericFactory< WorkerNodeBase, WorkerNode >

class	ManagedIceObject
	required for static factory methode create

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.

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.

Member Typedef Documentation

◆ ConfigType

using ConfigType = Tree::ConfigType

Type of a configuration.

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

cspace	The palnning cspace.
startCfg	The start configuration.
goalCfg	The goal configuration.
DCDStepSize	The dcd step size.
addParams	The adaptive dynamic domain parameters.
manager	A proxy to the manager node.
workerId	The worker's id.
batchSize	The batch size.
nodeCountDeltaForGoalConnectionTries	Number of nodes created (by a worker) before a connect to the goal node is tried (by this worker).
topicPrefix	The used topic prefix. (for the topic name to distribute updates)
rotationMatrix	The rotation matrix for the informed ellipsoid.

◆ ~WorkerNode()

~WorkerNode ( )

inlineoverride

dtor.

Asserts the worker thread was joined.

◆ 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

cfgReached	The configuration.
nodeNearestId	The nearest node's id.

◆ applyPendingUpdates()

void applyPendingUpdates ( )

protected

Applies all pending updates to the tree.

◆ distance()

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

inlinestatic

Calculates the euclidian distance between from and to.

Parameters

from	First vector.
to	Second vector.

Returns: The euclidian distance between from and to.

◆ doBatch()

void doBatch ( )

protected

Executes a batch.

◆ doBatchIteration()

void doBatchIteration ( )

protected

Executes one iteration of a batch.

◆ 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

nodeNearestId	The id of the nearest node.
cfgReached	The reached configuration.
outNodeBestId	Output parameter: The id of the optimal parent.
outCostReachedToNodeBest	Output parameter: The distance of reached and the optimal parent.
outKNearestIdsAndDistances	Output parameter: The k nearest nodes and their distance to reached.
outIsCollisionFreeCache	Output parameter: Cache storing collision checks for paths between nodes from knearest and reached.

◆ getDefaultName()

std::string getDefaultName ( ) const

inlineoverridevirtual

Returns: The component's default name.

Implements ManagedIceObject.

◆ getDimensionality()

std::size_t getDimensionality ( )

inlineprotected

Returns: The cspace's dimensionality

◆ getK()

std::size_t getK ( )

protected

Returns: Number of neighbours currently considered as parent.

◆ getRemoteUpdate()

Update getRemoteUpdate	(	Ice::Long	workerNodeId,
		Ice::Long	updateSubId )

protected

Parameters

workerNodeId	The update's worker id.
updateSubId	The update's sub id.

Returns: The update fetched from the manager node.

◆ getUpdate()

Update getUpdate	(	Ice::Long	updateId,
		const Ice::Current &	= Ice::emptyCurrent ) const

override

Parameters

updateId The updates id.

Returns: This workers update with the given id.

◆ ice_postUnmarshal()

void ice_postUnmarshal ( )

inlineoverride

noop

◆ initTree()

void initTree ( )

inlineprotected

initializes the tree by getting the current tree from the manager

◆ isCollisionFree()

bool isCollisionFree ( const ConfigType & cfg )

protected

Parameters

cfg	The configuration.

Returns: Whether the given configuration is collision free.

◆ isPathCollisionFree()

bool isPathCollisionFree	(	const ConfigType &	from,
		const ConfigType &	to )

protected

Parameters

from	The start configuration.
to	The goal configuration.

Returns: Whether the path from from to to is colliswion free.

◆ killWorker()

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

inlineoverride

Signals the worker thread to exit.

◆ onConnectComponent()

void onConnectComponent ( )

overridevirtual

Gets a proxy to the topic.

Implements ManagedIceObject.

◆ onDisconnectComponent()

void onDisconnectComponent ( )

inlineoverridevirtual

noop

Reimplemented from ManagedIceObject.

◆ onExitComponent()

void onExitComponent ( )

overridevirtual

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

Reimplemented from ManagedIceObject.

◆ onInitComponent()

void onInitComponent ( )

overridevirtual

Initializes the worker and starts the worker thread.

Implements ManagedIceObject.

◆ pauseWorker()

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

inlineoverride

◆ 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

nodeId	The root node's id
kNearestIdsAndDistances	The k nearest nodes and their distance to the root node.
isCollisionFreeCache	Cache storing collision checks for paths between nodes from knearest and the root node..

◆ 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

from	starting point for the interpolation. (assumed to be collision free)
to	end 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).

◆ 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

u	the update.

◆ workerTask()

void workerTask ( )

protected

The worker task.

It performs all planning.

Friends And Related Symbol Documentation

◆ GenericFactory< WorkerNodeBase, WorkerNode >

friend class GenericFactory< WorkerNodeBase, WorkerNode >

friend

◆ ManagedIceObject

friend class ManagedIceObject

friend

required for static factory methode create

Member Data Documentation

◆ globalWorkers

TreeUpdateInterfacePrx globalWorkers

protected

Proxy used to write to the update topic.

◆ killRequest

std::atomic_bool killRequest

protected

Wheether the node should shut down.

◆ onePastLastGoalConnect

std::size_t onePastLastGoalConnect

protected

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

◆ 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.

◆ tree

Tree tree

protected

The RRT tree.

◆ updateMutex

std::mutex updateMutex

mutableprotected

Mutex protecting the update structures.

◆ updateTopicName

std::string updateTopicName

protected

The update topic's name.

◆ workerIdSring

std::string workerIdSring

protected

A string representation of the worker id.

Used for output.

◆ workerPaused

std::atomic_bool workerPaused

protected

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

◆ workerThread

std::thread workerThread

protected

Thread executing the worker task.

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

RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.h
RobotComponents/components/MotionPlanning/Tasks/AdaptiveDynamicDomainInformedRRTStar/WorkerNode.cpp

Public Types

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Friends

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ ConfigType

Constructor & Destructor Documentation

◆ WorkerNode() [1/2]

◆ ~WorkerNode()

◆ WorkerNode() [2/2]

Member Function Documentation

◆ addAndRewireConfig()

◆ applyPendingUpdates()

◆ distance()

◆ doBatch()

◆ doBatchIteration()

◆ findBestParent()

◆ getDefaultName()

◆ getDimensionality()

◆ getK()

◆ getRemoteUpdate()

◆ getUpdate()

◆ ice_postUnmarshal()

◆ initTree()

◆ isCollisionFree()

◆ isPathCollisionFree()

◆ killWorker()

◆ onConnectComponent()

◆ onDisconnectComponent()

◆ onExitComponent()

◆ onInitComponent()

◆ pauseWorker()

◆ rewire()

◆ steer()

◆ updateTree()

◆ workerTask()

Friends And Related Symbol Documentation

◆ GenericFactory< WorkerNodeBase, WorkerNode >

◆ ManagedIceObject

Member Data Documentation

◆ globalWorkers

◆ killRequest

◆ onePastLastGoalConnect

◆ sampler

◆ tree

◆ updateMutex

◆ updateTopicName

◆ workerIdSring

◆ workerPaused

◆ workerThread