From 36d3e355450e95485b2f9d5588aa3008ec2b2aaa Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Tue, 14 Sep 2021 08:44:25 +0000
Subject: [PATCH] [StatesPathFinder] Catch core::path_planning_failed
exceptions
when calling a path planner in hpp-core.
Throw core::path_planning_failed when interrupted.
---
src/path-planner/in-state-path.cc | 2 +-
src/path-planner/states-path-finder.cc | 11 ++++++-----
2 files changed, 7 insertions(+), 6 deletions(-)
diff --git a/src/path-planner/in-state-path.cc b/src/path-planner/in-state-path.cc
index 728eca1b..69c065c8 100644
--- a/src/path-planner/in-state-path.cc
+++ b/src/path-planner/in-state-path.cc
@@ -229,4 +229,4 @@ namespace hpp {
} // namespace pathPlanner
} // namespace manipulation
-} // namespace hpp
\ No newline at end of file
+} // namespace hpp
diff --git a/src/path-planner/states-path-finder.cc b/src/path-planner/states-path-finder.cc
index 9b3b1d61..92cde65b 100644
--- a/src/path-planner/states-path-finder.cc
+++ b/src/path-planner/states-path-finder.cc
@@ -36,6 +36,7 @@
#include <hpp/constraints/explicit.hh>
#include <hpp/core/path-vector.hh>
+#include <hpp/core/path-planning-failed.hh>
#include <hpp/core/configuration-shooter.hh>
#include <hpp/core/collision-validation.hh>
#include <hpp/core/collision-validation-report.hh>
@@ -1083,7 +1084,7 @@ namespace hpp {
planner->setGoal(q2);
hppDout(info, "calling InStatePath::computePath for transition " << i);
return planner->solve();
- } catch(const std::exception&(e)) {
+ } catch(const core::path_planning_failed&(e)) {
std::ostringstream oss;
oss << "Error " << e.what() << "\n";
oss << "Solution \"latest\":\nFailed to build path at edge " << i << ": ";
@@ -1145,7 +1146,7 @@ namespace hpp {
hppDout(warning, "Solution " << idxSol_ << ": Success"
<< "\n-----------------------------------------------");
return ans;
- } catch(const std::exception&(e)) {
+ } catch(const core::path_planning_failed&(e)) {
std::ostringstream oss;
oss << "Error " << e.what() << "\n";
oss << "Solution " << idxSol_ << ": Failed to build path at edge " << i << ": ";
@@ -1222,7 +1223,7 @@ namespace hpp {
solved_ = true;
}
- } catch(const std::exception&(e)) {
+ } catch(const core::path_planning_failed&(e)) {
std::ostringstream oss;
oss << "Error " << e.what() << "\n";
oss << "Solution " << idxSol_ << ": Failed to build path at edge " << idxConfigList_ << ": ";
@@ -1248,7 +1249,7 @@ namespace hpp {
solved_ = false;
unsigned long int nIter (0);
startSolve ();
- if (interrupt_) throw std::runtime_error ("Interruption");
+ if (interrupt_) throw core::path_planning_failed("Interruption");
while (!solved_) {
// Execute one step
hppStartBenchmark(ONE_STEP);
@@ -1258,7 +1259,7 @@ namespace hpp {
++nIter;
//solved = problem()->target()->reached (roadmap());
- if (interrupt_) throw std::runtime_error ("Interruption");
+ if (interrupt_) throw core::path_planning_failed("Interruption");
}
return solution_;
}
--
GitLab