From 5984007b2abe130dba5a5f2b0af62008999d0d8a Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 6 Aug 2014 18:57:14 +0200
Subject: [PATCH] Fix bug related to commit
ecf9e6209e011a4aeb060eeb8b588fd243088ddb
---
src/manipulation-planner.cc | 30 ++++++++++++++++--------------
1 file changed, 16 insertions(+), 14 deletions(-)
diff --git a/src/manipulation-planner.cc b/src/manipulation-planner.cc
index e3071f2..a2b9e55 100644
--- a/src/manipulation-planner.cc
+++ b/src/manipulation-planner.cc
@@ -68,20 +68,22 @@ namespace hpp {
bool pathIsValid = extend (near->configuration (), q_rand, path);
// Insert new path to q_near in roadmap
- value_type t_final = path->timeRange ().second;
- if (pathIsValid && t_final != path->timeRange ().first) {
- ConfigurationPtr_t q_new (new Configuration_t
- ((*path) (t_final)));
- if (!belongs (q_new, newNodes)) {
- newNodes.push_back (roadmap ()->addNodeAndEdges
- (near, q_new, path));
- } else {
- core::NodePtr_t newNode = roadmap ()->addNode (q_new);
- roadmap ()->addEdge (near, newNode, path);
- core::interval_t timeRange = path->timeRange ();
- roadmap ()->addEdge (newNode, near, path->extract
- (core::interval_t (timeRange.second ,
- timeRange.first)));
+ if (pathIsValid) {
+ value_type t_final = path->timeRange ().second;
+ if (t_final != path->timeRange ().first) {
+ ConfigurationPtr_t q_new (new Configuration_t
+ ((*path) (t_final)));
+ if (!belongs (q_new, newNodes)) {
+ newNodes.push_back (roadmap ()->addNodeAndEdges
+ (near, q_new, path));
+ } else {
+ core::NodePtr_t newNode = roadmap ()->addNode (q_new);
+ roadmap ()->addEdge (near, newNode, path);
+ core::interval_t timeRange = path->timeRange ();
+ roadmap ()->addEdge (newNode, near, path->extract
+ (core::interval_t (timeRange.second ,
+ timeRange.first)));
+ }
}
}
}
--
GitLab