From 0aae68f00e5e7079ef16bf6b778f75113ea07781 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 8 Sep 2016 14:42:22 +0200
Subject: [PATCH] [C++] Fix removeCollisionPairsFromSrdf (update to API
changes)
---
src/parsers/srdf.hpp | 47 ++++++++++++++++++++++----------------------
1 file changed, 23 insertions(+), 24 deletions(-)
diff --git a/src/parsers/srdf.hpp b/src/parsers/srdf.hpp
index 42a1b4e74..e6cfd0202 100644
--- a/src/parsers/srdf.hpp
+++ b/src/parsers/srdf.hpp
@@ -39,13 +39,13 @@ namespace se3
/// It throws if the SRDF file is incorrect.
///
/// \param[in] model Model of the kinematic tree.
- /// \param[in] model_geom Model of the geometries.
+ /// \param[in] geomModel Model of the geometries.
/// \param[out] data_geom Data containing the active collision pairs.
/// \param[in] filename The complete path to the SRDF file.
/// \param[in] verbose Verbosity mode (print removed collision pairs and undefined link inside the model).
///
inline void removeCollisionPairsFromSrdf(const Model& model,
- GeometryModel & model_geom,
+ GeometryModel & geomModel,
const std::string & filename,
const bool verbose) throw (std::invalid_argument)
{
@@ -99,30 +99,29 @@ namespace se3
continue;
}
- typedef GeometryModel::GeomIndexList GeomIndexList;
- typedef std::map<JointIndex, GeomIndexList> GeomIndexListMap;
- GeomIndexListMap::const_iterator _innerObject1 = model_geom.innerObjects.find(joint_id1);
- if (_innerObject1 == model_geom.innerObjects.end()) continue;
- GeomIndexListMap::const_iterator _innerObject2 = model_geom.innerObjects.find(joint_id2);
- if (_innerObject2 == model_geom.innerObjects.end()) continue;
- const GeomIndexList & innerObject1 = _innerObject1->second;
- const GeomIndexList & innerObject2 = _innerObject2->second;
-
- for(GeomIndexList::const_iterator it1 = innerObject1.begin();
- it1 != innerObject1.end();
- ++it1)
- {
- if (model_geom.geometryObjects[*it1].parentFrame != frame_id1) continue;
- for(GeomIndexList::const_iterator it2 = innerObject2.begin();
- it2 != innerObject2.end();
- ++it2)
- {
- if (model_geom.geometryObjects[*it2].parentFrame != frame_id2) continue;
- model_geom.removeCollisionPair(CollisionPair(*it1, *it2));
- if(verbose)
- std::cout << "Remove collision pair (" << joint_id1 << "," << joint_id2 << ")" << std::endl;
+ typedef std::vector<CollisionPair> CollisionPairs_t;
+ bool didRemove = false;
+ for(CollisionPairs_t::iterator _colPair = geomModel.collisionPairs.begin();
+ _colPair != geomModel.collisionPairs.end(); ) {
+ const CollisionPair& colPair (*_colPair);
+ bool remove =
+ (
+ (geomModel.geometryObjects[colPair.first ].parentFrame == frame_id1)
+ && (geomModel.geometryObjects[colPair.second].parentFrame == frame_id2)
+ ) || (
+ (geomModel.geometryObjects[colPair.second].parentFrame == frame_id1)
+ && (geomModel.geometryObjects[colPair.first ].parentFrame == frame_id2)
+ );
+
+ if (remove) {
+ _colPair = geomModel.collisionPairs.erase(_colPair);
+ didRemove = true;
+ } else {
+ ++_colPair;
}
}
+ if(didRemove && verbose)
+ std::cout << "Remove collision pair (" << link1 << "," << link2 << ")" << std::endl;
}
} // BOOST_FOREACH
--
GitLab