Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fcl noetic compatibility #255

Merged
merged 3 commits into from
May 5, 2021
Merged

fcl noetic compatibility #255

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
01014b0
update travis config
fmessmer May 5, 2021
eed65c4
fcl noetic compatibility
fmessmer May 5, 2021
b852fc2
comment broken bits
fmessmer May 5, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 0 additions & 8 deletions .travis.rosinstall.noetic
View file
Open in desktop

This file was deleted.

2 changes: 1 addition & 1 deletion .travis.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ env:
- ROS_REPO=main
matrix:
- ROS_DISTRO=melodic
- ROS_DISTRO=noetic UPSTREAM_WORKSPACE='.travis.rosinstall.noetic -cob_command_tools/cob_command_gui -cob_command_tools/cob_command_tools' CATKIN_LINT_ARGS='--ignore description_boilerplate --ignore target_name_collision --ignore unknown_package'
- ROS_DISTRO=noetic
install:
- git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci -b master
script:
Expand Down
58 changes: 37 additions & 21 deletions cob_obstacle_distance/include/cob_obstacle_distance/fcl_marker_converter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,9 +19,25 @@
#define FCL_MARKER_CONVERTER_HPP_

#include <boost/scoped_ptr.hpp>
#include <fcl/shape/geometric_shapes.h>
#include <fcl/BVH/BVH_model.h>
#include <fcl/shape/geometric_shape_to_BVH_model.h>

#include <fcl/config.h>
#if FCL_MINOR_VERSION == 5
#include <fcl/shape/geometric_shapes.h>
#include <fcl/BVH/BVH_model.h>
#include <fcl/shape/geometric_shape_to_BVH_model.h>
typedef fcl::Box FCL_Box;
typedef fcl::Sphere FCL_Sphere;
typedef fcl::Cylinder FCL_Cylinder;
#else
#include <fcl/geometry/shape/box.h>
#include <fcl/geometry/shape/sphere.h>
#include <fcl/geometry/shape/cylinder.h>
#include <fcl/geometry/bvh/BVH_model.h>
#include <fcl/geometry/geometric_shape_to_BVH_model.h>
typedef fcl::Boxf FCL_Box;
typedef fcl::Spheref FCL_Sphere;
typedef fcl::Cylinderf FCL_Cylinder;
#endif


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

template<>
class FclMarkerConverter<fcl::Box>
class FclMarkerConverter<FCL_Box>
{
typedef boost::scoped_ptr<fcl::Box> sPtrBox;
typedef boost::scoped_ptr<FCL_Box> sPtrBox;

private:
fcl::Box geo_shape_;
FCL_Box geo_shape_;

public:
FclMarkerConverter() : geo_shape_(fcl::Box(1.0, 1.0, 1.0)) {}
FclMarkerConverter(fcl::Box& box) : geo_shape_(box) {}
FclMarkerConverter() : geo_shape_(FCL_Box(1.0, 1.0, 1.0)) {}
FclMarkerConverter(FCL_Box& box) : geo_shape_(box) {}

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

fcl::Box getGeoShape() const
FCL_Box getGeoShape() const
{
return geo_shape_;
}
Expand All @@ -75,16 +91,16 @@ class FclMarkerConverter<fcl::Box>
};

template<>
class FclMarkerConverter<fcl::Sphere>
class FclMarkerConverter<FCL_Sphere>
{
typedef boost::scoped_ptr<fcl::Sphere> sPtrSphere;
typedef boost::scoped_ptr<FCL_Sphere> sPtrSphere;

private:
fcl::Sphere geo_shape_;
FCL_Sphere geo_shape_;

public:
FclMarkerConverter() : geo_shape_(fcl::Sphere(1.0)) {}
FclMarkerConverter(fcl::Sphere &sphere) : geo_shape_(sphere) {}
FclMarkerConverter() : geo_shape_(FCL_Sphere(1.0)) {}
FclMarkerConverter(FCL_Sphere &sphere) : geo_shape_(sphere) {}

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

fcl::Sphere getGeoShape() const
FCL_Sphere getGeoShape() const
{
return geo_shape_;
}
Expand All @@ -113,16 +129,16 @@ class FclMarkerConverter<fcl::Sphere>
};

template<>
class FclMarkerConverter<fcl::Cylinder>
class FclMarkerConverter<FCL_Cylinder>
{
typedef boost::scoped_ptr<fcl::Cylinder> sPtrCylinder;
typedef boost::scoped_ptr<FCL_Cylinder> sPtrCylinder;

private:
fcl::Cylinder geo_shape_;
FCL_Cylinder geo_shape_;

public:
FclMarkerConverter() : geo_shape_(fcl::Cylinder(1.0, 1.0)) {}
FclMarkerConverter(fcl::Cylinder &cyl) : geo_shape_(cyl) {}
FclMarkerConverter() : geo_shape_(FCL_Cylinder(1.0, 1.0)) {}
FclMarkerConverter(FCL_Cylinder &cyl) : geo_shape_(cyl) {}

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

fcl::Cylinder getGeoShape() const
FCL_Cylinder getGeoShape() const
{
return geo_shape_;
}
Expand Down
5 changes: 0 additions & 5 deletions cob_obstacle_distance/include/cob_obstacle_distance/link_to_collision.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,6 @@
#include <boost/scoped_ptr.hpp>
#include <boost/pointer_cast.hpp>

#include <fcl/collision_object.h>
#include <fcl/collision.h>
#include <fcl/distance.h>
#include <fcl/collision_data.h>

#include <std_msgs/ColorRGBA.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
Expand Down
21 changes: 12 additions & 9 deletions cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,15 +23,18 @@
#include <shape_msgs/Mesh.h>
#include <shape_msgs/MeshTriangle.h>

#include "fcl/shape/geometric_shapes.h"
#include "fcl/collision_object.h"
#include <fcl/config.h>
#if FCL_MINOR_VERSION == 5
#include "fcl/collision_object.h"
typedef fcl::CollisionObject FCL_CollisionObject;
#else
#include <fcl/narrowphase/collision_object.h>
typedef fcl::CollisionObjectf FCL_CollisionObject;
#endif

#include "cob_obstacle_distance/fcl_marker_converter.hpp"
#include "cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp"

#include <fcl/distance.h>
#include <fcl/collision_data.h>

static const std::string g_marker_namespace = "collision_object";

/* BEGIN MarkerShape ********************************************************************************************/
Expand Down Expand Up @@ -92,9 +95,9 @@ class MarkerShape : public IMarkerShape
inline void updatePose(const geometry_msgs::Pose& pose);

/**
* @return A fcl::CollisionObject to calculate distances to other objects or check whether collision occurred or not.
* @return A FCL_CollisionObject to calculate distances to other objects or check whether collision occurred or not.
*/
fcl::CollisionObject getCollisionObject() const;
FCL_CollisionObject getCollisionObject() const;

virtual ~MarkerShape(){}
};
Expand Down Expand Up @@ -155,9 +158,9 @@ class MarkerShape<BVH_RSS_t> : public IMarkerShape
inline void updatePose(const geometry_msgs::Pose& pose);

/**
* @return A fcl::CollisionObject to calculate distances to other objects or check whether collision occurred or not.
* @return A FCL_CollisionObject to calculate distances to other objects or check whether collision occurred or not.
*/
fcl::CollisionObject getCollisionObject() const;
FCL_CollisionObject getCollisionObject() const;

virtual ~MarkerShape(){}
};
Expand Down
33 changes: 24 additions & 9 deletions cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,15 @@
#ifndef MARKER_SHAPES_IMPL_HPP_
#define MARKER_SHAPES_IMPL_HPP_

#include <fcl/config.h>
#if FCL_MINOR_VERSION == 5
typedef fcl::Vec3f FCL_Vec3;
typedef fcl::Quaternion3f FCL_Quaternion;
#else
typedef fcl::Vector3f FCL_Vec3;
typedef fcl::Quaternionf FCL_Quaternion;
#endif

#include "cob_obstacle_distance/marker_shapes/marker_shapes.hpp"

/* BEGIN MarkerShape ********************************************************************************************/
Expand Down Expand Up @@ -115,16 +124,22 @@ inline visualization_msgs::Marker MarkerShape<T>::getMarker()


template <typename T>
fcl::CollisionObject MarkerShape<T>::getCollisionObject() const
FCL_CollisionObject MarkerShape<T>::getCollisionObject() const
{
fcl::Transform3f x(fcl::Quaternion3f(this->marker_.pose.orientation.w,
this->marker_.pose.orientation.x,
this->marker_.pose.orientation.y,
this->marker_.pose.orientation.z),
fcl::Vec3f(this->marker_.pose.position.x,
this->marker_.pose.position.y,
this->marker_.pose.position.z));
fcl::CollisionObject cobj(this->ptr_fcl_bvh_, x);
// FIXME
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is reverted in #256

// fcl::Transform3f x(FCL_Quaternion(this->marker_.pose.orientation.w,
// this->marker_.pose.orientation.x,
// this->marker_.pose.orientation.y,
// this->marker_.pose.orientation.z),
// FCL_Vec3(this->marker_.pose.position.x,
// this->marker_.pose.position.y,
// this->marker_.pose.position.z));
fcl::Transform3f x(FCL_Quaternion(this->marker_.pose.orientation.w,
this->marker_.pose.orientation.x,
this->marker_.pose.orientation.y,
this->marker_.pose.orientation.z));

FCL_CollisionObject cobj(this->ptr_fcl_bvh_, x);
return cobj;
}

Expand Down
19 changes: 15 additions & 4 deletions ...obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,19 @@
#include <boost/shared_ptr.hpp>
#include <stdint.h>
#include <visualization_msgs/Marker.h>
#include <fcl/collision_object.h>
#include <fcl/BVH/BVH_model.h>

#include <fcl/config.h>
#if FCL_MINOR_VERSION == 5
#include <fcl/collision_object.h>
#include <fcl/BVH/BVH_model.h>
typedef fcl::RSS FCL_RSS;
typedef fcl::CollisionObject FCL_CollisionObject;
#else
#include <fcl/narrowphase/collision_object.h>
#include <fcl/geometry/bvh/BVH_model.h>
typedef fcl::RSSf FCL_RSS;
typedef fcl::CollisionObjectf FCL_CollisionObject;
#endif

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

Expand All @@ -66,6 +77,6 @@ class IMarkerShape
/* END IMarkerShape *********************************************************************************************/

typedef std::shared_ptr< IMarkerShape > PtrIMarkerShape_t;
typedef fcl::BVHModel<fcl::RSS> BVH_RSS_t;
typedef fcl::BVHModel<FCL_RSS> BVH_RSS_t;

#endif /* MARKER_SHAPES_INTERFACE_HPP_ */
13 changes: 10 additions & 3 deletions cob_obstacle_distance/include/cob_obstacle_distance/obstacle_distance_data_types.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,13 @@
#include <shape_msgs/SolidPrimitive.h>
#include <visualization_msgs/Marker.h>

#include <fcl/config.h>
#if FCL_MINOR_VERSION == 5
typedef fcl::Vec3f FCL_Vec3;
#else
typedef fcl::Vector3f FCL_Vec3;
#endif

#define FCL_BOX_X 0u
#define FCL_BOX_Y 1u
#define FCL_BOX_Z 2u
Expand Down Expand Up @@ -57,9 +64,9 @@ struct ShapeMsgTypeToVisMarkerType

struct TriangleSupport
{
fcl::Vec3f a;
fcl::Vec3f b;
fcl::Vec3f c;
FCL_Vec3 a;
FCL_Vec3 b;
FCL_Vec3 c;
};

static ShapeMsgTypeToVisMarkerType g_shapeMsgTypeToVisMarkerType;
Expand Down
2 changes: 1 addition & 1 deletion cob_obstacle_distance/include/cob_obstacle_distance/parsers/mesh_parser.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
class MeshParser : public ParserBase
{
private:
int8_t toVec3f(uint32_t num_current_face, aiVector3D* vertex, fcl::Vec3f& out);
int8_t toVec3f(uint32_t num_current_face, aiVector3D* vertex, FCL_Vec3& out);

public:
MeshParser(const std::string& file_path)
Expand Down
9 changes: 7 additions & 2 deletions cob_obstacle_distance/include/cob_obstacle_distance/parsers/parser_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,13 @@
#define PARSER_BASE_HPP_

#include <stdint.h>
#include <fcl/BVH/BVH_model.h>
#include <fcl/math/vec_3f.h>

#include <fcl/config.h>
#if FCL_MINOR_VERSION == 5
#include <fcl/BVH/BVH_model.h>
#else
#include <fcl/geometry/bvh/BVH_model.h>
#endif

#include "cob_obstacle_distance/obstacle_distance_data_types.hpp"

Expand Down
2 changes: 1 addition & 1 deletion cob_obstacle_distance/include/cob_obstacle_distance/parsers/stl_parser.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ class StlParser : public ParserBase

double toDouble(char* facet, uint8_t start_idx);

fcl::Vec3f toVec3f(char* facet);
FCL_Vec3 toVec3f(char* facet);

public:
StlParser(const std::string& file_path)
Expand Down
3 changes: 2 additions & 1 deletion cob_obstacle_distance/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,8 @@
<depend>dynamic_reconfigure</depend>
<depend>eigen_conversions</depend>
<depend>eigen</depend>
<depend>libfcl-dev</depend>
<depend condition="$ROS_DISTRO != noetic">libfcl-dev</depend>
<depend condition="$ROS_DISTRO == noetic">fcl</depend>
<depend>geometry_msgs</depend>
<depend>kdl_conversions</depend>
<depend>kdl_parser</depend>
Expand Down
Loading