From db91521f44fabf75727157a995f14ea92380acea Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 15 Feb 2016 19:49:30 +0100
Subject: [PATCH] Log more info in ManipulationPlanner
---
.../hpp/manipulation/manipulation-planner.hh | 17 ++++++++-
src/graph/node.cc | 9 ++++-
src/manipulation-planner.cc | 37 +++++++++++++++++--
3 files changed, 56 insertions(+), 7 deletions(-)
diff --git a/include/hpp/manipulation/manipulation-planner.hh b/include/hpp/manipulation/manipulation-planner.hh
index a3fb50b..9194b0f 100644
--- a/include/hpp/manipulation/manipulation-planner.hh
+++ b/include/hpp/manipulation/manipulation-planner.hh
@@ -40,12 +40,14 @@ namespace hpp {
public hpp::core::PathPlanner
{
public:
+ typedef std::list<std::size_t> ErrorFreqs_t;
+
/// Create an instance and return a shared pointer to the instance
static ManipulationPlannerPtr_t create (const core::Problem& problem,
const core::RoadmapPtr_t& roadmap);
/// One step of extension.
- ///
+ ///
/// A set of constraints is chosen using the graph of constraints.
/// A constraint extension is done using a chosen set.
///
@@ -60,6 +62,16 @@ namespace hpp {
bool extend (RoadmapNodePtr_t q_near,
const ConfigurationPtr_t &q_rand, core::PathPtr_t& validPath);
+ /// Get the number of occurrence of each errors.
+ ///
+ /// \sa ManipulationPlanner::errorList
+ ErrorFreqs_t getEdgeStat (const graph::EdgePtr_t& edge) const;
+
+ /// Get the list of possible outputs of the extension step.
+ ///
+ /// \sa ManipulationPlanner::getEdgeStat
+ static StringList_t errorList ();
+
protected:
/// Protected constructor
ManipulationPlanner (const Problem& problem,
@@ -100,7 +112,8 @@ namespace hpp {
STEERING_METHOD = 1,
PATH_VALIDATION = 2,
PATH_PROJECTION_SHORTER = 3,
- PATH_PROJECTION_ZERO = 4
+ PATH_PROJECTION_ZERO = 4,
+ PARTLY_EXTENDED = 5
};
static const std::vector<Reason> reasons_;
diff --git a/src/graph/node.cc b/src/graph/node.cc
index f56672c..dfbabe6 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -128,7 +128,14 @@ namespace hpp {
void Node::updateWeight (const EdgePtr_t& e, const Weight_t& w)
{
- neighbors_.insert (e, w);
+ for (Neighbors_t::const_iterator it = neighbors_.begin();
+ it != neighbors_.end(); ++it) {
+ if (it->second == e) {
+ /// Update the weights
+ neighbors_.insert (e, w);
+ }
+ }
+ hppDout (error, "Edge not found");
}
} // namespace graph
} // namespace manipulation
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index b2a9386..881201c 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -61,7 +61,8 @@ namespace hpp {
(SuccessBin::createReason ("SteeringMethod"))
(SuccessBin::createReason ("PathValidation returned length 0"))
(SuccessBin::createReason ("Path could not be fully projected"))
- (SuccessBin::createReason ("Path could not be projected"));
+ (SuccessBin::createReason ("Path could not be projected"))
+ (SuccessBin::createReason ("Extended partly"));
ManipulationPlannerPtr_t ManipulationPlanner::create (const core::Problem& problem,
const core::RoadmapPtr_t& roadmap)
@@ -93,6 +94,32 @@ namespace hpp {
return false;
}
+ ManipulationPlanner::ErrorFreqs_t ManipulationPlanner::getEdgeStat
+ (const graph::EdgePtr_t& edge) const
+ {
+ const std::size_t& id = edge->id ();
+ ErrorFreqs_t ret;
+ if (indexPerEdgeStatistics_.size() <= id ||
+ indexPerEdgeStatistics_[id] < 0) {
+ for (int i = 0; i < 6; ++i) ret.push_back (0);
+ } else {
+ const SuccessStatistics& ss =
+ perEdgeStatistics_[indexPerEdgeStatistics_[id]];
+ ret.push_back (ss.nbSuccess ());
+ for (int i = 0; i < 6; ++i)
+ ret.push_back (ss.nbFailure (reasons_[i]));
+ }
+ return ret;
+ }
+
+ StringList_t ManipulationPlanner::errorList ()
+ {
+ StringList_t ret;
+ ret.push_back ("Success");
+ for (int i = 0; i < 6; ++i) ret.push_back (reasons_[i].what);
+ return ret;
+ }
+
void ManipulationPlanner::oneStep ()
{
HPP_START_TIMECOUNTER(oneStep);
@@ -257,8 +284,10 @@ namespace hpp {
es.addFailure (reasons_[PATH_VALIDATION]);
validPath = fullValidPath;
} else {
- if (extendStep_ == 1 || fullyValid) validPath = fullValidPath;
- else {
+ if (extendStep_ == 1 || fullyValid) {
+ validPath = fullValidPath;
+ es.addSuccess ();
+ } else {
const value_type& length = fullValidPath->length();
const value_type& t_init = fullValidPath->timeRange ().first;
try {
@@ -269,8 +298,8 @@ namespace hpp {
es.addFailure (reasons_[PATH_PROJECTION_SHORTER]);
return false;
}
+ es.addFailure (reasons_[PARTLY_EXTENDED]);
}
- es.addSuccess ();
hppDout (info, "Extension:" << std::endl
<< es);
}
--
GitLab