Log extension results by edge in ManipulationPlanner

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

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)