diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh index 0a8e14dac95a157755776a4f156b6c1e3dab0273..a79d306fb8f8fb04ddf318cea6cb9cb6912a149f 100644 --- a/include/hpp/manipulation/manipulation-planner.hh +++ b/include/hpp/manipulation/manipulation-planner.hh @@ -108,13 +108,14 @@ namespace hpp { /// A Reason is associated to each EdgePtr_t that generated a failure. enum TypeOfFailure { - PROJECTION = 0, - STEERING_METHOD = 1, - PATH_VALIDATION_ZERO = 2, - PATH_PROJECTION_ZERO = 3, - PATH_PROJECTION_SHORTER = 4, - PATH_VALIDATION_SHORTER = 5, - PARTLY_EXTENDED = 6 + PATH_PROJECTION_SHORTER = 0, + PATH_VALIDATION_SHORTER = 1, + REACHED_DESTINATION_NODE = 2, + FAILURE = 3, + PROJECTION = 4, + STEERING_METHOD = 5, + PATH_VALIDATION_ZERO = 6, + PATH_PROJECTION_ZERO = 7 }; static const std::vector<Reason> reasons_; diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc index eaa52388597b45f381bbe2098b502e30db1750f0..7b46d8319bf5f186aeb441b3423c8d5f56407542 100644 --- a/src/manipulation-planner.cc +++ b/src/manipulation-planner.cc @@ -57,13 +57,14 @@ namespace hpp { const std::vector<ManipulationPlanner::Reason> ManipulationPlanner::reasons_ = boost::assign::list_of - (SuccessBin::createReason ("[Fail] Projection")) - (SuccessBin::createReason ("[Fail] SteeringMethod")) - (SuccessBin::createReason ("[Fail] Path validation returned length 0")) - (SuccessBin::createReason ("[Fail] Path could not be projected")) - (SuccessBin::createReason ("[Info] Path could not be fully projected")) - (SuccessBin::createReason ("[Info] Path could not be fully validated")) - (SuccessBin::createReason ("[Info] Extended partly")); + (SuccessBin::createReason ("--Path could not be fully projected")) + (SuccessBin::createReason ("--Path could not be fully validated")) + (SuccessBin::createReason ("--Reached destination node")) + (SuccessBin::createReason ("Failure")) + (SuccessBin::createReason ("--Projection of configuration on edge leaf")) + (SuccessBin::createReason ("--SteeringMethod")) + (SuccessBin::createReason ("--Path validation returned length 0")) + (SuccessBin::createReason ("--Path could not be projected at all")); ManipulationPlannerPtr_t ManipulationPlanner::create (const core::Problem& problem, const core::RoadmapPtr_t& roadmap) @@ -242,10 +243,12 @@ namespace hpp { SuccessStatistics& es = edgeStat (edge); if (!edge->applyConstraints (n_near, qProj_)) { HPP_STOP_TIMECOUNTER (applyConstraints); + es.addFailure (reasons_[FAILURE]); es.addFailure (reasons_[PROJECTION]); return false; } if (qProj_.isApprox (*q_near)) { + es.addFailure (reasons_[FAILURE]); es.addFailure (reasons_[PATH_PROJECTION_ZERO]); return false; } @@ -254,6 +257,7 @@ namespace hpp { HPP_START_TIMECOUNTER (buildPath); if (!edge->build (path, *q_near, qProj_)) { HPP_STOP_TIMECOUNTER (buildPath); + es.addFailure (reasons_[FAILURE]); es.addFailure (reasons_[STEERING_METHOD]); return false; } @@ -266,10 +270,10 @@ namespace hpp { if (projShorter) { if (!projPath || projPath->length () == 0) { HPP_STOP_TIMECOUNTER (projectPath); + es.addFailure (reasons_[FAILURE]); es.addFailure (reasons_[PATH_PROJECTION_ZERO]); return false; } - es.addFailure (reasons_[PATH_PROJECTION_SHORTER]); } HPP_STOP_TIMECOUNTER (projectPath); } else projPath = path; @@ -283,16 +287,17 @@ namespace hpp { (projPath, false, fullValidPath, report); } catch (const core::projection_error& e) { hppDout (error, e.what ()); + es.addFailure (reasons_[FAILURE]); es.addFailure (reasons_[PATH_VALIDATION_ZERO]); return false; } HPP_STOP_TIMECOUNTER (validatePath); if (fullValidPath->length () == 0) { + es.addFailure (reasons_[FAILURE]); es.addFailure (reasons_[PATH_VALIDATION_ZERO]); validPath = fullValidPath; return false; } else { - if (!fullyValid) es.addFailure (reasons_[PATH_VALIDATION_SHORTER]); if (extendStep_ == 1 || fullyValid) { validPath = fullValidPath; } else { @@ -303,6 +308,7 @@ namespace hpp { (core::interval_t(t_init, t_init + length * extendStep_)); } catch (const core::projection_error& e) { hppDout (error, e.what()); + es.addSuccess (); es.addFailure (reasons_[PATH_PROJECTION_SHORTER]); return false; } @@ -310,8 +316,18 @@ namespace hpp { hppDout (info, "Extension:" << std::endl << es); } - if (!projShorter && fullValidPath) es.addSuccess (); - else es.addFailure (reasons_[PARTLY_EXTENDED]); + if (!projShorter && fullValidPath) { + es.addSuccess (); + es.addFailure (reasons_ [REACHED_DESTINATION_NODE]); + } + else { + es.addSuccess (); + if (projShorter) { + es.addFailure (reasons_ [PATH_PROJECTION_SHORTER]); + } else { + es.addFailure (reasons_ [PATH_VALIDATION_SHORTER]); + } + } return true; }