From 48487bc3ae6f9b44d1cd70f73caa35f5d906b5f8 Mon Sep 17 00:00:00 2001 From: fmessmer Date: Wed, 5 May 2021 18:42:30 +0200 Subject: [PATCH] Revert "comment broken bits" This reverts commit b852fc2dd4c5aa29210a266e1a689afe10db6960. --- .../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, 9 insertions(+), 21 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 cf20fbe1..9dd4a0c1 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,18 +126,13 @@ 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)); + 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/distance_manager.cpp b/cob_obstacle_distance/src/distance_manager.cpp index ea91f423..9a238131 100644 --- a/cob_obstacle_distance/src/distance_manager.cpp +++ b/cob_obstacle_distance/src/distance_manager.cpp @@ -273,9 +273,7 @@ void DistanceManager::calculate() FCL_CollisionObject collision_obj = obstacle->getCollisionObject(); FCL_DistanceResult dist_result; FCL_DistanceRequest dist_request(true, 5.0, 0.01); - //FIXME - // double dist = fcl::distance(&ooi_co, &collision_obj, dist_request, dist_result); - double dist = 0; + fcl::FCL_REAL dist = fcl::distance(&ooi_co, &collision_obj, dist_request, dist_result); 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 1503ef1c..5f2f6b0a 100644 --- a/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp +++ b/cob_obstacle_distance/src/marker_shapes/marker_shapes_impl.cpp @@ -165,18 +165,13 @@ 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)); + 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; }