Fix the computation of shadow points

[chris20623]

Issue

I think the computation of shadow points does not work correctly in some cases. Shadow points may be missed and classified as outside points instead.

Analysis

For each robot body, the points of intersection with the ray to the considered point are computed. These points of intersection are appended to the intersections vector. Then, the sign of a dot product involving the first point of intersection is checked.

However, the intersections vector is not cleared before the next body is checked. So, if the first intersecting body yields a negative dot product, positive dot products resulting from subsequent bodies will be ignored.

Solution

Move the definition of the intersections vector into the scope of the loop body so that the vector is reset before each intersectsRay() call and intersections[0] subsequently corresponds to the j-th robot body.

Successfully tested on melodic and noetic.

[v4hn]

I'm a bit surprised this is still used, because robot_body_filter provides a pr2-independent generalization. :-)

@peci1 could you review this patch please? Looks reasonable to me with the explanation.

[chris20623]

Thanks for the prompt reply.

I'm a bit surprised this is still used, because robot_body_filter provides a pr2-independent generalization. :-)

We have been using robot_self_filter (with own modifications) for quite a long time, but didn't need the shadow computation feature until recently. I see that this issue has already been fixed in robot_body_filter (by calling intersections.clear() in line 221 of src/RayCastingShapeMask.cpp, which should eventually have the same effect).

Perhaps it would be helpful if someone could add a reference to robot_body_filter on the ROS wiki page http://wiki.ros.org/robot_self_filter. Right now, it is quite difficult to see which of all those forks is the most up-to-date one.

[v4hn]

Thanks for the prompt reply.

You were just lucky. ;)

I see that this issue has already been fixed in robot_body_filter (by calling intersections.clear() in line 221 of src/RayCastingShapeMask.cpp, which should eventually have the same effect).

Thanks for looking through the code, I only had a glimpse and did not immediately find the corresponding code path.
clearing the vector should indeed be preferred to avoid additional allocations in each iteration.
Could you update the patch to do so?

The state of the robot_self_filter/robot_body_filter is somewhat sad. The PR2 repository is not actively worked on and thus people forked quite a lot. I referenced @peci1's fork because I know he actively works on quite a number of issues of this code, contributing great improvements. He should have asked to take over maintenance of this repository when he forked 😎

@peci1 can we/should we make more prominent advertisement for your fork/rewrite here?

[peci1]

Hi, the solution proposed by @v4hn would be preferred.

@v4hn Feel free to advertise robot_body_filter ;) If you feel the need for a larger discussion about robot_self_filter vs robot_body_filter, open an issue and invite me in for the discussion.

[v4hn]

If you feel the need for a larger discussion about robot_self_filter vs robot_body_filter, open an issue and invite me in for the discussion.

See #23 .

@chris20623 I'll merge your patch if CI passes.

[chris20623]

The PR has been updated by using the solution chosen in robot_body_filter (including the comments, which are very helpful to understand what the code is doing).

[chris20623]

Thanks a lot for reviewing and merging this so fast!