Skip to content

Commit e583c81

Browse files
fmessmerfmessmer
committed
fcl noetic compatibility
1 parent 3f26461 commit e583c81

15 files changed

+149
-75
lines changed

‎cob_obstacle_distance/include/cob_obstacle_distance/fcl_marker_converter.hpp‎

Lines changed: 37 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,25 @@
1919
#define FCL_MARKER_CONVERTER_HPP_
2020

2121
#include <boost/scoped_ptr.hpp>
22-
#include <fcl/shape/geometric_shapes.h>
23-
#include <fcl/BVH/BVH_model.h>
24-
#include <fcl/shape/geometric_shape_to_BVH_model.h>
22+
23+
#include <fcl/config.h>
24+
#if FCL_MINOR_VERSION == 5
25+
#include <fcl/shape/geometric_shapes.h>
26+
#include <fcl/BVH/BVH_model.h>
27+
#include <fcl/shape/geometric_shape_to_BVH_model.h>
28+
typedef fcl::Box FCL_Box;
29+
typedef fcl::Sphere FCL_Sphere;
30+
typedef fcl::Cylinder FCL_Cylinder;
31+
#else
32+
#include <fcl/geometry/shape/box.h>
33+
#include <fcl/geometry/shape/sphere.h>
34+
#include <fcl/geometry/shape/cylinder.h>
35+
#include <fcl/geometry/bvh/BVH_model.h>
36+
#include <fcl/geometry/geometric_shape_to_BVH_model.h>
37+
typedef fcl::Boxf FCL_Box;
38+
typedef fcl::Spheref FCL_Sphere;
39+
typedef fcl::Cylinderf FCL_Cylinder;
40+
#endif
2541

2642

2743
#include "cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp"
@@ -43,16 +59,16 @@ class FclMarkerConverter
4359
};
4460

4561
template<>
46-
class FclMarkerConverter<fcl::Box>
62+
class FclMarkerConverter<FCL_Box>
4763
{
48-
typedef boost::scoped_ptr<fcl::Box> sPtrBox;
64+
typedef boost::scoped_ptr<FCL_Box> sPtrBox;
4965

5066
private:
51-
fcl::Box geo_shape_;
67+
FCL_Box geo_shape_;
5268

5369
public:
54-
FclMarkerConverter() : geo_shape_(fcl::Box(1.0, 1.0, 1.0)) {}
55-
FclMarkerConverter(fcl::Box& box) : geo_shape_(box) {}
70+
FclMarkerConverter() : geo_shape_(FCL_Box(1.0, 1.0, 1.0)) {}
71+
FclMarkerConverter(FCL_Box& box) : geo_shape_(box) {}
5672

5773
void assignValues(visualization_msgs::Marker& marker)
5874
{
@@ -62,7 +78,7 @@ class FclMarkerConverter<fcl::Box>
6278
marker.type = visualization_msgs::Marker::CUBE;
6379
}
6480

65-
fcl::Box getGeoShape() const
81+
FCL_Box getGeoShape() const
6682
{
6783
return geo_shape_;
6884
}
@@ -75,16 +91,16 @@ class FclMarkerConverter<fcl::Box>
7591
};
7692

7793
template<>
78-
class FclMarkerConverter<fcl::Sphere>
94+
class FclMarkerConverter<FCL_Sphere>
7995
{
80-
typedef boost::scoped_ptr<fcl::Sphere> sPtrSphere;
96+
typedef boost::scoped_ptr<FCL_Sphere> sPtrSphere;
8197

8298
private:
83-
fcl::Sphere geo_shape_;
99+
FCL_Sphere geo_shape_;
84100

85101
public:
86-
FclMarkerConverter() : geo_shape_(fcl::Sphere(1.0)) {}
87-
FclMarkerConverter(fcl::Sphere &sphere) : geo_shape_(sphere) {}
102+
FclMarkerConverter() : geo_shape_(FCL_Sphere(1.0)) {}
103+
FclMarkerConverter(FCL_Sphere &sphere) : geo_shape_(sphere) {}
88104

89105
void assignValues(visualization_msgs::Marker &marker)
90106
{
@@ -94,7 +110,7 @@ class FclMarkerConverter<fcl::Sphere>
94110
marker.type = visualization_msgs::Marker::SPHERE;
95111
}
96112

97-
fcl::Sphere getGeoShape() const
113+
FCL_Sphere getGeoShape() const
98114
{
99115
return geo_shape_;
100116
}
@@ -113,16 +129,16 @@ class FclMarkerConverter<fcl::Sphere>
113129
};
114130

115131
template<>
116-
class FclMarkerConverter<fcl::Cylinder>
132+
class FclMarkerConverter<FCL_Cylinder>
117133
{
118-
typedef boost::scoped_ptr<fcl::Cylinder> sPtrCylinder;
134+
typedef boost::scoped_ptr<FCL_Cylinder> sPtrCylinder;
119135

120136
private:
121-
fcl::Cylinder geo_shape_;
137+
FCL_Cylinder geo_shape_;
122138

123139
public:
124-
FclMarkerConverter() : geo_shape_(fcl::Cylinder(1.0, 1.0)) {}
125-
FclMarkerConverter(fcl::Cylinder &cyl) : geo_shape_(cyl) {}
140+
FclMarkerConverter() : geo_shape_(FCL_Cylinder(1.0, 1.0)) {}
141+
FclMarkerConverter(FCL_Cylinder &cyl) : geo_shape_(cyl) {}
126142

127143
void assignValues(visualization_msgs::Marker &marker)
128144
{
@@ -132,7 +148,7 @@ class FclMarkerConverter<fcl::Cylinder>
132148
marker.type = visualization_msgs::Marker::CYLINDER;
133149
}
134150

135-
fcl::Cylinder getGeoShape() const
151+
FCL_Cylinder getGeoShape() const
136152
{
137153
return geo_shape_;
138154
}

‎cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes.hpp‎

Lines changed: 12 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -23,8 +23,15 @@
2323
#include <shape_msgs/Mesh.h>
2424
#include <shape_msgs/MeshTriangle.h>
2525

26+
#include <fcl/config.h>
27+
#if FCL_MINOR_VERSION == 5
2628
#include "fcl/shape/geometric_shapes.h"
27-
#include "fcl/collision_object.h"
29+
#include "fcl/collision_object.h"
30+
typedef fcl::CollisionObject FCL_CollisionObject;
31+
#else
32+
#include <fcl/narrowphase/collision_object.h>
33+
typedef fcl::CollisionObjectf FCL_CollisionObject;
34+
#endif
2835

2936
#include "cob_obstacle_distance/fcl_marker_converter.hpp"
3037
#include "cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp"
@@ -92,9 +99,9 @@ class MarkerShape : public IMarkerShape
9299
inline void updatePose(const geometry_msgs::Pose& pose);
93100

94101
/**
95-
* @return A fcl::CollisionObject to calculate distances to other objects or check whether collision occurred or not.
102+
* @return A FCL_CollisionObject to calculate distances to other objects or check whether collision occurred or not.
96103
*/
97-
fcl::CollisionObject getCollisionObject() const;
104+
FCL_CollisionObject getCollisionObject() const;
98105

99106
virtual ~MarkerShape(){}
100107
};
@@ -155,9 +162,9 @@ class MarkerShape<BVH_RSS_t> : public IMarkerShape
155162
inline void updatePose(const geometry_msgs::Pose& pose);
156163

157164
/**
158-
* @return A fcl::CollisionObject to calculate distances to other objects or check whether collision occurred or not.
165+
* @return A FCL_CollisionObject to calculate distances to other objects or check whether collision occurred or not.
159166
*/
160-
fcl::CollisionObject getCollisionObject() const;
167+
FCL_CollisionObject getCollisionObject() const;
161168

162169
virtual ~MarkerShape(){}
163170
};

‎cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -115,7 +115,7 @@ inline visualization_msgs::Marker MarkerShape<T>::getMarker()
115115

116116

117117
template <typename T>
118-
fcl::CollisionObject MarkerShape<T>::getCollisionObject() const
118+
FCL_CollisionObject MarkerShape<T>::getCollisionObject() const
119119
{
120120
fcl::Transform3f x(fcl::Quaternion3f(this->marker_.pose.orientation.w,
121121
this->marker_.pose.orientation.x,
@@ -125,6 +125,8 @@ fcl::CollisionObject MarkerShape<T>::getCollisionObject() const
125125
this->marker_.pose.position.y,
126126
this->marker_.pose.position.z));
127127
fcl::CollisionObject cobj(this->ptr_fcl_bvh_, x);
128+
129+
FCL_CollisionObject cobj(this->ptr_fcl_bvh_, x);
128130
return cobj;
129131
}
130132

‎cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp‎

Lines changed: 15 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -21,8 +21,19 @@
2121
#include <boost/shared_ptr.hpp>
2222
#include <stdint.h>
2323
#include <visualization_msgs/Marker.h>
24-
#include <fcl/collision_object.h>
25-
#include <fcl/BVH/BVH_model.h>
24+
25+
#include <fcl/config.h>
26+
#if FCL_MINOR_VERSION == 5
27+
#include <fcl/collision_object.h>
28+
#include <fcl/BVH/BVH_model.h>
29+
typedef fcl::RSS FCL_RSS;
30+
typedef fcl::CollisionObject FCL_CollisionObject;
31+
#else
32+
#include <fcl/narrowphase/collision_object.h>
33+
#include <fcl/geometry/bvh/BVH_model.h>
34+
typedef fcl::RSSf FCL_RSS;
35+
typedef fcl::CollisionObjectf FCL_CollisionObject;
36+
#endif
2637

2738
/* BEGIN IMarkerShape *******************************************************************************************/
2839
/// Interface class marking methods that have to be implemented in derived classes.
@@ -41,7 +52,7 @@ class IMarkerShape
4152
virtual visualization_msgs::Marker getMarker() = 0;
4253
virtual void updatePose(const geometry_msgs::Vector3& pos, const geometry_msgs::Quaternion& quat) = 0;
4354
virtual void updatePose(const geometry_msgs::Pose& pose) = 0;
44-
virtual fcl::CollisionObject getCollisionObject() const = 0;
55+
virtual FCL_CollisionObject getCollisionObject() const = 0;
4556
virtual geometry_msgs::Pose getMarkerPose() const = 0;
4657
virtual geometry_msgs::Pose getOriginRelToFrame() const = 0;
4758

@@ -66,6 +77,6 @@ class IMarkerShape
6677
/* END IMarkerShape *********************************************************************************************/
6778

6879
typedef std::shared_ptr< IMarkerShape > PtrIMarkerShape_t;
69-
typedef fcl::BVHModel<fcl::RSS> BVH_RSS_t;
80+
typedef fcl::BVHModel<FCL_RSS> BVH_RSS_t;
7081

7182
#endif /* MARKER_SHAPES_INTERFACE_HPP_ */

‎cob_obstacle_distance/include/cob_obstacle_distance/obstacle_distance_data_types.hpp‎

Lines changed: 11 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,14 @@
2424
#include <shape_msgs/SolidPrimitive.h>
2525
#include <visualization_msgs/Marker.h>
2626

27+
#include <fcl/config.h>
28+
#include <fcl/common/types.h>
29+
#if FCL_MINOR_VERSION == 5
30+
typedef fcl::Vec3f FCL_Vec3;
31+
#else
32+
typedef fcl::Vector3f FCL_Vec3;
33+
#endif
34+
2735
#define FCL_BOX_X 0u
2836
#define FCL_BOX_Y 1u
2937
#define FCL_BOX_Z 2u
@@ -57,9 +65,9 @@ struct ShapeMsgTypeToVisMarkerType
5765

5866
struct TriangleSupport
5967
{
60-
fcl::Vec3f a;
61-
fcl::Vec3f b;
62-
fcl::Vec3f c;
68+
FCL_Vec3 a;
69+
FCL_Vec3 b;
70+
FCL_Vec3 c;
6371
};
6472

6573
static ShapeMsgTypeToVisMarkerType g_shapeMsgTypeToVisMarkerType;

‎cob_obstacle_distance/include/cob_obstacle_distance/parsers/mesh_parser.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@
2727
class MeshParser : public ParserBase
2828
{
2929
private:
30-
int8_t toVec3f(uint32_t num_current_face, aiVector3D* vertex, fcl::Vec3f& out);
30+
int8_t toVec3f(uint32_t num_current_face, aiVector3D* vertex, FCL_Vec3& out);
3131

3232
public:
3333
MeshParser(const std::string& file_path)

‎cob_obstacle_distance/include/cob_obstacle_distance/parsers/parser_base.hpp‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,14 @@
1919
#define PARSER_BASE_HPP_
2020

2121
#include <stdint.h>
22-
#include <fcl/BVH/BVH_model.h>
23-
#include <fcl/math/vec_3f.h>
22+
23+
#include <fcl/config.h>
24+
#include <fcl/common/types.h>
25+
#if FCL_MINOR_VERSION == 5
26+
#include <fcl/BVH/BVH_model.h>
27+
#else
28+
#include <fcl/geometry/bvh/BVH_model.h>
29+
#endif
2430

2531
#include "cob_obstacle_distance/obstacle_distance_data_types.hpp"
2632

‎cob_obstacle_distance/include/cob_obstacle_distance/parsers/stl_parser.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ class StlParser : public ParserBase
2626

2727
double toDouble(char* facet, uint8_t start_idx);
2828

29-
fcl::Vec3f toVec3f(char* facet);
29+
FCL_Vec3 toVec3f(char* facet);
3030

3131
public:
3232
StlParser(const std::string& file_path)

‎cob_obstacle_distance/package.xml‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,8 @@
2121
<depend>dynamic_reconfigure</depend>
2222
<depend>eigen_conversions</depend>
2323
<depend>eigen</depend>
24-
<depend>libfcl-dev</depend>
24+
<depend condition="$ROS_DISTRO != noetic">libfcl-dev</depend>
25+
<depend condition="$ROS_DISTRO == noetic">fcl</depend>
2526
<depend>geometry_msgs</depend>
2627
<depend>kdl_conversions</depend>
2728
<depend>kdl_parser</depend>

‎cob_obstacle_distance/src/cob_obstacle_distance.cpp‎

Lines changed: 19 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -18,10 +18,25 @@
1818
#include <ctime>
1919
#include <vector>
2020
#include <ros/ros.h>
21-
#include <fcl/shape/geometric_shapes.h>
2221
#include <fstream>
2322
#include <thread>
2423

24+
#include <fcl/config.h>
25+
#if FCL_MINOR_VERSION == 5
26+
#include <fcl/shape/geometric_shapes.h>
27+
typedef fcl::Box FCL_Box;
28+
typedef fcl::Sphere FCL_Sphere;
29+
typedef fcl::Cylinder FCL_Cylinder;
30+
#else
31+
#include <fcl/geometry/shape/box.h>
32+
#include <fcl/geometry/shape/sphere.h>
33+
#include <fcl/geometry/shape/cylinder.h>
34+
typedef fcl::Boxf FCL_Box;
35+
typedef fcl::Spheref FCL_Sphere;
36+
typedef fcl::Cylinderf FCL_Cylinder;
37+
#endif
38+
39+
2540
#include "cob_obstacle_distance/obstacle_distance_data_types.hpp"
2641
#include "cob_obstacle_distance/distance_manager.hpp"
2742
#include "cob_obstacle_distance/marker_shapes/marker_shapes.hpp"
@@ -70,16 +85,16 @@ int main(int argc, char** argv)
7085
*/
7186
void addTestObstacles(DistanceManager& dm)
7287
{
73-
fcl::Sphere s(0.1);
74-
fcl::Box b(0.1, 0.1, 0.1); // Take care the nearest point for collision is one of the eight corners!!! This might lead to jittering
88+
FCL_Sphere s(0.1);
89+
FCL_Box b(0.1, 0.1, 0.1); // Take care the nearest point for collision is one of the eight corners!!! This might lead to jittering
7590

7691
PtrIMarkerShape_t sptr_Bvh(new MarkerShape<BVH_RSS_t>(dm.getRootFrame(),
7792
"package://cob_gazebo_objects/Media/models/milk.dae",
7893
-0.35,
7994
-0.35,
8095
0.8));
8196

82-
PtrIMarkerShape_t sptr_Sphere(new MarkerShape<fcl::Sphere>(dm.getRootFrame(), s, 0.35, -0.35, 0.8));
97+
PtrIMarkerShape_t sptr_Sphere(new MarkerShape<FCL_Sphere>(dm.getRootFrame(), s, 0.35, -0.35, 0.8));
8398

8499
dm.addObstacle("Funny Sphere", sptr_Sphere);
85100
dm.addObstacle("Funny Mesh", sptr_Bvh);

0 commit comments

Comments
 (0)