Task Class Reference

An addirrt* task. More...

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

+ Inheritance diagram for Task:

Public Member Functions

void abortTask (const Ice::Current &=Ice::emptyCurrent) override
 Aborts the task. More...
 
PathWithCostSeq getAllPathsWithCost (const Ice::Current &=Ice::emptyCurrent) const override
 
PathWithCost getBestPath (const Ice::Current &=Ice::emptyCurrent) const override
 
Ice::Int getMaxCpus (const Ice::Current &=Ice::emptyCurrent) const override
 
Ice::Long getNodeCount (const Ice::Current &=Ice::emptyCurrent) const override
 
Path getNthPath (Ice::Long n, const Ice::Current &=Ice::emptyCurrent) const override
 
PathWithCost getNthPathWithCost (Ice::Long index, const Ice::Current &=Ice::emptyCurrent) const override
 
Path getPath (const Ice::Current &=Ice::emptyCurrent) const override
 
Ice::Long getPathCount (const Ice::Current &=Ice::emptyCurrent) const override
 
PathWithCost getPathWithCost (const Ice::Current &=Ice::emptyCurrent) const override
 
void run (const RemoteObjectNodePrxList &remoteNodes, const Ice::Current &=Ice::emptyCurrent) override
 Runs the task. More...
 
void setMaxCpus (Ice::Int maxCpus, const Ice::Current &=Ice::emptyCurrent) override
 
void setPaths (const PathWithCostSeq &newPathList, const Ice::Current &=Ice::emptyCurrent) override
 Used by the manager to store its found paths. More...
 
 Task (CSpaceBasePtr cspace, const cprs::ComputingPowerRequestStrategyBasePtr &planningComputingPowerRequestStrategy, VectorXf startCfg, VectorXf goalCfg, const std::string &taskName="ADDIRRTStarTask", Ice::Long maximalPlanningTimeInSeconds=300, AdaptiveDynamicDomainParameters addParams=generateADDParamsFromDCDStepsize(0.01f), float targetCost=0, float dcdStep=0.01f, Ice::Long batchSize=10, Ice::Long nodeCountDeltaForGoalConnectionTries=50, Ice::Long initialWorkerCount=1, Ice::Long maximalWorkerCount=std::numeric_limits< Ice::Long >::max())
 Ctor. More...
 
- Public Member Functions inherited from CPRSAwareMotionPlanningTask
 CPRSAwareMotionPlanningTask (const VectorXf &startCfg, const VectorXf &goalCfg, const CSpaceBasePtr &cspace, Ice::Float dcdStep, Ice::Long maximalPlanningTimeInSeconds, const cprs::ComputingPowerRequestStrategyBasePtr &planningComputingPowerRequestStrategy, const std::string &taskName)
 ctor More...
 
- Public Member Functions inherited from MotionPlanningTaskWithDefaultMembers
CSpaceBasePtr getCSpace (const Ice::Current &=Ice::emptyCurrent) const override
 
float getDcdStep (const Ice::Current &=Ice::emptyCurrent) const override
 
VectorXf getGoal (const Ice::Current &=Ice::emptyCurrent) const override
 
Ice::Long getMaximalPlanningTimeInSeconds (const Ice::Current &=Ice::emptyCurrent) const override
 
VectorXf getStart (const Ice::Current &=Ice::emptyCurrent) const override
 
 MotionPlanningTaskWithDefaultMembers (const VectorXf &startCfg, const VectorXf &goalCfg, const CSpaceBasePtr &cspace, Ice::Float dcdStep, Ice::Long maximalPlanningTimeInSeconds, const std::string &taskName)
 ctor More...
 
- Public Member Functions inherited from MotionPlanningTask
void addTaskStatusCallback (std::function< void(TaskStatus::Status)> cb)
 
Ice::Long getPlanningTime (const Ice::Current &) const override
 
MotionPlanningTaskBasePrx & getProxy ()
 
Ice::Long getRefiningTime (const Ice::Current &) const override
 
Ice::Long getRunningTime (const Ice::Current &) const override
 
std::string getTaskName (const Ice::Current &=Ice::emptyCurrent) const override
 
TaskStatus::Status getTaskStatus (const Ice::Current &=Ice::emptyCurrent) const override
 
 MotionPlanningTask ()=default
 
virtual void onPlanningDone ()
 
virtual void onRefiningDone ()
 
virtual void postEnqueueing ()
 Called by the planning server after the task was enqueued. More...
 
virtual void registerAtIceAdapter (Ice::ObjectAdapterPtr &adapter, const Ice::Identity ident)
 
bool setTaskStatus (TaskStatus::Status newTaskStatus, const Ice::Current &=Ice::emptyCurrent) override
 
 ~MotionPlanningTask () override=default
 
- Public Member Functions inherited from MotionPlanningTaskCI
bool finishedRunning (const Ice::Current &=Ice::emptyCurrent) const override
 Returns whether the task has finished planning. More...
 
bool isRunning (const Ice::Current &=Ice::emptyCurrent) const override
 Returns whether the task is currently planning. More...
 
void waitForFinishedPlanning_async (const AMD_MotionPlanningTaskControlInterface_waitForFinishedPlanningPtr &cb, const Ice::Current &=Ice::emptyCurrent) override
 
void waitForFinishedRunning_async (const AMD_MotionPlanningTaskControlInterface_waitForFinishedRunningPtr &cb, const Ice::Current &=Ice::emptyCurrent) override
 
- Public Member Functions inherited from MotionPlanningMultiPathWithCostTaskCI
PathSeq getAllPaths (const Ice::Current &=Ice::emptyCurrent) const override
 
Path getNthPath (Ice::Long n, const Ice::Current &=Ice::emptyCurrent) const override
 
Path getPath (const Ice::Current &=Ice::emptyCurrent) const override
 
PathWithCost getPathWithCost (const Ice::Current &=Ice::emptyCurrent) const override
 
- Public Member Functions inherited from MotionPlanningMultiPathTaskCI
Path getPath (const Ice::Current &=Ice::emptyCurrent) const override
 
- Public Member Functions inherited from MotionPlanningWithCostTaskCI
Path getPath (const Ice::Current &=Ice::emptyCurrent) const override
 

Protected Member Functions

void checkParameters ()
 Checks for illegal parameters. More...
 
 Task ()
 Ctor used by object factories. More...
 
- Protected Member Functions inherited from CPRSAwareMotionPlanningTask
 CPRSAwareMotionPlanningTask ()=default
 ctor used for object factories More...
 
- Protected Member Functions inherited from MotionPlanningTaskWithDefaultMembers
 MotionPlanningTaskWithDefaultMembers ()=default
 

Protected Attributes

Ice::Long cachedNodeCount
 The cahced node count. More...
 
RemoteHandle< ManagerNodeBasePrx > manager
 The manager node. More...
 
std::condition_variable_any managerDone
 CV used by the dispatcher thread to wait until planning is done. More...
 
std::recursive_mutex mutex
 Mutex to protect internal structures. More...
 
PathWithCostSeq paths
 All found paths. More...
 
- Protected Attributes inherited from MotionPlanningTask
Ice::Long planningTime = 0
 The planning time in microseconds. More...
 
Ice::Long refiningTime = 0
 The refining time in microseconds. More...
 
- Protected Attributes inherited from MotionPlanningTaskCI
AMDCallbackCollection waitForFinishedPlanning
 
AMDCallbackCollection waitForFinishedRunning
 

Friends

template<class Base , class Derived >
class ::armarx::GenericFactory
 

Detailed Description

An addirrt* task.

The olanning algorithm used is a combination of:

  • bulk distributed rrt.
  • informed rrt*
  • adaptive dynamic domain

bulks and batches are used as synonyms.

Definition at line 59 of file Task.h.

Constructor & Destructor Documentation

◆ Task() [1/2]

Task ( CSpaceBasePtr  cspace,
const cprs::ComputingPowerRequestStrategyBasePtr &  planningComputingPowerRequestStrategy,
VectorXf  startCfg,
VectorXf  goalCfg,
const std::string &  taskName = "ADDIRRTStarTask",
Ice::Long  maximalPlanningTimeInSeconds = 300,
AdaptiveDynamicDomainParameters  addParams = generateADDParamsFromDCDStepsize(0.01f),
float  targetCost = 0,
float  dcdStep = 0.01f,
Ice::Long  batchSize = 10,
Ice::Long  nodeCountDeltaForGoalConnectionTries = 50,
Ice::Long  initialWorkerCount = 1,
Ice::Long  maximalWorkerCount = std::numeric_limits<Ice::Long>::max() 
)

Ctor.

Parameters
cspaceThe planning cspace.
planningComputingPowerRequestStrategyThe used cprs.
startCfgThe start configuration.
goalCfgThe goal configuration.
addParamsThe parameters for adaptive dynamic domain.
targetCostThe target cost. (planning stops when a path with a length <= was found)
dcdStepThe dcd step size.
maximalPlanningTimeInSecondsThe maximal planning time in seconds. (planning will stop after this time)
batchSizeThe size of a batch.
nodeCountDeltaForGoalConnectionTriesNumber of nodes created (by a worker) before a connect to the goal node is tried (by this worker).
initialWorkerCountThe in itaial number of worker processes.
maximalWorkerCountThe maximal number of worker processes.

Definition at line 296 of file Task.cpp.

+ Here is the call graph for this function:

◆ Task() [2/2]

Task ( )
inlineprotected

Ctor used by object factories.

Definition at line 172 of file Task.h.

Member Function Documentation

◆ abortTask()

void abortTask ( const Ice::Current &  = Ice::emptyCurrent)
override

Aborts the task.

Definition at line 100 of file Task.cpp.

+ Here is the call graph for this function:

◆ checkParameters()

void checkParameters ( )
protected

Checks for illegal parameters.

Definition at line 231 of file Task.cpp.

+ Here is the caller graph for this function:

◆ getAllPathsWithCost()

PathWithCostSeq getAllPathsWithCost ( const Ice::Current &  = Ice::emptyCurrent) const
override
Returns
All found paths.

Definition at line 87 of file Task.cpp.

+ Here is the call graph for this function:

◆ getBestPath()

PathWithCost getBestPath ( const Ice::Current &  = Ice::emptyCurrent) const
override
Returns
The shortest found path. (with its cost)

Definition at line 36 of file Task.cpp.

+ Here is the call graph for this function:

◆ getMaxCpus()

Ice::Int getMaxCpus ( const Ice::Current &  = Ice::emptyCurrent) const
override

Definition at line 349 of file Task.cpp.

◆ getNodeCount()

Ice::Long getNodeCount ( const Ice::Current &  = Ice::emptyCurrent) const
override
Returns
The current node count.

Definition at line 331 of file Task.cpp.

+ Here is the call graph for this function:

◆ getNthPath()

Path getNthPath ( Ice::Long  n,
const Ice::Current &  = Ice::emptyCurrent 
) const
inlineoverride

Definition at line 103 of file Task.h.

+ Here is the call graph for this function:

◆ getNthPathWithCost()

PathWithCost getNthPathWithCost ( Ice::Long  index,
const Ice::Current &  = Ice::emptyCurrent 
) const
override
Parameters
indexThe index.
Returns
The path at the given index.

Definition at line 66 of file Task.cpp.

+ Here is the call graph for this function:

◆ getPath()

Path getPath ( const Ice::Current &  = Ice::emptyCurrent) const
inlineoverride

Definition at line 108 of file Task.h.

+ Here is the call graph for this function:

◆ getPathCount()

Ice::Long getPathCount ( const Ice::Current &  = Ice::emptyCurrent) const
override
Returns
The number of found paths.

Definition at line 54 of file Task.cpp.

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

◆ getPathWithCost()

PathWithCost getPathWithCost ( const Ice::Current &  = Ice::emptyCurrent) const
inlineoverridevirtual

Implements MotionPlanningWithCostTaskCI.

Definition at line 99 of file Task.h.

+ Here is the call graph for this function:

◆ run()

void run ( const RemoteObjectNodePrxList &  remoteNodes,
const Ice::Current &  = Ice::emptyCurrent 
)
override

Runs the task.

Parameters
remoteNodesThe list of RemoteObjectNodeInterfacePrx used to distribute work to computers.

Definition at line 128 of file Task.cpp.

+ Here is the call graph for this function:

◆ setMaxCpus()

void setMaxCpus ( Ice::Int  maxCpus,
const Ice::Current &  = Ice::emptyCurrent 
)
override

Definition at line 344 of file Task.cpp.

◆ setPaths()

void setPaths ( const PathWithCostSeq &  newPathList,
const Ice::Current &  = Ice::emptyCurrent 
)
override

Used by the manager to store its found paths.

Parameters
newPathListThe paths.

Definition at line 216 of file Task.cpp.

+ Here is the call graph for this function:

Friends And Related Function Documentation

◆ ::armarx::GenericFactory

friend class ::armarx::GenericFactory
friend

Definition at line 162 of file Task.h.

Member Data Documentation

◆ cachedNodeCount

Ice::Long cachedNodeCount
protected

The cahced node count.

The cache is filled when the manager node shuts down.

Definition at line 204 of file Task.h.

◆ manager

RemoteHandle<ManagerNodeBasePrx> manager
protected

The manager node.

Definition at line 195 of file Task.h.

◆ managerDone

std::condition_variable_any managerDone
protected

CV used by the dispatcher thread to wait until planning is done.

Definition at line 190 of file Task.h.

◆ mutex

std::recursive_mutex mutex
mutableprotected

Mutex to protect internal structures.

Definition at line 181 of file Task.h.

◆ paths

PathWithCostSeq paths
protected

All found paths.

Definition at line 199 of file Task.h.


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