ompl::geometric::BiTRRT::GrowResult ompl::geometric::BiTRRT::extendTree(Motion* nearest, TreeData& tree, Motion* toMotion, Motion*& result)
{
bool reach = true;
// Compute the state to extend toward
double d = si_->distance(nearest->state, toMotion->state);
// Truncate the random state to be no more than maxDistance_ from nearest neighbor
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nearest->state, toMotion->state, maxDistance_ / d, toMotion->state);
d = maxDistance_;
reach = false;
}
// Validating the motion
// If we are in the goal tree, we validate the motion in reverse
// si_->checkMotion assumes that the first argument is valid, so we must check this explicitly
// If the motion is valid, check the probabilistic transition test and the
// expansion control to ensure high quality nodes are added.
bool validMotion = (tree == tStart_ ? si_->checkMotion(nearest->state, toMotion->state) :
si_->isValid(toMotion->state) && si_->checkMotion(toMotion->state, nearest->state)) &&
transitionTest(opt_->motionCost(nearest->state, toMotion->state)) &&
minExpansionControl(d);
if (validMotion)
{
result = addMotion(toMotion->state, tree, nearest);
return reach ? SUCCESS : ADVANCED;
}
return FAILED;
}
ompl::base::PlannerStatus
ompl::geometric::TRRT::solve(const base::PlannerTerminationCondition &plannerTerminationCondition)
{
// Basic error checking
checkValidity();
// Goal information
base::Goal *goal = pdef_->getGoal().get();
auto *goalRegion = dynamic_cast<base::GoalSampleableRegion *>(goal);
// Input States ---------------------------------------------------------------------------------
// Loop through valid input states and add to tree
while (const base::State *state = pis_.nextStart())
{
// Allocate memory for a new start state motion based on the "space-information"-size
auto *motion = new Motion(si_);
// Copy destination <= source
si_->copyState(motion->state, state);
// Set cost for this start state
motion->cost = opt_->stateCost(motion->state);
if (nearestNeighbors_->size() == 0) // do not overwrite best/worst from previous call to solve
worstCost_ = bestCost_ = motion->cost;
// Add start motion to the tree
nearestNeighbors_->add(motion);
}
// Check that input states exist
if (nearestNeighbors_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
// Create state sampler if this is TRRT's first run
if (!sampler_)
sampler_ = si_->allocStateSampler();
// Debug
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
nearestNeighbors_->size());
// Solver variables ------------------------------------------------------------------------------------
// the final solution
Motion *solution = nullptr;
// the approximate solution, returned if no final solution found
Motion *approxSolution = nullptr;
// track the distance from goal to closest solution yet found
double approxDifference = std::numeric_limits<double>::infinity();
// distance between states - the intial state and the interpolated state (may be the same)
double randMotionDistance;
// Create random motion and a pointer (for optimization) to its state
auto *randMotion = new Motion(si_);
Motion *nearMotion;
// STATES
// The random state
base::State *randState = randMotion->state;
// The new state that is generated between states *to* and *from*
base::State *interpolatedState = si_->allocState(); // Allocates "space information"-sized memory for a state
// The chosen state btw rand_state and interpolated_state
base::State *newState;
// Begin sampling --------------------------------------------------------------------------------------
while (plannerTerminationCondition() == false)
{
// I.
// Sample random state (with goal biasing probability)
if (goalRegion && rng_.uniform01() < goalBias_ && goalRegion->canSample())
{
// Bias sample towards goal
goalRegion->sampleGoal(randState);
}
else
{
// Uniformly Sample
sampler_->sampleUniform(randState);
}
// II.
// Find closest state in the tree
nearMotion = nearestNeighbors_->nearest(randMotion);
// III.
// Distance from near state q_n to a random state
randMotionDistance = si_->distance(nearMotion->state, randState);
// Check if the rand_state is too far away
if (randMotionDistance > maxDistance_)
{
// Computes the state that lies at time t in [0, 1] on the segment that connects *from* state to *to* state.
// The memory location of *state* is not required to be different from the memory of either *from* or *to*.
si_->getStateSpace()->interpolate(nearMotion->state, randState, maxDistance_ / randMotionDistance,
interpolatedState);
// Update the distance between near and new with the interpolated_state
randMotionDistance = si_->distance(nearMotion->state, interpolatedState);
// Use the interpolated state as the new state
newState = interpolatedState;
}
else // Random state is close enough
newState = randState;
// IV.
// this stage integrates collision detections in the presence of obstacles and checks for collisions
if (!si_->checkMotion(nearMotion->state, newState))
continue; // try a new sample
// Minimum Expansion Control
// A possible side effect may appear when the tree expansion toward unexplored regions remains slow, and the
// new nodes contribute only to refine already explored regions.
if (!minExpansionControl(randMotionDistance))
continue; // give up on this one and try a new sample
base::Cost childCost = opt_->stateCost(newState);
// Only add this motion to the tree if the transition test accepts it
if (!transitionTest(opt_->motionCost(nearMotion->state, newState)))
continue; // give up on this one and try a new sample
// V.
// Create a motion
auto *motion = new Motion(si_);
si_->copyState(motion->state, newState);
motion->parent = nearMotion; // link q_new to q_near as an edge
motion->cost = childCost;
// Add motion to data structure
nearestNeighbors_->add(motion);
if (opt_->isCostBetterThan(motion->cost, bestCost_)) // motion->cost is better than the existing best
bestCost_ = motion->cost;
if (opt_->isCostBetterThan(worstCost_, motion->cost)) // motion->cost is worse than the existing worst
worstCost_ = motion->cost;
// VI.
// Check if this motion is the goal
double distToGoal = 0.0;
bool isSatisfied = goal->isSatisfied(motion->state, &distToGoal);
if (isSatisfied)
{
approxDifference = distToGoal; // the tolerated error distance btw state and goal
solution = motion; // set the final solution
break;
}
// Is this the closest solution we've found so far
if (distToGoal < approxDifference)
{
approxDifference = distToGoal;
approxSolution = motion;
}
} // end of solver sampling loop
// Finish solution processing --------------------------------------------------------------------
bool solved = false;
bool approximate = false;
// Substitute an empty solution with the best approximation
if (solution == nullptr)
{
solution = approxSolution;
approximate = true;
}
// Generate solution path for real/approx solution
if (solution != nullptr)
{
lastGoalMotion_ = solution;
// construct the solution path
std::vector<Motion *> mpath;
while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
}
// set the solution path
auto path(std::make_shared<PathGeometric>(si_));
for (int i = mpath.size() - 1; i >= 0; --i)
path->append(mpath[i]->state);
pdef_->addSolutionPath(path, approximate, approxDifference, getName());
solved = true;
}
// Clean up ---------------------------------------------------------------------------------------
si_->freeState(interpolatedState);
if (randMotion->state)
si_->freeState(randMotion->state);
delete randMotion;
OMPL_INFORM("%s: Created %u states", getName().c_str(), nearestNeighbors_->size());
return base::PlannerStatus(solved, approximate);
}
//-----------------------------------------------------------------------------
int ctkWorkflowTest2(int argc, char * argv [] )
{
QApplication app(argc, argv);
int defaultTime = 100;
// create the steps and the workflow
ctkWorkflow *workflow = new ctkWorkflow();
ctkWorkflowStep *step1 = new ctkWorkflowStep(workflow, "Step 1");
step1->setName("Step 1");
step1->setDescription("Description for step 1");
ctkWorkflowStep *step2 = new ctkWorkflowStep(workflow, "Step 2");
step2->setName("Step 2");
step2->setDescription("Description for step 2");
ctkWorkflowStep *step3 = new ctkWorkflowStep(workflow, "Step 3");
step3->setName("Step 3");
step3->setDescription("Description for step 3");
ctkWorkflowStep *step4 = new ctkWorkflowStep(workflow, "Step 4");
step4->setName("Step 4");
step4->setDescription("Description for step 4");
// create the qObjects that implement the required functions, and
// communicate with the workflow using signals and slots
ctkExampleWorkflowStepUsingSignalsAndSlots* qObject1 = new ctkExampleWorkflowStepUsingSignalsAndSlots;
ctkExampleWorkflowStepUsingSignalsAndSlots* qObject2 = new ctkExampleWorkflowStepUsingSignalsAndSlots;
ctkExampleWorkflowStepUsingSignalsAndSlots* qObject3 = new ctkExampleWorkflowStepUsingSignalsAndSlots;
ctkExampleWorkflowStepUsingSignalsAndSlots* qObject4 = new ctkExampleWorkflowStepUsingSignalsAndSlots;
// use the qObjects for validation
QObject::connect(step1->ctkWorkflowStepQObject(), SIGNAL(invokeValidateCommand(const QString&)), qObject1, SLOT(validate(const QString&)));
QObject::connect(qObject1, SIGNAL(validationComplete(bool, const QString&)), workflow, SLOT(evaluateValidationResults(bool, const QString&)));
QObject::connect(step2->ctkWorkflowStepQObject(), SIGNAL(invokeValidateCommand(const QString&)), qObject2, SLOT(validate(const QString&)));
QObject::connect(qObject2, SIGNAL(validationComplete(bool, const QString&)), workflow, SLOT(evaluateValidationResults(bool, const QString&)));
// step 3's validation will always fail
QObject::connect(step3->ctkWorkflowStepQObject(), SIGNAL(invokeValidateCommand(const QString&)), qObject3, SLOT(validateFails()));
QObject::connect(qObject3, SIGNAL(validationComplete(bool, const QString&)), workflow, SLOT(evaluateValidationResults(bool, const QString&)));
QObject::connect(step4->ctkWorkflowStepQObject(), SIGNAL(invokeValidateCommand(const QString&)), qObject4, SLOT(validate(const QString&)));
QObject::connect(qObject4, SIGNAL(validationComplete(bool, const QString&)), workflow, SLOT(evaluateValidationResults(bool, const QString&)));
// use the qObjects for entry processing
QObject::connect(step1->ctkWorkflowStepQObject(), SIGNAL(invokeOnEntryCommand(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)), qObject1, SLOT(onEntry(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)));
QObject::connect(qObject1, SIGNAL(onEntryComplete()), workflow, SLOT(processingAfterOnEntry()));
QObject::connect(step2->ctkWorkflowStepQObject(), SIGNAL(invokeOnEntryCommand(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)), qObject2, SLOT(onEntry(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)));
QObject::connect(qObject2, SIGNAL(onEntryComplete()), workflow, SLOT(processingAfterOnEntry()));
QObject::connect(step3->ctkWorkflowStepQObject(), SIGNAL(invokeOnEntryCommand(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)), qObject3, SLOT(onEntry(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)));
QObject::connect(qObject3, SIGNAL(onEntryComplete()), workflow, SLOT(processingAfterOnEntry()));
QObject::connect(step4->ctkWorkflowStepQObject(), SIGNAL(invokeOnEntryCommand(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)), qObject4, SLOT(onEntry(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)));
QObject::connect(qObject4, SIGNAL(onEntryComplete()), workflow, SLOT(processingAfterOnEntry()));
// use the qObjects for exit processing
QObject::connect(step1->ctkWorkflowStepQObject(), SIGNAL(invokeOnExitCommand(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)), qObject1, SLOT(onExit(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)));
QObject::connect(qObject1, SIGNAL(onExitComplete()), workflow, SLOT(processingAfterOnExit()));
QObject::connect(step2->ctkWorkflowStepQObject(), SIGNAL(invokeOnExitCommand(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)), qObject2, SLOT(onExit(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)));
QObject::connect(qObject2, SIGNAL(onExitComplete()), workflow, SLOT(processingAfterOnExit()));
QObject::connect(step3->ctkWorkflowStepQObject(), SIGNAL(invokeOnExitCommand(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)), qObject3, SLOT(onExit(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)));
QObject::connect(qObject3, SIGNAL(onExitComplete()), workflow, SLOT(processingAfterOnExit()));
QObject::connect(step4->ctkWorkflowStepQObject(), SIGNAL(invokeOnExitCommand(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)), qObject4, SLOT(onExit(const ctkWorkflowStep*, const ctkWorkflowInterstepTransition::InterstepTransitionType)));
QObject::connect(qObject4, SIGNAL(onExitComplete()), workflow, SLOT(processingAfterOnExit()));
step1->setHasValidateCommand(1);
step2->setHasValidateCommand(1);
step3->setHasValidateCommand(1);
step4->setHasValidateCommand(1);
step1->setHasOnEntryCommand(1);
step2->setHasOnEntryCommand(1);
step3->setHasOnEntryCommand(1);
step4->setHasOnEntryCommand(1);
step1->setHasOnExitCommand(1);
step2->setHasOnExitCommand(1);
step3->setHasOnExitCommand(1);
step4->setHasOnExitCommand(1);
// set the initial step (which sets the initial state)
workflow->setInitialStep(step1);
// add the first and second steps
if (!workflow->addTransition(step1, step2))
{
std::cerr << "could not add 1st and 2nd step" << std::endl;
return EXIT_FAILURE;
}
// add the third step
if (!workflow->addTransition(step2, step3))
{
std::cerr << "could not add 3rd step" << std::endl;
return EXIT_FAILURE;
}
// add the fourth step
if (!workflow->addTransition(step3, step4))
{
std::cerr << "could not add 4rd step" << std::endl;
return EXIT_FAILURE;
}
// start the workflow
if (!testStartWorkflow(workflow, defaultTime, app, 1, step1, qObject1, true, 0, qObject2, 0, 0, qObject3, 0, 0, qObject4, 0, 0))
{
std::cerr << "error starting workflow";
return EXIT_FAILURE;
}
// trigger transition to the next step
workflow->goForward();
if (!transitionTest(workflow, defaultTime, app, step2, qObject1, 1, 1, qObject2, 1, 0, qObject3, 0, 0, qObject4, 0, 0))
{
std::cerr << "error transitioning to step 2";
return EXIT_FAILURE;
}
// trigger transition to the next step
workflow->goForward();
if (!transitionTest(workflow, defaultTime, app, step3, qObject1, 1, 1, qObject2, 1, 1, qObject3, 1, 0, qObject4, 0, 0))
{
std::cerr << "error transitioning to step 3";
return EXIT_FAILURE;
}
// trigger transition to the next state (this should fail!)
workflow->goForward();
if (!transitionTest(workflow, defaultTime, app, step3, qObject1, 1, 1, qObject2, 1, 1, qObject3, 1, 0, qObject4, 0, 0))
{
std::cerr << "error after transition failure at step 3";
return EXIT_FAILURE;
}
// make sure the workflow stops properly
if (!testStopWorkflow(workflow, defaultTime, app, qObject1, 1, 1, qObject2, 1, 1, qObject3, 1, 1, qObject4, 0, 0))
{
std::cerr << "error after stopping workflow";
return EXIT_FAILURE;
}
// handles deletions of the workflow, steps, states and transitions
delete workflow;
return EXIT_SUCCESS;
}
bool ompl::geometric::TRRTConnect::connect(TreeData &tree, TreeGrowingInfo &tgi,
Motion *rmotion) {
++connectCount;
unsigned int steps = 0;
Motion *nmotion;
if (tree.stateInBoxZos_) {
//Among the nearest K nodes in the tree, find state with lowest cost to go
//The returned nmotion is collision-free
//If no collision-free motion is found, NULL is returned
nmotion = minCostNeighbor(tree,tgi,rmotion);
if (!nmotion) return false;
} else {
//Find nearest node in the tree
//The returned motion has not checked for collisions
//It never returns NULL
nmotion = tree->nearest(rmotion);
}
//State to add
base::State *dstate;
//Distance from near state to random state
double d(si_->distance(nmotion->state,rmotion->state));
//Check if random state is too far away
if (d > maxDistance_) {
si_->getStateSpace()->interpolate(nmotion->state,rmotion->state,
maxDistance_/d,tgi.xstate);
//Use the interpolated state as the new state
dstate = tgi.xstate;
} else {
//Random state is close enough
dstate = rmotion->state;
}
//Check for collisions
if (!tree.stateInBoxZos_) {
//If we are in the start tree, we just check the motion like we normally do.
//If we are in the goal tree, we need to check the motion in reverse,
//but checkMotion() assumes the first state it receives as argument is valid,
//so we check that one first.
bool validMotion(tree.start_? si_->checkMotion(nmotion->state,dstate) :
(si_->getStateValidityChecker()->isValid(dstate) &&
si_->checkMotion(dstate,nmotion->state)));
if (!validMotion) return false;
}
//Compute motion cost
base::Cost cost;
if (tree.stateInBoxZos_) {
if (tree.start_) {
cost = opt_->motionCost(nmotion->state,dstate);
} else {
cost = opt_->motionCost(dstate,nmotion->state);
}
} else {
//opt_ is of type FOSOptimizationObjective,
//there is no need to check the conversion
cost = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
preSolveMotionCost(nmotion->state,dstate);
}
//Only add this motion to the tree if the transition test accepts it
//Temperature must not be updated
while (transitionTest(cost,d,tree,false)) {
//Create a motion
Motion *motion(new Motion(si_));
si_->copyState(motion->state,dstate);
motion->parent = nmotion;
motion->root = nmotion->root;
tgi.xmotion = motion;
//Add motion to the tree
tree->add(motion);
steps++;
//Check if now the tree has a motion inside BoxZos
if (!tree.stateInBoxZos_) {
//tree.stateInBoxZos_ = cost.v < DBL_EPSILON*std::min(d,maxDistance_);
tree.stateInBoxZos_ = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
boxZosDistance(motion->state) < DBL_EPSILON;
if (tree.stateInBoxZos_) tree.temp_ = initTemperature_;
}
//Update frontier nodes and non frontier nodes count
++tree.frontierCount_;//Participates in the tree expansion
//If reached
if (dstate == rmotion->state) {
// std::cout << steps << " steps" << std::endl;
return true;
}
//Current near motion is the motion just added
nmotion = motion;
//Distance from near state to random state
d = si_->distance(nmotion->state,rmotion->state);
//Check if random state is too far away
if (d > maxDistance_) {
si_->getStateSpace()->interpolate(nmotion->state,rmotion->state,
maxDistance_/d,tgi.xstate);
//Use the interpolated state as the new state
dstate = tgi.xstate;
} else {
//Random state is close enough
dstate = rmotion->state;
}
//Check for collisions
//If we are in the start tree, we just check the motion like we normally do.
//If we are in the goal tree, we need to check the motion in reverse,
//but checkMotion() assumes the first state it receives as argument is valid,
//so we check that one first.
bool validMotion(tree.start_ ? si_->checkMotion(nmotion->state,dstate) :
(si_->getStateValidityChecker()->isValid(dstate) &&
si_->checkMotion(dstate,nmotion->state)));
if (!validMotion) return false;
//Compute motion cost
if (tree.stateInBoxZos_) {
if (tree.start_) {
cost = opt_->motionCost(nmotion->state,dstate);
} else {
cost = opt_->motionCost(dstate,nmotion->state);
}
} else {
//opt_ is of type FOSOptimizationObjective,
//there is no need to check the conversion
cost = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
preSolveMotionCost(nmotion->state,dstate);
}
}
// std::cout << steps << " steps" << std::endl;
return false;
}
ompl::geometric::TRRTConnect::ExtendResult
ompl::geometric::TRRTConnect::extend(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion) {
++extendCount;
Motion *nmotion;
/*if (tree.stateInBoxZos_) {
//Among the nearest K nodes in the tree, find state with lowest cost to go
//The returned nmotion is collision-free
//If no collision-free motion is found, NULL is returned
nmotion = minCostNeighbor(tree,tgi,rmotion);
if (!nmotion) return TRAPPED;
} else {*/
//Find nearest node in the tree
//The returned motion has not been checked for collisions
//It never returns NULL
nmotion = tree->nearest(rmotion);
//}
//State to add
base::State *dstate;
//Distance from near state to random state
double d(si_->distance(nmotion->state,rmotion->state));
//Check if random state is too far away
if (d > maxDistance_) {
si_->getStateSpace()->interpolate(nmotion->state,rmotion->state,
maxDistance_/d,tgi.xstate);
//Use the interpolated state as the new state
dstate = tgi.xstate;
} else {
//Random state is close enough
dstate = rmotion->state;
}
//Check for collisions
//if (!tree.stateInBoxZos_) {
//If we are in the start tree, we just check the motion like we normally do.
//If we are in the goal tree, we need to check the motion in reverse,
//but checkMotion() assumes the first state it receives as argument is valid,
//so we check that one first.
bool validMotion(tree.start_ ? si_->checkMotion(nmotion->state,dstate) :
(si_->getStateValidityChecker()->isValid(dstate) &&
si_->checkMotion(dstate,nmotion->state)));
if (!validMotion) return TRAPPED;
//}
//Minimum Expansion Control
//A possible side effect may appear when the tree expansion towards
//unexplored regions remains slow, and the new nodes contribute
//only to refine already explored regions.
if (!minExpansionControl(d,tree)) {
return TRAPPED;//Give up on this one and try a new sample
}
//Compute motion cost
base::Cost cost;
if (tree.stateInBoxZos_) {
if (tree.start_) {
cost = opt_->motionCost(nmotion->state,dstate);
} else {
cost = opt_->motionCost(dstate,nmotion->state);
}
} else {
//opt_ is of type FOSOptimizationObjective,
//there is no need to check the conversion
cost = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
preSolveMotionCost(nmotion->state,dstate);
}
//Only add this motion to the tree if the transition test accepts it
//Temperature must be updated
if (!transitionTest(cost,d,tree,true)) {
return TRAPPED;//Give up on this one and try a new sample
}
//Create a motion
Motion *motion(new Motion(si_));
si_->copyState(motion->state,dstate);
motion->parent = nmotion;
motion->root = nmotion->root;
tgi.xmotion = motion;
//Add motion to the tree
tree->add(motion);
//Check if now the tree has a motion inside BoxZos
if (!tree.stateInBoxZos_) {
tree.stateInBoxZos_ = cost.value() < DBL_EPSILON*std::min(d,maxDistance_);
if (tree.stateInBoxZos_) tree.temp_ = initTemperature_;
}
//Update frontier nodes and non frontier nodes count
if (d > frontierThreshold_) {//Participates in the tree expansion
++tree.frontierCount_;
} else {//Participates in the tree refinement
++tree.nonFrontierCount_;
}
return (d > maxDistance_) ? ADVANCED : REACHED;
}
//-----------------------------------------------------------------------------
int ctkWorkflowTest1(int argc, char * argv [] )
{
QApplication app(argc, argv);
int defaultTime = 100;
// create two steps and the workflow
ctkWorkflow *workflow = new ctkWorkflow();
ctkExampleDerivedWorkflowStep *step1 = new ctkExampleDerivedWorkflowStep(workflow, "Step 1");
step1->setName("Step 1");
step1->setDescription("Description for step 1");
ctkExampleDerivedWorkflowStep *step2 = new ctkExampleDerivedWorkflowStep(workflow, "Step 2");
step2->setName("Step 2");
step2->setDescription("Description for step 2");
// --------------------------------------------------------------------------
// try to add a transition for a step with the same id
ctkExampleDerivedWorkflowStep *step1Duplicated = new ctkExampleDerivedWorkflowStep(workflow, "Step 1");
if (workflow->addTransition(step1, step1Duplicated))
{
std::cerr << "workflow connected two steps with the same id";
return EXIT_FAILURE;
}
// try to add a transition from a step to itself
if (workflow->addTransition(step1, step1))
{
std::cerr << "workflow connected two steps with the same id";
return EXIT_FAILURE;
}
// --------------------------------------------------------------------------
// workflow with no steps
// try erroneously starting with no steps
if (!testStartWorkflow(workflow, defaultTime, app, 0))
{
std::cerr << "empty workflow is running after start()";
return EXIT_FAILURE;
}
// add the first step
if (!workflow->addTransition(step1, 0))
{
std::cerr << "could not add first step";
return EXIT_FAILURE;
}
// try erroneously starting with no initial step
if (!testStartWorkflow(workflow, defaultTime, app, 0))
{
std::cerr << "workflow is running after start() with no initial step";
return EXIT_FAILURE;
}
// --------------------------------------------------------------------------
// workflow with one step
// set the initial step (which sets the initial state)
workflow->setInitialStep(step1);
// try starting with one step
if (!testStartWorkflow(workflow, defaultTime, app, 1, step1, step1, 1, 0, step2, 0, 0))
{
std::cerr << "workflow is not running after start() with a single step";
return EXIT_FAILURE;
}
// triggering ValidationTransition and TransitionToPreviousStep
// should keep us in the same step, when there is only one step
workflow->goForward();
if (!transitionTest(workflow, defaultTime, app, step1, step1, 1, 0, step2, 0, 0))
{
std::cerr << "error in validation transition in a workflow with a single step";
return EXIT_FAILURE;
}
// transition to the previous step
workflow->goBackward();
if (!transitionTest(workflow, defaultTime, app, step1, step1, 1, 0, step2, 0, 0))
{
std::cerr << "error after transition to previous step in a workflow with a single step";
return EXIT_FAILURE;
}
// stop the workflow
if (!testStopWorkflow(workflow, defaultTime, app, step1, 1, 1, step2, 0, 0))
{
std::cerr << "workflow with one step still running after stop";
return EXIT_FAILURE;
}
// --------------------------------------------------------------------------
// workflow with two steps
// add the second step
if (!workflow->addTransition(step1, step2))
{
std::cerr << "could not add second step";
return EXIT_FAILURE;
}
// start the workflow
if (!testStartWorkflow(workflow, defaultTime, app, 1, step1, step1, 2, 1, step2, 0, 0))
{
std::cerr << "workflow is not running after start() with two steps";
return EXIT_FAILURE;
}
// make sure the workflow has the steps
if (!workflow->hasStep(step1->id()))
{
std::cerr << "Step1 not added to workflow";
return EXIT_FAILURE;
}
if (!workflow->hasStep(step2->id()))
{
std::cerr << "Step2 not added to workflow";
return EXIT_FAILURE;
}
// if (workflow->numberOfSteps() != 2)
// {
// std::cerr << "workflow has " << workflow->numberOfSteps() << " steps, not 2";
// return EXIT_FAILURE;
// }
// Test that the workflow transitions from processing to validation state
workflow->goForward();
if (!transitionTest(workflow, defaultTime, app, step2, step1, 2, 2, step2, 1, 0))
{
std::cerr << "error transitioning to next step in workflow with two steps";
return EXIT_FAILURE;
}
// Test that the workflow transitions back to the previous step
workflow->goBackward();
if (!transitionTest(workflow, defaultTime, app, step1, step1, 3, 2, step2, 1, 1))
{
std::cerr << "error transitioning to previous step in workflow with step steps";
return EXIT_FAILURE;
}
// make sure the workflow stops properly
if (!testStopWorkflow(workflow, defaultTime, app, step1, 3, 3, step2, 1, 1))
{
std::cerr << "workflow with two steps is running after stop()";
return EXIT_FAILURE;
}
// --------------------------------------------------------------------------
// workflow with three steps
// add a third step manually
ctkExampleDerivedWorkflowStep *step3 = new ctkExampleDerivedWorkflowStep(workflow, "Step 3");
step3->setName("Step 3");
step3->setDescription("Description for step 3");
if (!workflow->addTransition(step2, step3, "", ctkWorkflow::Forward))
{
std::cerr << "could not add step 3 with forward transition";
return EXIT_FAILURE;
}
if (!workflow->addTransition(step2, step3, "", ctkWorkflow::Backward))
{
std::cerr << "could not add next transition between step2 and step 3";
return EXIT_FAILURE;
}
if (workflow->forwardSteps(step1).length() != 1
|| workflow->forwardSteps(step1).first() != step2
|| workflow->forwardSteps(step2).length() != 1
|| workflow->forwardSteps(step2).first() != step3
|| workflow->forwardSteps(step3).length() != 0)
{
std::cerr << "error in list of forward steps" << std::endl;
return EXIT_FAILURE;
}
if (workflow->backwardSteps(step1).length() != 0
|| workflow->backwardSteps(step2).length() != 1
|| workflow->backwardSteps(step2).first() != step1
|| workflow->backwardSteps(step3).length() != 1
|| workflow->backwardSteps(step3).first() != step2)
{
std::cerr << "error in list of backward steps" << std::endl;
return EXIT_FAILURE;
}
if (!workflow->hasStep(step3->id()))
{
std::cerr << "Step3 not added to workflow";
return EXIT_FAILURE;
}
// if (workflow->numberOfSteps() != 3)
// {
// std::cerr << "workflow has " << workflow->numberOfSteps() << " steps, not 2";
// return EXIT_FAILURE;
// }
// now that we've stopped and restarted the state machine, we should
// be back in the initial step (step 1)
if (!testStartWorkflow(workflow, defaultTime, app, 1, step1, step1, 4, 3, step2, 1, 1, step3, 0, 0))
{
std::cerr << "workflow is not running after start() with three steps";
return EXIT_FAILURE;
}
// test to make sure our lists of forward and backwards steps is correct
if (!workflow->canGoForward(step1)
|| workflow->canGoBackward(step1)
|| !workflow->canGoForward(step2)
|| !workflow->canGoBackward(step2)
|| workflow->canGoForward(step3)
|| !workflow->canGoBackward(step3))
{
std::cerr << "error in can go forward/backward" << std::endl;
return EXIT_FAILURE;
}
// Test that the workflow transitions from step1 to step 2 to step 3
workflow->goForward();
QTimer::singleShot(defaultTime, &app, SLOT(quit()));
app.exec();
workflow->goForward();
if (!transitionTest(workflow, defaultTime, app, step3, step1, 4, 4, step2, 2, 2, step3, 1, 0))
{
std::cerr << "error transitioning to step3 in workflow with three steps";
return EXIT_FAILURE;
}
// Test that the workflow transitions back to the previous step
workflow->goBackward();
if (!transitionTest(workflow, defaultTime, app, step2, step1, 4, 4, step2, 3, 2, step3, 1, 1))
{
std::cerr << "error transitioning to previous step in workflow with three steps";
return EXIT_FAILURE;
}
// make sure the workflow stops properly
if (!testStopWorkflow(workflow, defaultTime, app, step1, 4, 4, step2, 3, 3, step3, 1, 1))
{
std::cerr << "error stopping workflow with three steps";
return EXIT_FAILURE;
}
// --------------------------------------------------------------------------
// workflow with a finish step (step 3)
// restart the workflow
if (!testStartWorkflow(workflow, defaultTime, app, 1, step1, step1, 5, 4, step2, 3, 3, step3, 1, 1))
{
std::cerr << "workflow with finish step is not running after start()";
return EXIT_FAILURE;
}
// try to go automatically to step 3
workflow->goToStep("Step 3");
if (!transitionTest(workflow, defaultTime, app, step1, step1, 6, 5, step2, 4, 4, step3, 2, 2))
{
std::cerr << "error after going to finish step";
return EXIT_FAILURE;
}
// try to go automatically to step 3 again
workflow->goToStep("Step 3");
if (!transitionTest(workflow, defaultTime, app, step1, step1, 7, 6, step2, 5, 5, step3, 3, 3))
{
std::cerr << "error after going to finish step the second time";
return EXIT_FAILURE;
}
// stop workflow
if (!testStopWorkflow(workflow, defaultTime, app, step1, 7, 7, step2, 5, 5, step3, 3, 3))
{
std::cerr << "error stopping workflow with finish step";
return EXIT_FAILURE;
}
// --------------------------------------------------------------------------
// workflow with two finishing steps (step3 and step4)
ctkExampleDerivedWorkflowStep *step4 = new ctkExampleDerivedWorkflowStep(workflow, "Step 4");
step4->setName("Step 4");
step4->setDescription("Description for step 4");
workflow->addTransition(step3, step4);
// restart the workflow
if (!testStartWorkflow(workflow, defaultTime, app, 1, step1, step1, 8, 7, step2, 5, 5, step3, 3, 3, step4, 0, 0))
{
std::cerr << "workflow with two finish steps is not running after start()";
return EXIT_FAILURE;
}
// try to go automatically to step 3
workflow->goToStep("Step 3");
if (!transitionTest(workflow, defaultTime, app, step1, step1, 9, 8, step2, 6, 6, step3, 4, 4, step4, 0, 0))
{
std::cerr << "error going to the first finish step of two";
return EXIT_FAILURE;
}
// try to go automatically to step 4
workflow->goToStep("Step 4");
if (!transitionTest(workflow, defaultTime, app, step1, step1, 10, 9, step2, 7, 7, step3, 5, 5, step4, 1, 1))
{
std::cerr << "error going to the second finish step of two";
return EXIT_FAILURE;
}
// go to step 3 (a finish step)
workflow->goForward();
QTimer::singleShot(defaultTime, &app, SLOT(quit()));
app.exec();
workflow->goForward();
if (!transitionTest(workflow, defaultTime, app, step3, step1, 10, 10, step2, 8, 8, step3, 6, 5, step4, 1, 1))
{
std::cerr << "error going from step1 to step3";
return EXIT_FAILURE;
}
// try to go automatically to step 4 (another goTo step)
workflow->goToStep("Step 4");
if (!transitionTest(workflow, defaultTime, app, step3, step1, 10, 10, step2, 8, 8, step3, 7, 6, step4, 2, 2))
{
std::cerr << "error going from the first finish step to the second finish step";
return EXIT_FAILURE;
}
// go to step 4, and then go forward (should not let you go past last step)
workflow->goForward();
QTimer::singleShot(defaultTime, &app, SLOT(quit()));
app.exec();
workflow->goForward();
if (!transitionTest(workflow, defaultTime, app, step4, step1, 10, 10, step2, 8, 8, step3, 7, 7, step4, 3, 2))
{
std::cerr << "error going forward past last step - shouldn't let you";
return EXIT_FAILURE;
}
// now try to go from step 4 to step 4 (should loop)
workflow->goToStep("Step 4");
if (!transitionTest(workflow, defaultTime, app, step4, step1, 10, 10, step2, 8, 8, step3, 7, 7, step4, 4, 3))
{
std::cerr << "error looping from step 4 to step 4";
return EXIT_FAILURE;
}
// go back to step 3, and then go from step 3 to step 3 (should loop without hitting step4)
workflow->goBackward();
QTimer::singleShot(defaultTime, &app, SLOT(quit()));
app.exec();
workflow->goToStep("Step 3");
if (!transitionTest(workflow, defaultTime, app, step3, step1, 10, 10, step2, 8, 8, step3, 9, 8, step4, 4, 4))
{
std::cerr << "error looping from step 3 to step 3";
return EXIT_FAILURE;
}
// handles deletions of the workflow, steps, states and transitions
delete workflow;
return EXIT_SUCCESS;
}
