From 7804b08f3d1ba4287579dd87cc81fec36a7d3ffa Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Tue, 26 Jul 2016 17:00:26 +0200
Subject: [PATCH] [C++] The cloud of points in GeometryObject is now a
fcl::CollisionGeometry instead of fcl::CollisionObject
---
src/algorithm/geometry.hxx | 2 +-
src/multibody/geometry.hpp | 18 +++++++++---------
src/multibody/geometry.hxx | 10 +++++-----
src/parsers/urdf/geometry.hxx | 17 ++++++++---------
unittest/geom.cpp | 10 ++++------
5 files changed, 27 insertions(+), 30 deletions(-)
diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index 65e2bfb11..b747f4df5 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -145,7 +145,7 @@ namespace se3
BOOST_FOREACH(const GeometryObject & geom,geomModel.geometryObjects)
{
const boost::shared_ptr<const fcl::CollisionGeometry> & fcl
- = geom.collision_object.collisionGeometry();
+ = geom.collision_geometry;
const SE3 & jMb = geom.placement; // placement in joint.
double radius = geomData.radius[geom.parent];
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index 95a48f177..a48a41d41 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -187,7 +187,7 @@ struct GeometryObject
JointIndex parent;
/// \brief The actual cloud of points representing the collision mesh of the object
- fcl::CollisionObject collision_object;
+ boost::shared_ptr<fcl::CollisionGeometry> collision_geometry;
/// \brief Position of geometry object in parent joint's frame
SE3 placement;
@@ -196,11 +196,11 @@ struct GeometryObject
std::string mesh_path;
- GeometryObject(const std::string & name, const JointIndex parent, const fcl::CollisionObject & collision,
+ GeometryObject(const std::string & name, const JointIndex parent, const boost::shared_ptr<fcl::CollisionGeometry> & collision,
const SE3 & placement, const std::string & mesh_path)
: name(name)
, parent(parent)
- , collision_object(collision)
+ , collision_geometry(collision)
, placement(placement)
, mesh_path(mesh_path)
{}
@@ -209,7 +209,7 @@ struct GeometryObject
{
name = other.name;
parent = other.parent;
- collision_object = other.collision_object;
+ collision_geometry = other.collision_geometry;
placement = other.placement;
mesh_path = other.mesh_path;
return *this;
@@ -221,7 +221,7 @@ struct GeometryObject
{
return ( lhs.name == rhs.name
&& lhs.parent == rhs.parent
- && lhs.collision_object == rhs.collision_object
+ && lhs.collision_geometry == rhs.collision_geometry
&& lhs.placement == rhs.placement
&& lhs.mesh_path == rhs.mesh_path
);
@@ -231,7 +231,7 @@ struct GeometryObject
{
os << "Name: \t \n" << geom_object.name << "\n"
<< "Parent ID: \t \n" << geom_object.parent << "\n"
- // << "collision object: \t \n" << geom_object.collision_object << "\n"
+ // << "collision object: \t \n" << geom_object.collision_geometry << "\n"
<< "Position in parent frame: \t \n" << geom_object.placement << "\n"
<< "Absolute path to mesh file: \t \n" << geom_object.mesh_path << "\n"
<< std::endl;
@@ -271,17 +271,17 @@ struct GeometryObject
~GeometryModel() {};
/**
- * @brief Add a geometry object of a given type to a GeometryModel
+ * @brief Add a geometry object to a GeometryModel
*
* @param[in] parent Index of the parent joint
- * @param[in] co The actual fcl CollisionObject
+ * @param[in] co The actual fcl CollisionGeometry
* @param[in] placement The relative placement regarding to the parent frame
* @param[in] geom_name The name of the Geometry Object
* @param[in] mesh_path The absolute path to the mesh
*
* @return The index of the new added GeometryObject in geometryObjects
*/
- inline GeomIndex addGeometryObject(const JointIndex parent, const fcl::CollisionObject & co,
+ inline GeomIndex addGeometryObject(const JointIndex parent, const boost::shared_ptr<fcl::CollisionGeometry> & co,
const SE3 & placement, const std::string & geom_name = "",
const std::string & mesh_path = "") throw(std::invalid_argument);
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 26f97e260..b8f08b879 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -42,7 +42,7 @@ namespace se3
{
inline GeometryModel::GeomIndex GeometryModel::addGeometryObject(const JointIndex parent,
- const fcl::CollisionObject & co,
+ const boost::shared_ptr<fcl::CollisionGeometry> & co,
const SE3 & placement,
const std::string & geom_name,
const std::string & mesh_path) throw(std::invalid_argument)
@@ -246,8 +246,8 @@ namespace se3
fcl::CollisionRequest collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP);
fcl::CollisionResult collisionResult;
- fcl::collide (model_geom.geometryObjects[co1].collision_object.collisionGeometry().get(), oMg_fcl[co1],
- model_geom.geometryObjects[co2].collision_object.collisionGeometry().get(), oMg_fcl[co2],
+ fcl::collide (model_geom.geometryObjects[co1].collision_geometry.get(), oMg_fcl[co1],
+ model_geom.geometryObjects[co2].collision_geometry.get(), oMg_fcl[co2],
collisionRequest, collisionResult);
return CollisionResult (collisionResult, co1, co2);
@@ -284,8 +284,8 @@ namespace se3
fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
fcl::DistanceResult result;
- fcl::distance ( model_geom.geometryObjects[co1].collision_object.collisionGeometry().get(), oMg_fcl[co1],
- model_geom.geometryObjects[co2].collision_object.collisionGeometry().get(), oMg_fcl[co2],
+ fcl::distance ( model_geom.geometryObjects[co1].collision_geometry.get(), oMg_fcl[co1],
+ model_geom.geometryObjects[co2].collision_geometry.get(), oMg_fcl[co2],
distanceRequest, result);
return DistanceResult (result, co1, co2);
diff --git a/src/parsers/urdf/geometry.hxx b/src/parsers/urdf/geometry.hxx
index d5d97710f..c3c3a6feb 100644
--- a/src/parsers/urdf/geometry.hxx
+++ b/src/parsers/urdf/geometry.hxx
@@ -47,15 +47,15 @@ namespace se3
* @brief Get a fcl::CollisionObject from an urdf geometry, searching
* for it in specified package directories
*
- * @param[in] urdf_geometry The input urdf geometry
+ * @param[in] urdf_geometry A shared pointer on the input urdf Geometry
* @param[in] package_dirs A vector containing the different directories where to search for packages
* @param[out] mesh_path The Absolute path of the mesh currently read
*
- * @return The geometry converted as a fcl::CollisionObject
+ * @return A shared pointer on the he geometry converted as a fcl::CollisionGeometry
*/
- inline fcl::CollisionObject retrieveCollisionGeometry(const boost::shared_ptr < ::urdf::Geometry> urdf_geometry,
- const std::vector < std::string > & package_dirs,
- std::string & mesh_path)
+ inline boost::shared_ptr<fcl::CollisionGeometry> retrieveCollisionGeometry(const boost::shared_ptr < ::urdf::Geometry> urdf_geometry,
+ const std::vector < std::string > & package_dirs,
+ std::string & mesh_path)
{
boost::shared_ptr < fcl::CollisionGeometry > geometry;
@@ -120,9 +120,8 @@ namespace se3
{
throw std::runtime_error(std::string("The polyhedron retrived is empty"));
}
- fcl::CollisionObject collisionObject (geometry, fcl::Transform3f());
- return collisionObject;
+ return geometry;
}
/**
@@ -202,12 +201,12 @@ namespace se3
std::size_t objectId = 0;
for (typename std::vector< boost::shared_ptr< T > >::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
{
- fcl::CollisionObject geometry_object = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
+ const boost::shared_ptr<fcl::CollisionGeometry> geometry = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
SE3 geomPlacement = convertFromUrdf((*i)->origin);
std::ostringstream geometry_object_suffix;
geometry_object_suffix << "_" << objectId;
const std::string & geometry_object_name = std::string(link_name + geometry_object_suffix.str());
- geom_model.addGeometryObject(model.getFrameParent(link_name), geometry_object, geomPlacement, geometry_object_name, mesh_path);
+ geom_model.addGeometryObject(model.getFrameParent(link_name), geometry, geomPlacement, geometry_object_name, mesh_path);
++objectId;
}
}
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 09c8a545c..19595a3f5 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -149,13 +149,11 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
idx = model.addJoint(model.getJointId("universe"),JointModelPlanar(),SE3::Identity(),"planar2_joint");
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity(),"planar2_body");
- boost::shared_ptr<fcl::Box> Sample(new fcl::Box(1));
- fcl::CollisionObject box1(Sample, fcl::Transform3f());
- model_geom.addGeometryObject(model.getJointId("planar1_joint"),box1, SE3::Identity(), "ff1_collision_object", "");
+ boost::shared_ptr<fcl::Box> sample(new fcl::Box(1));
+ model_geom.addGeometryObject(model.getJointId("planar1_joint"),sample, SE3::Identity(), "ff1_collision_object", "");
- boost::shared_ptr<fcl::Box> Sample2(new fcl::Box(1));
- fcl::CollisionObject box2(Sample, fcl::Transform3f());
- model_geom.addGeometryObject(model.getJointId("planar2_joint"),box2, SE3::Identity(), "ff2_collision_object", "");
+ boost::shared_ptr<fcl::Box> sample2(new fcl::Box(1));
+ model_geom.addGeometryObject(model.getJointId("planar2_joint"),sample2, SE3::Identity(), "ff2_collision_object", "");
se3::Data data(model);
se3::GeometryData data_geom(model_geom);
--
GitLab