Unverified Commit ab36b3b0 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by GitHub
Browse files

Merge pull request #250 from florent-lamiraux/devel

[PathPlanner] throw specific type of exception when failing.
parents f7afb7c4 56bc09b5
Pipeline #16123 passed with stage
in 6 minutes and 17 seconds
......@@ -95,6 +95,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/core/path-optimization/spline-gradient-based-abstract.hh
include/hpp/core/path-optimizer.hh
include/hpp/core/path-planner.hh
include/hpp/core/path-planning-failed.hh
include/hpp/core/path-planner/k-prm-star.hh
include/hpp/core/path-planner/bi-rrt-star.hh
include/hpp/core/path-validation.hh
......
// Copyright (c) 2021 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_CORE_PATH_PLANNING_FAILED_HH
#define HPP_CORE_PATH_PLANNING_FAILED_HH
#include <exception>
namespace hpp {
namespace core {
struct HPP_CORE_DLLAPI path_planning_failed : public std::exception
{
path_planning_failed () : msg_ () {}
path_planning_failed (const std::string& msg) : msg_ (msg) {}
path_planning_failed (const path_planning_failed& other)
: std::exception (other), msg_ (other.msg_) {}
virtual ~path_planning_failed () _GLIBCXX_USE_NOEXCEPT {};
virtual const char* what () const noexcept { return msg_.c_str (); };
std::string msg_;
};
} // namespace core
} // namespace hpp
#endif // HPP_CORE_PATH_PLANNING_FAILED_HH
......@@ -177,6 +177,9 @@ namespace hpp {
void addEdges (const NodePtr_t from, const NodePtr_t& to,
const PathPtr_t& path);
/// Add the nodes and edges of a roadmap into this one.
void merge(const RoadmapPtr_t& other);
/// Add a goal configuration
/// \param config configuration
/// If configuration is already in the roadmap, tag corresponding node
......
......@@ -28,6 +28,7 @@
#include <hpp/core/node.hh>
#include <hpp/core/edge.hh>
#include <hpp/core/path.hh>
#include <hpp/core/path-planning-failed.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/core/path-projector.hh>
#include <hpp/core/steering-method.hh>
......@@ -114,7 +115,7 @@ namespace hpp {
if (solved ) {
hppDout (info, "tryConnectInitAndGoals succeeded");
}
if (interrupt_) throw std::runtime_error ("Interruption");
if (interrupt_) throw path_planning_failed ("Interruption");
while (!solved) {
// Check limits
std::ostringstream oss;
......@@ -122,7 +123,7 @@ namespace hpp {
if (!stopWhenProblemIsSolved_
&& problem()->target()->reached (roadmap())) break;
oss << "Maximal number of iterations reached: " << maxIterations_;
throw std::runtime_error (oss.str ().c_str ());
throw path_planning_failed (oss.str ().c_str ());
}
bpt::ptime timeStop(bpt::microsec_clock::universal_time());
value_type elapsed_ms = static_cast<value_type>(
......@@ -132,7 +133,7 @@ namespace hpp {
&& problem()->target()->reached (roadmap())) break;
oss << "time out (" << timeOut_ << "s) reached after " <<
elapsed_ms*1e-3 << "s";
throw std::runtime_error (oss.str ().c_str ());
throw path_planning_failed (oss.str ().c_str ());
}
// Execute one step
......@@ -144,7 +145,7 @@ namespace hpp {
// Check if problem is solved.
++nIter;
solved = stopWhenProblemIsSolved_ && problem()->target()->reached (roadmap());
if (interrupt_) throw std::runtime_error ("Interruption");
if (interrupt_) throw path_planning_failed ("Interruption");
}
PathVectorPtr_t planned = computePath ();
return finishSolve (planned);
......
......@@ -22,6 +22,7 @@
#include <hpp/core/configuration-shooter.hh>
#include <hpp/core/config-validations.hh>
#include <hpp/core/path-planning-failed.hh>
#include <hpp/core/path-projector.hh>
#include <hpp/core/path-validation.hh>
#include <hpp/core/path-validation-report.hh>
......@@ -92,7 +93,7 @@ namespace hpp {
case FAILURE:
oss << "kPRM* failed to solve problem with " << numberNodes_
<< " nodes.";
throw std::runtime_error (oss.str ().c_str ());
throw path_planning_failed(oss.str ().c_str ());
}
}
......@@ -123,7 +124,7 @@ namespace hpp {
nbTry++;
} while (!valid && nbTry < 10000);
if (!valid) {
throw std::runtime_error
throw path_planning_failed
("Failed to generate free configuration after 10000 trials.");
}
r->addNode (qrand);
......
......@@ -179,6 +179,22 @@ namespace hpp {
return nodeFrom;
}
void Roadmap::merge(const RoadmapPtr_t& other)
{
// Map nodes of other roadmap with nodes of this one
std::map<core::NodePtr_t, core::NodePtr_t> cNode;
for (const core::NodePtr_t& node: other->nodes()) {
cNode[node] = this->addNode(node->configuration());
}
for (const core::EdgePtr_t& edge: other->edges()) {
if (edge->path()->length() == 0)
assert (edge->from() == edge->to());
else
this->addEdges(cNode[edge->from()], cNode[edge->to()],
edge->path());
}
}
NodePtr_t
Roadmap::nearestNode (const Configuration_t& configuration,
value_type& minDistance, bool reverse)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment