From c10fd2ae64041ff97a0339f32f1a7b60cd8ce0a8 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 15 Feb 2016 16:41:47 +0100
Subject: [PATCH] Log extension results by edge in ManipulationPlanner
---
.../hpp/manipulation/manipulation-planner.hh | 44 ++++-----------
src/manipulation-planner.cc | 53 ++++++++++---------
2 files changed, 39 insertions(+), 58 deletions(-)
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index 429b705..a3fb50b 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/include/hpp/manipulation/manipulation-planner.hh
@@ -89,44 +89,20 @@ namespace hpp {
/// extend.
typedef ::hpp::statistics::SuccessStatistics SuccessStatistics;
typedef ::hpp::statistics::SuccessBin SuccessBin;
- SuccessStatistics extendStatistics_;
+ typedef ::hpp::statistics::SuccessBin::Reason Reason;
+ SuccessStatistics& edgeStat (const graph::EdgePtr_t& edge);
+ std::vector<int> indexPerEdgeStatistics_;
+ std::vector<SuccessStatistics> perEdgeStatistics_;
/// A Reason is associated to each EdgePtr_t that generated a failure.
enum TypeOfFailure {
- PROJECTION,
- STEERING_METHOD,
- PATH_VALIDATION,
- PATH_PROJECTION_SHORTER,
- PATH_PROJECTION_ZERO
+ PROJECTION = 0,
+ STEERING_METHOD = 1,
+ PATH_VALIDATION = 2,
+ PATH_PROJECTION_SHORTER = 3,
+ PATH_PROJECTION_ZERO = 4
};
- struct Reasons {
- typedef ::hpp::statistics::SuccessBin::Reason Reason;
- Reason projFailed, smFailed, pvFailed, pprojShorter, pprojZero;
-
- Reasons (const Reason& proj, const Reason& sm, const Reason& pv, const Reason& pps, const Reason& ppz) :
- projFailed (proj), smFailed (sm), pvFailed (pv), pprojShorter (pps), pprojZero (ppz) {}
- const Reason& get (TypeOfFailure t)
- {
- switch (t) {
- case PROJECTION:
- return projFailed;
- case STEERING_METHOD:
- return smFailed;
- case PATH_VALIDATION:
- return pvFailed;
- case PATH_PROJECTION_SHORTER:
- return pprojShorter;
- case PATH_PROJECTION_ZERO:
- return pprojZero;
- }
- return ::hpp::statistics::SuccessBin::REASON_UNKNOWN;
- }
- };
- typedef std::pair < graph::EdgePtr_t, Reasons > EdgeReasonPair;
- typedef std::map < graph::EdgePtr_t, Reasons > EdgeReasonMap;
- EdgeReasonMap failureReasons_;
-
- void addFailure (TypeOfFailure t, const graph::EdgePtr_t& edge);
+ static const std::vector<Reason> reasons_;
const value_type extendStep_;
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index 53c2aeb..b2a9386 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -17,6 +17,7 @@
#include "hpp/manipulation/manipulation-planner.hh"
#include <boost/tuple/tuple.hpp>
+#include <boost/assign/list_of.hpp>
#include <hpp/util/pointer.hh>
#include <hpp/util/timer.hh>
@@ -54,6 +55,14 @@ namespace hpp {
HPP_DEFINE_TIMECOUNTER(validatePath);
}
+ const std::vector<ManipulationPlanner::Reason>
+ ManipulationPlanner::reasons_ = boost::assign::list_of
+ (SuccessBin::createReason ("Projection"))
+ (SuccessBin::createReason ("SteeringMethod"))
+ (SuccessBin::createReason ("PathValidation returned length 0"))
+ (SuccessBin::createReason ("Path could not be fully projected"))
+ (SuccessBin::createReason ("Path could not be projected"));
+
ManipulationPlannerPtr_t ManipulationPlanner::create (const core::Problem& problem,
const core::RoadmapPtr_t& roadmap)
{
@@ -202,9 +211,10 @@ namespace hpp {
}
qProj_ = *q_rand;
HPP_START_TIMECOUNTER (applyConstraints);
+ SuccessStatistics& es = edgeStat (edge);
if (!edge->applyConstraints (n_near, qProj_)) {
HPP_STOP_TIMECOUNTER (applyConstraints);
- addFailure (PROJECTION, edge);
+ es.addFailure (reasons_[PROJECTION]);
return false;
}
HPP_STOP_TIMECOUNTER (applyConstraints);
@@ -212,7 +222,7 @@ namespace hpp {
HPP_START_TIMECOUNTER (buildPath);
if (!edge->build (path, *q_near, qProj_)) {
HPP_STOP_TIMECOUNTER (buildPath);
- addFailure (STEERING_METHOD, edge);
+ es.addFailure (reasons_[STEERING_METHOD]);
return false;
}
HPP_STOP_TIMECOUNTER (buildPath);
@@ -222,10 +232,10 @@ namespace hpp {
if (!pathProjector->apply (path, projPath)) {
if (!projPath || projPath->length () == 0) {
HPP_STOP_TIMECOUNTER (projectPath);
- addFailure (PATH_PROJECTION_ZERO, edge);
+ es.addFailure (reasons_[PATH_PROJECTION_ZERO]);
return false;
}
- addFailure (PATH_PROJECTION_SHORTER, edge);
+ es.addFailure (reasons_[PATH_PROJECTION_SHORTER]);
}
HPP_STOP_TIMECOUNTER (projectPath);
} else projPath = path;
@@ -239,12 +249,12 @@ namespace hpp {
(projPath, false, fullValidPath, report);
} catch (const core::projection_error& e) {
hppDout (error, e.what ());
- addFailure (PATH_VALIDATION, edge);
+ es.addFailure (reasons_[PATH_VALIDATION]);
return false;
}
HPP_STOP_TIMECOUNTER (validatePath);
if (fullValidPath->length () == 0) {
- addFailure (PATH_VALIDATION, edge);
+ es.addFailure (reasons_[PATH_VALIDATION]);
validPath = fullValidPath;
} else {
if (extendStep_ == 1 || fullyValid) validPath = fullValidPath;
@@ -256,34 +266,29 @@ namespace hpp {
(core::interval_t(t_init, t_init + length * extendStep_));
} catch (const core::projection_error& e) {
hppDout (error, e.what());
- addFailure (PATH_PROJECTION_SHORTER, edge);
+ es.addFailure (reasons_[PATH_PROJECTION_SHORTER]);
return false;
}
}
- extendStatistics_.addSuccess ();
+ es.addSuccess ();
hppDout (info, "Extension:" << std::endl
- << extendStatistics_);
+ << es);
}
return true;
}
- void ManipulationPlanner::addFailure (TypeOfFailure t, const graph::EdgePtr_t& edge)
+ ManipulationPlanner::SuccessStatistics& ManipulationPlanner::edgeStat
+ (const graph::EdgePtr_t& edge)
{
- EdgeReasonMap::iterator it = failureReasons_.find (edge);
- if (it == failureReasons_.end ()) {
- std::string edgeStr = edge->name () + " - ";
- Reasons r (SuccessBin::createReason (edgeStr + "Projection"),
- SuccessBin::createReason (edgeStr + "SteeringMethod"),
- SuccessBin::createReason (edgeStr + "PathValidation returned length 0"),
- SuccessBin::createReason (edgeStr + "Path could not be fully projected"),
- SuccessBin::createReason (edgeStr + "Path could not be projected"));
- failureReasons_.insert (EdgeReasonPair (edge, r));
- extendStatistics_.addFailure (r.get (t));
- return;
+ const std::size_t& id = edge->id ();
+ if (indexPerEdgeStatistics_.size () <= id) {
+ indexPerEdgeStatistics_.resize (id + 1, -1);
+ }
+ if (indexPerEdgeStatistics_[id] < 0) {
+ indexPerEdgeStatistics_[id] = perEdgeStatistics_.size();
+ perEdgeStatistics_.push_back (SuccessStatistics (edge->name (), 2));
}
- Reasons r = it->second;
- extendStatistics_.addFailure (r.get (t));
- hppDout (info, "Extension failed." << std::endl << extendStatistics_);
+ return perEdgeStatistics_[indexPerEdgeStatistics_[id]];
}
inline std::size_t ManipulationPlanner::tryConnectToRoadmap (const core::Nodes_t nodes)
--
GitLab