Skip to content

Commit

Permalink
ABI compatibility for melodic.
Browse files Browse the repository at this point in the history
  • Loading branch information
@peci1
peci1 committed Apr 26, 2020
1 parent ceb6a42 commit ed4cf13
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 11 deletions.
9 changes: 4 additions & 5 deletions include/geometric_shapes/bodies.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -496,7 +496,9 @@ class Box : public Body

// pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
Eigen::Vector3d center_;
Eigen::Matrix3d invRot_;
Eigen::Vector3d normalL_;
Eigen::Vector3d normalW_;
Eigen::Vector3d normalH_;

Eigen::Vector3d corner1_; //!< The translated, but not rotated min corner
Eigen::Vector3d corner2_; //!< The translated, but not rotated max corner
Expand Down Expand Up @@ -528,9 +530,7 @@ class ConvexMesh : public Body
setDimensions(shape);
}

virtual ~ConvexMesh()
{
}
virtual ~ConvexMesh();

/** \brief Returns an empty vector */
virtual std::vector<double> getDimensions() const;
Expand Down Expand Up @@ -583,7 +583,6 @@ class ConvexMesh : public Body
EigenSTL::vector_Vector3d vertices_;
std::vector<unsigned int> triangles_;
std::map<unsigned int, unsigned int> plane_for_triangle_;
std::map<unsigned int, unsigned int> triangle_for_plane_;
Eigen::Vector3d mesh_center_;
double mesh_radiusB_;
Eigen::Vector3d box_offset_;
Expand Down
33 changes: 27 additions & 6 deletions src/bodies.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,10 @@ void filterIntersections(std::vector<detail::intersc>& ipts, EigenSTL::vector_Ve
intersections->push_back(p.pt);
}
}

// HACK: The global map g_triangle_for_plane_ is needed for ABI compatibility with the melodic version of
// geometric_shapes; in newer releases, it should instead be added as a member to ConvexMesh::MeshData.
std::unordered_map<const ConvexMesh*, std::map<unsigned int, unsigned int>> g_triangle_for_plane_;

This comment has been minimized.

Sign in to view

Copy link
@v4hn

v4hn Apr 27, 2020

Contributor

@peci1 @rhaschke

I'm no fan of global storages, but if it's required... (why didn't we ever set sonames for this package too grml)

Anyway, this is a global standard container, so you need mutex locking for access!

This comment has been minimized.

Sign in to view

Copy link
@peci1

peci1 Apr 27, 2020

Author Contributor

Maybe sonames could be introduced in noetic?

I was also thinking about mutexes for this container, but evaluated the risk to be small, given the fact it is a hash map. But for correctness, I can add them.

This comment has been minimized.

Sign in to view

Copy link
@v4hn

v4hn Apr 27, 2020

Contributor

Maybe sonames could be introduced in noetic?

Would make sense, but it's overhead too and would need to be decided by multiple maintainers.
The current policy in MoveIt, to bump SONAME on every release, no matter whether ABI or API changed, is probably not a good idea for this repository, which means someone would have explicitly bump it whenever something became incompatible.

I was also thinking about mutexes for this container, but evaluated the risk to be small, given the fact it is a hash map. But for correctness, I can add them.

Yes, but it's a hashmap for scene objects and I believe we recently talked about people having a few thousand of them in their scene. Sure, iterators remain stable, but access to them does not while the r/b-tree is rebalanced on insert/delete.

This comment has been minimized.

Sign in to view

Copy link
@simonschmeisser

simonschmeisser Jun 5, 2020

Contributor

I'm currently debugging a crash in bodies::sphere::cloneAt which is called by bodies::Body::setPose. If there were so-names it would clearly have told me that I messed up packaging (using geometric_shapes from OSRF packages but self-packaged MoveIt)

were the mutex later on added or is this still open? ( I found no other thread doing anything shapey however ...)

This comment has been minimized.

Sign in to view

Copy link
@peci1

peci1 Jun 5, 2020

Author Contributor

d582479 ?
} // namespace detail

inline Eigen::Vector3d normalize(const Eigen::Vector3d& dir)
Expand Down Expand Up @@ -555,7 +559,7 @@ bool bodies::Box::samplePointInside(random_numbers::RandomNumberGenerator& rng,

bool bodies::Box::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const
{
const Eigen::Vector3d aligned = (invRot_ * (p - center_)).cwiseAbs();
const Eigen::Vector3d aligned = (pose_.linear().transpose() * (p - center_)).cwiseAbs();
return aligned[0] <= length2_ && aligned[1] <= width2_ && aligned[2] <= height2_;
}

Expand Down Expand Up @@ -596,7 +600,10 @@ void bodies::Box::updateInternalData()
radiusB_ = sqrt(radius2_);

ASSERT_ISOMETRY(pose_);
invRot_ = pose_.linear().transpose();
Eigen::Matrix3d basis = pose_.linear();
normalL_ = basis.col(0);
normalW_ = basis.col(1);
normalH_ = basis.col(2);

// rotation is intentionally not applied, the corners are used in intersectsRay()
const Eigen::Vector3d tmp(length2_, width2_, height2_);
Expand Down Expand Up @@ -676,8 +683,9 @@ bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vect

// The implemented method only works for axis-aligned boxes. So we treat ours as such, cancel its rotation, and
// rotate the origin and dir instead. corner1_ and corner2_ are corners with canceled rotation.
const Eigen::Vector3d o(invRot_ * (origin - center_) + center_);
const Eigen::Vector3d d(invRot_ * dirNorm);
const Eigen::Matrix3d invRot = pose_.linear().transpose();
const Eigen::Vector3d o(invRot * (origin - center_) + center_);
const Eigen::Vector3d d(invRot * dirNorm);

Eigen::Vector3d tmpTmin, tmpTmax;
tmpTmin = (corner1_ - o).cwiseQuotient(d);
Expand Down Expand Up @@ -921,6 +929,12 @@ void bodies::ConvexMesh::useDimensions(const shapes::Shape* shape)
mesh_data_->mesh_radiusB_ = sqrt(mesh_data_->mesh_radiusB_);
mesh_data_->triangles_.reserve(num_facets);

// HACK: only needed for ABI compatibility with melodic
if (detail::g_triangle_for_plane_.find(this) == detail::g_triangle_for_plane_.end())
detail::g_triangle_for_plane_.emplace(this, std::map<unsigned int, unsigned int>());
else
detail::g_triangle_for_plane_[this].clear();

// neccessary for qhull macro
facetT* facet;
FORALLfacets
Expand All @@ -945,7 +959,7 @@ void bodies::ConvexMesh::useDimensions(const shapes::Shape* shape)
}

mesh_data_->plane_for_triangle_[(mesh_data_->triangles_.size() - 1) / 3] = mesh_data_->planes_.size() - 1;
mesh_data_->triangle_for_plane_[mesh_data_->planes_.size() - 1] = (mesh_data_->triangles_.size() - 1) / 3;
detail::g_triangle_for_plane_[this][mesh_data_->planes_.size() - 1] = (mesh_data_->triangles_.size() - 1) / 3;
}
qh_freeqhull(!qh_ALL);
int curlong, totlong;
Expand Down Expand Up @@ -1130,7 +1144,7 @@ bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const
// we also cannot simply subtract padding_ from it, because padding of the points on the plane causes a different
// effect than adding padding along this plane's normal (padding effect is direction-dependent)
const auto scaled_point_on_plane =
scaled_vertices_->at(mesh_data_->triangles_[3 * mesh_data_->triangle_for_plane_[i]]);
scaled_vertices_->at(mesh_data_->triangles_[3 * detail::g_triangle_for_plane_[this][i]]);
const double w_scaled_padded = -plane_vec.dot(scaled_point_on_plane);
const double dist = plane_vec.dot(point) + w_scaled_padded - detail::ZERO;
if (dist > 0.0)
Expand Down Expand Up @@ -1264,6 +1278,13 @@ bool bodies::ConvexMesh::intersectsRay(const Eigen::Vector3d& origin, const Eige
return result;
}

bodies::ConvexMesh::~ConvexMesh()
{
// HACK: only needed for ABI compatibility with melodic
if (detail::g_triangle_for_plane_.find(this) != detail::g_triangle_for_plane_.end())
detail::g_triangle_for_plane_.erase(this);
}

bodies::BodyVector::BodyVector()
{
}
Expand Down

0 comments on commit ed4cf13

Please sign in to comment.