From 33160bdfad6535f9a6177dc2d2cfb1f0a5c39f0c Mon Sep 17 00:00:00 2001
From: Diane Bury <diane.bury@gmail.com>
Date: Thu, 3 Feb 2022 11:24:39 +0100
Subject: [PATCH] [GraphValidation] Add method to get collision list for a node
---
include/hpp/manipulation/graph/validation.hh | 17 +++++++++++++++++
src/graph/validation.cc | 6 +++++-
2 files changed, 22 insertions(+), 1 deletion(-)
diff --git a/include/hpp/manipulation/graph/validation.hh b/include/hpp/manipulation/graph/validation.hh
index 62f53b54..3d1c217e 100644
--- a/include/hpp/manipulation/graph/validation.hh
+++ b/include/hpp/manipulation/graph/validation.hh
@@ -24,6 +24,7 @@
# include <hpp/manipulation/config.hh>
# include <hpp/manipulation/fwd.hh>
# include <hpp/manipulation/graph/fwd.hh>
+# include <hpp/manipulation/graph/graph.hh>
namespace hpp {
namespace manipulation {
@@ -38,6 +39,9 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI Validation
{
public:
+ typedef std::vector<std::string> Collision;
+ typedef std::vector<Collision> CollisionList;
+ typedef std::map<std::string, CollisionList> CollisionMap;
Validation(const core::ProblemPtr_t& problem)
: problem_ (problem) {}
@@ -76,6 +80,11 @@ namespace hpp {
/// \note Even if true is returned, the report can contain warnings.
bool validateGraph (const GraphPtr_t& graph);
+ CollisionList getCollisionsForNode (const std::string& nodeName)
+ {
+ return collisions_[nodeName];
+ }
+
private:
void addWarning (const GraphComponentPtr_t& c, const std::string& w)
@@ -88,8 +97,16 @@ namespace hpp {
errors_.push_back (Message (c, w));
}
+ void addCollision (const GraphComponentPtr_t& c, const std::string& obj1,
+ const std::string& obj2)
+ {
+ Collision coll = Collision{obj1, obj2};
+ collisions_[c->name()].push_back(coll);
+ }
+
typedef std::pair<GraphComponentPtr_t, std::string> Message;
std::vector<Message> warnings_, errors_;
+ CollisionMap collisions_;
core::ProblemPtr_t problem_;
};
diff --git a/src/graph/validation.cc b/src/graph/validation.cc
index 6365bc3f..23d8a024 100644
--- a/src/graph/validation.cc
+++ b/src/graph/validation.cc
@@ -25,9 +25,9 @@
#include <hpp/core/collision-validation.hh>
#include <hpp/core/configuration-shooter.hh>
#include <hpp/core/relative-motion.hh>
+#include <hpp/core/collision-validation-report.hh>
#include "hpp/manipulation/problem.hh"
-#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/graph/edge.hh"
#include "hpp/manipulation/graph/state.hh"
#include "hpp/manipulation/graph/state-selector.hh"
@@ -157,6 +157,10 @@ namespace hpp {
oss << incindent << "The following collision pairs will always "
"collide." << incendl << *colReport << decindent;
addError (state, oss.str());
+ if (HPP_DYNAMIC_PTR_CAST(core::CollisionValidationReport, colReport)) {
+ std::pair<std::string, std::string> names = HPP_DYNAMIC_PTR_CAST(core::CollisionValidationReport, colReport)->getObjectNames();
+ addCollision (state, names.first, names.second);
+ }
success = false;
}
--
GitLab