Detaching attached collision object bugfix

[mlautman]

Description

Detaching an attached collision object can cause segfaults in melodic.

The segfault was found running Melodic on 18.04

In this method, attached_body is a raw pointer to an AttachedBody in robot_state_.
The planning scene saves the attached_body's shapes_ vector with const std::vector<shapes::ShapeConstPtr>& shapes = attached_body->getShapes();. However, when robot_state_->clearAttachedBody(name) is called, it deletes the AttachedBody causing the underlying ShapeConstPtrs to be freed. Then when world_->addToObject(name, shapes, poses) is called, it segfaults as the data in the shapes_ vector is no longer available.

By moving world_->addToObject(name, shapes, poses) beforerobot_state_->clearAttachedBody(name), we create another shared pointer to the same underlying data in the shapes vector so that when robot_state_->clearAttachedBody(name) is called, it does not free the ShapeConstPtrs.

Another fix would be to use const std::vector<shapes::ShapeConstPtr> shapes = attached_body->getShapes(); instead of const std::vector<shapes::ShapeConstPtr>& shapes = attached_body->getShapes(); but I think this fix is better

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extended the tutorials / documentation, if necessary reference
• Include a screenshot if changing a GUI
• Document API changes relevant to the user in the moveit/MIGRATION.md notes
• Created tests, which fail without this PR reference
• Decide if this should be cherry-picked to other current ROS branches
• While waiting for someone to review your request, please consider reviewing another open pull request to support the maintainers

[felixvd]

I have no idea how this was running before the change. I don't see how it could break anything and the new order makes much more sense. Good with me.

[rconnorlawson]

I agree with @felixvd, this looks like a good change. Of the two options that @mlautman mentions, the chosen solution is the correct one, since it maintains the validity of shapes reference until it is no longer needed without doing any additional work. The other solution unnecessarily copies the contents of AttachedBody::shapes_ into a temporary vector.

As a point of interest, here's some discussion of how this possibly worked beforehand:

The shapes reference points to a vector owned by the AttachedBody. When clearAttachedBody deletes its argument, two things happen:

1. Each ShapeConstPtr contained in AttachedBody::shapes_, is destructed, thus decrementing the reference count for each shape and potentially freeing the shape if there are no other references.
2. The vector AttachedBody::shapes_ is destructed, which will free the underlying array that holds each ShapeConstPtr. This is the object that the reference shapes points.

Using shapes after calling clearAttachedBodies is undefined behavior since the object it points has been destructed (Step 2). However, the underlying array for AttachedBody::shapes_ is likely small in a lot of cases, and the whole underlying array was just called into physical memory in order to run the destructor for each element (Step 1).

Memory isn't emptied or overwritten when allocations are deleted, it's only "freed". This indicates to the allocator and the operating system that the memory isn't in use anymore, but the contents of the memory aren't changed. If the thread of execution running this function can outrace any other processes that might try to write something else into the physical memory where this vector resides, there won't be a segfault immediately. The danger if that happens is that some of the ShapeConstPtrs were accidentally freed, which will lead to unpredictable segfaults down the road when they're later accessed.