Box/ray intersections are broken in 0.5.4
skohlbr opened this issue · comments
I'm on 16.04/Kinetic and geometric_shapes installed from .debs (0.5.4). It appears box/ray intersections do not work at all, at least in my application (ray-based self filtering). I created a test URDF with 3 primitives for testing ray intersection functionality. When I use 3 spheres for these shapes, things look as expected, with the sphere shapes shadowing cloud data behind them:
If I change the primitives to boxes of approximately same size and keep everything else the same, no filtering takes place anymore, as the intersection test appears not to work as intended:
Note the gap at the ceiling is due to LIDAR physically not covering that part and unrelated to the observed issue. I noted #68 supposedly recently fixed box-ray intersection, so wondering if this accidentally introduced another bug.
The code used for the intersection test in my use case is here: https://github.com/team-vigir/robot_self_filter/blob/vlp16_filter/include/robot_self_filter/self_mask.h#L603
@LeroyR Please could you have a look at this issue, as you changed this code.
The intersection code seems to be fine, hit locations and ordering seems to be fine.
They cant be totally broken as the tests suite complete and my experiments did still finished as expected.
my code looks something like this:
bodies::BodyPtr body(bodies::createBodyFromShape(shape.get()));
body->setScale(1);
Eigen::Affine3d bodyPose;
tf2::fromMsg(pose, bodyPose);
body->setPose(bodyPose);
EigenSTL::vector_Vector3d intersections;
intersected = body->intersectsRay(origin, dir, &intersections, 1);
for (auto a : intersections) { ...
Can you confirm it worked in 0.5.3 or produce example points & a box that should intersect but fail?
This should be fixed via #109 in Melodic.