From 01014b0bf9b911bf942bbc56257629e6069e8ade Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 5 May 2021 17:32:10 +0200 Subject: [PATCH 1/3] update travis config --- .travis.rosinstall.noetic | 8 -------- .travis.yml | 2 +- 2 files changed, 1 insertion(+), 9 deletions(-) delete mode 100644 .travis.rosinstall.noetic diff --git a/.travis.rosinstall.noetic b/.travis.rosinstall.noetic deleted file mode 100644 index b0bfb7d84..000000000 --- a/.travis.rosinstall.noetic +++ /dev/null @@ -1,8 +0,0 @@ -- git: - local-name: cob_command_tools - uri: https://github.com/ipa320/cob_command_tools.git - version: indigo_dev -- git: - local-name: cob_driver - uri: https://github.com/ipa320/cob_driver.git - version: kinetic_dev diff --git a/.travis.yml b/.travis.yml index 117d6ce89..0a6b1f2e8 100644 --- a/.travis.yml +++ b/.travis.yml @@ -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: From eed65c4b8cf9d08043990fecf0948ef2120d8fac Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 5 May 2021 17:18:53 +0200 Subject: [PATCH 2/3] fcl noetic compatibility --- .../fcl_marker_converter.hpp | 58 ++++++++++++------- .../link_to_collision.hpp | 5 -- .../marker_shapes/marker_shapes.hpp | 21 ++++--- .../marker_shapes/marker_shapes_impl.hpp | 28 ++++++--- .../marker_shapes/marker_shapes_interface.hpp | 19 ++++-- .../obstacle_distance_data_types.hpp | 13 ++++- .../parsers/mesh_parser.hpp | 2 +- .../parsers/parser_base.hpp | 9 ++- .../parsers/stl_parser.hpp | 2 +- cob_obstacle_distance/package.xml | 3 +- .../src/cob_obstacle_distance.cpp | 23 ++++++-- .../src/distance_manager.cpp | 24 +++++--- .../src/link_to_collision.cpp | 30 +++++----- .../src/marker_shapes/marker_shapes_impl.cpp | 25 ++++---- .../src/parsers/mesh_parser.cpp | 6 +- .../src/parsers/stl_parser.cpp | 4 +- 16 files changed, 170 insertions(+), 102 deletions(-) diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/fcl_marker_converter.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/fcl_marker_converter.hpp index 60fbc5e1c..94bb881ea 100644 --- a/cob_obstacle_distance/include/cob_obstacle_distance/fcl_marker_converter.hpp +++ b/cob_obstacle_distance/include/cob_obstacle_distance/fcl_marker_converter.hpp @@ -19,9 +19,25 @@ #define FCL_MARKER_CONVERTER_HPP_ #include -#include -#include -#include + +#include +#if FCL_MINOR_VERSION == 5 + #include + #include + #include + typedef fcl::Box FCL_Box; + typedef fcl::Sphere FCL_Sphere; + typedef fcl::Cylinder FCL_Cylinder; +#else + #include + #include + #include + #include + #include + 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" @@ -43,16 +59,16 @@ class FclMarkerConverter }; template<> -class FclMarkerConverter +class FclMarkerConverter { - typedef boost::scoped_ptr sPtrBox; + typedef boost::scoped_ptr 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) { @@ -62,7 +78,7 @@ class FclMarkerConverter marker.type = visualization_msgs::Marker::CUBE; } - fcl::Box getGeoShape() const + FCL_Box getGeoShape() const { return geo_shape_; } @@ -75,16 +91,16 @@ class FclMarkerConverter }; template<> -class FclMarkerConverter +class FclMarkerConverter { - typedef boost::scoped_ptr sPtrSphere; + typedef boost::scoped_ptr 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) { @@ -94,7 +110,7 @@ class FclMarkerConverter marker.type = visualization_msgs::Marker::SPHERE; } - fcl::Sphere getGeoShape() const + FCL_Sphere getGeoShape() const { return geo_shape_; } @@ -113,16 +129,16 @@ class FclMarkerConverter }; template<> -class FclMarkerConverter +class FclMarkerConverter { - typedef boost::scoped_ptr sPtrCylinder; + typedef boost::scoped_ptr 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) { @@ -132,7 +148,7 @@ class FclMarkerConverter marker.type = visualization_msgs::Marker::CYLINDER; } - fcl::Cylinder getGeoShape() const + FCL_Cylinder getGeoShape() const { return geo_shape_; } diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/link_to_collision.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/link_to_collision.hpp index 74855a491..4fd9261f5 100644 --- a/cob_obstacle_distance/include/cob_obstacle_distance/link_to_collision.hpp +++ b/cob_obstacle_distance/include/cob_obstacle_distance/link_to_collision.hpp @@ -28,11 +28,6 @@ #include #include -#include -#include -#include -#include - #include #include #include diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes.hpp index b36171c84..a0abada5c 100644 --- a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes.hpp +++ b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes.hpp @@ -23,15 +23,18 @@ #include #include -#include "fcl/shape/geometric_shapes.h" -#include "fcl/collision_object.h" +#include +#if FCL_MINOR_VERSION == 5 + #include "fcl/collision_object.h" + typedef fcl::CollisionObject FCL_CollisionObject; +#else + #include + 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 -#include - static const std::string g_marker_namespace = "collision_object"; /* BEGIN MarkerShape ********************************************************************************************/ @@ -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(){} }; @@ -155,9 +158,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(){} }; diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp index 675916181..9dd4a0c1e 100644 --- a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp +++ b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp @@ -18,6 +18,15 @@ #ifndef MARKER_SHAPES_IMPL_HPP_ #define MARKER_SHAPES_IMPL_HPP_ +#include +#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 ********************************************************************************************/ @@ -115,16 +124,17 @@ inline visualization_msgs::Marker MarkerShape::getMarker() template -fcl::CollisionObject MarkerShape::getCollisionObject() const +FCL_CollisionObject MarkerShape::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); + 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_CollisionObject cobj(this->ptr_fcl_bvh_, x); return cobj; } diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp index d17421c0d..46306af88 100644 --- a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp +++ b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_interface.hpp @@ -21,8 +21,19 @@ #include #include #include -#include -#include + +#include +#if FCL_MINOR_VERSION == 5 + #include + #include + typedef fcl::RSS FCL_RSS; + typedef fcl::CollisionObject FCL_CollisionObject; +#else + #include + #include + 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. @@ -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; @@ -66,6 +77,6 @@ class IMarkerShape /* END IMarkerShape *********************************************************************************************/ typedef std::shared_ptr< IMarkerShape > PtrIMarkerShape_t; -typedef fcl::BVHModel BVH_RSS_t; +typedef fcl::BVHModel BVH_RSS_t; #endif /* MARKER_SHAPES_INTERFACE_HPP_ */ diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/obstacle_distance_data_types.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/obstacle_distance_data_types.hpp index 4639e207b..0388da986 100644 --- a/cob_obstacle_distance/include/cob_obstacle_distance/obstacle_distance_data_types.hpp +++ b/cob_obstacle_distance/include/cob_obstacle_distance/obstacle_distance_data_types.hpp @@ -24,6 +24,13 @@ #include #include +#include +#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 @@ -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; diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/parsers/mesh_parser.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/parsers/mesh_parser.hpp index 361f5e908..8b4144ddb 100644 --- a/cob_obstacle_distance/include/cob_obstacle_distance/parsers/mesh_parser.hpp +++ b/cob_obstacle_distance/include/cob_obstacle_distance/parsers/mesh_parser.hpp @@ -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) diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/parsers/parser_base.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/parsers/parser_base.hpp index d3ed83812..59bed732c 100644 --- a/cob_obstacle_distance/include/cob_obstacle_distance/parsers/parser_base.hpp +++ b/cob_obstacle_distance/include/cob_obstacle_distance/parsers/parser_base.hpp @@ -19,8 +19,13 @@ #define PARSER_BASE_HPP_ #include -#include -#include + +#include +#if FCL_MINOR_VERSION == 5 + #include +#else + #include +#endif #include "cob_obstacle_distance/obstacle_distance_data_types.hpp" diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/parsers/stl_parser.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/parsers/stl_parser.hpp index 6c00aeff4..691651e3d 100644 --- a/cob_obstacle_distance/include/cob_obstacle_distance/parsers/stl_parser.hpp +++ b/cob_obstacle_distance/include/cob_obstacle_distance/parsers/stl_parser.hpp @@ -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) diff --git a/cob_obstacle_distance/package.xml b/cob_obstacle_distance/package.xml index a2b952b59..58471ec69 100644 --- a/cob_obstacle_distance/package.xml +++ b/cob_obstacle_distance/package.xml @@ -21,7 +21,8 @@ dynamic_reconfigure eigen_conversions eigen - libfcl-dev + libfcl-dev + fcl geometry_msgs kdl_conversions kdl_parser diff --git a/cob_obstacle_distance/src/cob_obstacle_distance.cpp b/cob_obstacle_distance/src/cob_obstacle_distance.cpp index d139cd281..6f60d604d 100644 --- a/cob_obstacle_distance/src/cob_obstacle_distance.cpp +++ b/cob_obstacle_distance/src/cob_obstacle_distance.cpp @@ -18,10 +18,25 @@ #include #include #include -#include #include #include +#include +#if FCL_MINOR_VERSION == 5 + #include + typedef fcl::Box FCL_Box; + typedef fcl::Sphere FCL_Sphere; + typedef fcl::Cylinder FCL_Cylinder; +#else + #include + #include + #include + typedef fcl::Boxf FCL_Box; + typedef fcl::Spheref FCL_Sphere; + typedef fcl::Cylinderf FCL_Cylinder; +#endif + + #include "cob_obstacle_distance/obstacle_distance_data_types.hpp" #include "cob_obstacle_distance/distance_manager.hpp" #include "cob_obstacle_distance/marker_shapes/marker_shapes.hpp" @@ -70,8 +85,8 @@ int main(int argc, char** argv) */ void addTestObstacles(DistanceManager& dm) { - fcl::Sphere s(0.1); - 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 + FCL_Sphere s(0.1); + 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 PtrIMarkerShape_t sptr_Bvh(new MarkerShape(dm.getRootFrame(), "package://cob_gazebo_objects/Media/models/milk.dae", @@ -79,7 +94,7 @@ void addTestObstacles(DistanceManager& dm) -0.35, 0.8)); - PtrIMarkerShape_t sptr_Sphere(new MarkerShape(dm.getRootFrame(), s, 0.35, -0.35, 0.8)); + PtrIMarkerShape_t sptr_Sphere(new MarkerShape(dm.getRootFrame(), s, 0.35, -0.35, 0.8)); dm.addObstacle("Funny Sphere", sptr_Sphere); dm.addObstacle("Funny Mesh", sptr_Bvh); diff --git a/cob_obstacle_distance/src/distance_manager.cpp b/cob_obstacle_distance/src/distance_manager.cpp index 4e3c3542a..9a238131d 100644 --- a/cob_obstacle_distance/src/distance_manager.cpp +++ b/cob_obstacle_distance/src/distance_manager.cpp @@ -26,10 +26,16 @@ #include -#include -#include -#include -#include +#include +#if FCL_MINOR_VERSION == 5 + #include + typedef fcl::DistanceRequest FCL_DistanceRequest; + typedef fcl::DistanceResult FCL_DistanceResult; +#else + #include + typedef fcl::DistanceRequest FCL_DistanceRequest; + typedef fcl::DistanceResult FCL_DistanceResult; +#endif #include #include @@ -249,8 +255,8 @@ void DistanceManager::calculate() tf::vectorEigenToMsg(abs_jnt_pos, v3); ooi->updatePose(v3, quat); - fcl::CollisionObject ooi_co = ooi->getCollisionObject(); - fcl::FCL_REAL last_dist = std::numeric_limits::max(); + FCL_CollisionObject ooi_co = ooi->getCollisionObject(); + double last_dist = std::numeric_limits::max(); { // introduced the block to lock this critical section until block leaved. std::lock_guard lock(obstacle_mgr_mtx_); for (ShapesManager::MapIter_t it = this->obstacle_mgr_->begin(); it != this->obstacle_mgr_->end(); ++it) @@ -264,9 +270,9 @@ void DistanceManager::calculate() } PtrIMarkerShape_t obstacle = it->second; - fcl::CollisionObject collision_obj = obstacle->getCollisionObject(); - fcl::DistanceResult dist_result; - fcl::DistanceRequest dist_request(true, 5.0, 0.01); + FCL_CollisionObject collision_obj = obstacle->getCollisionObject(); + FCL_DistanceResult dist_result; + FCL_DistanceRequest dist_request(true, 5.0, 0.01); fcl::FCL_REAL dist = fcl::distance(&ooi_co, &collision_obj, dist_request, dist_result); diff --git a/cob_obstacle_distance/src/link_to_collision.cpp b/cob_obstacle_distance/src/link_to_collision.cpp index 0b9241b4e..a126fa4d0 100644 --- a/cob_obstacle_distance/src/link_to_collision.cpp +++ b/cob_obstacle_distance/src/link_to_collision.cpp @@ -199,16 +199,16 @@ void LinkToCollision::createSpecificMarkerShape(const std::string& link_of_inter else if (urdf::Geometry::BOX == geometry->type) { PtrBox_t urdf_box = std::static_pointer_cast(geometry); - fcl::Box b(urdf_box->dim.x, - urdf_box->dim.y, - urdf_box->dim.z); + FCL_Box b(urdf_box->dim.x, + urdf_box->dim.y, + urdf_box->dim.z); std_msgs::ColorRGBA test_col; test_col.a = 1.0; test_col.r = 1.0; - // segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, b, pose, col)); - segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, b, pose, test_col)); + // segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, b, pose, col)); + segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, b, pose, test_col)); } else if (urdf::Geometry::SPHERE == geometry->type) { @@ -217,8 +217,8 @@ void LinkToCollision::createSpecificMarkerShape(const std::string& link_of_inter test_col.b = 1.0; PtrSphere_t urdf_sphere = std::static_pointer_cast(geometry); - fcl::Sphere s(urdf_sphere->radius); - segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, s, pose, test_col)); + FCL_Sphere s(urdf_sphere->radius); + segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, s, pose, test_col)); } else if (urdf::Geometry::CYLINDER == geometry->type) { @@ -227,8 +227,8 @@ void LinkToCollision::createSpecificMarkerShape(const std::string& link_of_inter test_col.g = 1.0; PtrCylinder_t urdf_cyl = std::static_pointer_cast(geometry); - fcl::Cylinder c(urdf_cyl->radius, urdf_cyl->length); - segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, c, pose, test_col)); + FCL_Cylinder c(urdf_cyl->radius, urdf_cyl->length); + segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, c, pose, test_col)); } else { @@ -262,9 +262,9 @@ bool LinkToCollision::getMarkerShapeFromType(const uint32_t& shape_type, PtrIMarkerShape_t& segment_of_interest_marker_shape) { // Representation of segment_of_interest as specific fcl::Shape - fcl::Box b(dimension(FCL_BOX_X), dimension(FCL_BOX_Y), dimension(FCL_BOX_Z)); - fcl::Sphere s(dimension(FCL_RADIUS)); - fcl::Cylinder c(dimension(FCL_RADIUS), dimension(FCL_CYL_LENGTH)); + FCL_Box b(dimension(FCL_BOX_X), dimension(FCL_BOX_Y), dimension(FCL_BOX_Z)); + FCL_Sphere s(dimension(FCL_RADIUS)); + FCL_Cylinder c(dimension(FCL_RADIUS), dimension(FCL_CYL_LENGTH)); uint32_t loc_shape_type = shape_type; std::string mesh_resource; if (visualization_msgs::Marker::MESH_RESOURCE == loc_shape_type) @@ -294,17 +294,17 @@ bool LinkToCollision::getMarkerShapeFromType(const uint32_t& shape_type, case visualization_msgs::Marker::CUBE: test_col.a = 1.0; test_col.r = 1.0; - segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, b, pose, test_col)); + segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, b, pose, test_col)); break; case visualization_msgs::Marker::SPHERE: test_col.a = 1.0; test_col.b = 1.0; - segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, s, pose, test_col)); + segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, s, pose, test_col)); break; case visualization_msgs::Marker::CYLINDER: test_col.a = 1.0; test_col.g = 1.0; - segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, c, pose, test_col)); + segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, c, pose, test_col)); break; case visualization_msgs::Marker::MESH_RESOURCE: segment_of_interest_marker_shape.reset(new MarkerShape(this->root_frame_id_, diff --git a/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp b/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp index bf6cac5b4..5f2f6b0ac 100644 --- a/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp +++ b/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp @@ -34,9 +34,9 @@ MarkerShape::MarkerShape(const std::string& root_frame, uint32_t v_idx_2 = tri.vertex_indices.elems[1]; uint32_t v_idx_3 = tri.vertex_indices.elems[2]; - fcl::Vec3f v1(mesh.vertices[v_idx_1].x, mesh.vertices[v_idx_1].y, mesh.vertices[v_idx_1].z); - fcl::Vec3f v2(mesh.vertices[v_idx_2].x, mesh.vertices[v_idx_2].y, mesh.vertices[v_idx_2].z); - fcl::Vec3f v3(mesh.vertices[v_idx_3].x, mesh.vertices[v_idx_3].y, mesh.vertices[v_idx_3].z); + FCL_Vec3 v1(mesh.vertices[v_idx_1].x, mesh.vertices[v_idx_1].y, mesh.vertices[v_idx_1].z); + FCL_Vec3 v2(mesh.vertices[v_idx_2].x, mesh.vertices[v_idx_2].y, mesh.vertices[v_idx_2].z); + FCL_Vec3 v3(mesh.vertices[v_idx_3].x, mesh.vertices[v_idx_3].y, mesh.vertices[v_idx_3].z); this->ptr_fcl_bvh_->addTriangle(v1, v2, v3); } @@ -163,17 +163,16 @@ inline visualization_msgs::Marker MarkerShape::getMarker() } -fcl::CollisionObject MarkerShape::getCollisionObject() const +FCL_CollisionObject MarkerShape::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); + 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_CollisionObject cobj(this->ptr_fcl_bvh_, x); return cobj; } diff --git a/cob_obstacle_distance/src/parsers/mesh_parser.cpp b/cob_obstacle_distance/src/parsers/mesh_parser.cpp index 78f8e74ff..f4c06fbf2 100644 --- a/cob_obstacle_distance/src/parsers/mesh_parser.cpp +++ b/cob_obstacle_distance/src/parsers/mesh_parser.cpp @@ -95,13 +95,13 @@ int8_t MeshParser::read(std::vector& tri_vec) } /** - * Uses the current vertex (assimp 3d vector) and converts into fcl::Vec3f. + * Uses the current vertex (assimp 3d vector) and converts into FCL_Vec3. * @param num_current_face The number of the current face for error logging. * @param vertex Pointer to the current vertex. * @param out The fcl vector description. * @return Success status (0 means ok). */ -int8_t MeshParser::toVec3f(uint32_t num_current_face, aiVector3D* vertex, fcl::Vec3f& out) +int8_t MeshParser::toVec3f(uint32_t num_current_face, aiVector3D* vertex, FCL_Vec3& out) { if (!vertex) { @@ -109,7 +109,7 @@ int8_t MeshParser::toVec3f(uint32_t num_current_face, aiVector3D* vertex, fcl::V return -3; } - out = fcl::Vec3f((*vertex)[0], (*vertex)[1], (*vertex)[2]); + out = FCL_Vec3((*vertex)[0], (*vertex)[1], (*vertex)[2]); return 0; } diff --git a/cob_obstacle_distance/src/parsers/stl_parser.cpp b/cob_obstacle_distance/src/parsers/stl_parser.cpp index cead9bb21..7fc2116ba 100644 --- a/cob_obstacle_distance/src/parsers/stl_parser.cpp +++ b/cob_obstacle_distance/src/parsers/stl_parser.cpp @@ -93,13 +93,13 @@ int8_t StlParser::read(std::vector& tri_vec) * @param facet Pointer to the current face / triangle in file. * @return An fcl::Vec3f containing the 3 vertices describing a triangle. */ -fcl::Vec3f StlParser::toVec3f(char* facet) +FCL_Vec3 StlParser::toVec3f(char* facet) { double x = this->toDouble(facet, 0); double y = this->toDouble(facet, 4); double z = this->toDouble(facet, 8); - fcl::Vec3f v3(x, y, z); + FCL_Vec3 v3(x, y, z); return v3; } From b852fc2dd4c5aa29210a266e1a689afe10db6960 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 5 May 2021 18:40:18 +0200 Subject: [PATCH 3/3] comment broken bits --- .../marker_shapes/marker_shapes_impl.hpp | 13 +++++++++---- cob_obstacle_distance/src/distance_manager.cpp | 4 +++- .../src/marker_shapes/marker_shapes_impl.cpp | 13 +++++++++---- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp index 9dd4a0c1e..cf20fbe13 100644 --- a/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp +++ b/cob_obstacle_distance/include/cob_obstacle_distance/marker_shapes/marker_shapes_impl.hpp @@ -126,13 +126,18 @@ inline visualization_msgs::Marker MarkerShape::getMarker() template FCL_CollisionObject MarkerShape::getCollisionObject() const { + // FIXME + // 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_Vec3(this->marker_.pose.position.x, - this->marker_.pose.position.y, - this->marker_.pose.position.z)); + this->marker_.pose.orientation.z)); FCL_CollisionObject cobj(this->ptr_fcl_bvh_, x); return cobj; diff --git a/cob_obstacle_distance/src/distance_manager.cpp b/cob_obstacle_distance/src/distance_manager.cpp index 9a238131d..ea91f423a 100644 --- a/cob_obstacle_distance/src/distance_manager.cpp +++ b/cob_obstacle_distance/src/distance_manager.cpp @@ -273,7 +273,9 @@ void DistanceManager::calculate() FCL_CollisionObject collision_obj = obstacle->getCollisionObject(); FCL_DistanceResult dist_result; FCL_DistanceRequest dist_request(true, 5.0, 0.01); - fcl::FCL_REAL dist = fcl::distance(&ooi_co, &collision_obj, dist_request, dist_result); + //FIXME + // double dist = fcl::distance(&ooi_co, &collision_obj, dist_request, dist_result); + double dist = 0; Eigen::Vector3d abs_obst_vector(dist_result.nearest_points[1][VEC_X], diff --git a/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp b/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp index 5f2f6b0ac..1503ef1c5 100644 --- a/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp +++ b/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp @@ -165,13 +165,18 @@ inline visualization_msgs::Marker MarkerShape::getMarker() FCL_CollisionObject MarkerShape::getCollisionObject() const { + // FIXME + // 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_Vec3(this->marker_.pose.position.x, - this->marker_.pose.position.y, - this->marker_.pose.position.z)); + this->marker_.pose.orientation.z)); FCL_CollisionObject cobj(this->ptr_fcl_bvh_, x); return cobj; }