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; }