RobotState leaks in RobotState::attachBody

[simonschmeisser]

RobotState has two functions to attach an AttachedBody:

void moveit::core::RobotState::attachBody(AttachedBody *attached_body)
{
    attached_body_map_[attached_body->getName()] = attached_body;
    attached_body->computeTransform(getGlobalLinkTransform(attached_body->getAttachedLink()));
     if (attached_body_update_callback_)
          attached_body_update_callback_(attached_body, true);
 }

vs

void moveit::core::RobotState::attachBody(const std::string &id,
                                      const std::vector<shapes::ShapeConstPtr> &shapes,
                                      const EigenSTL::vector_Affine3d &attach_trans,
                                      const std::set<std::string> &touch_links,
                                      const std::string &link,
                                      const trajectory_msgs::JointTrajectory &detach_posture)
 {
      const LinkModel *l = robot_model_->getLinkModel(link);
      AttachedBody *ab = new AttachedBody(l, id, shapes, attach_trans, touch_links, detach_posture);
      attached_body_map_[id] = ab;
      ab->computeTransform(getGlobalLinkTransform(l));
      if (attached_body_update_callback_)
          attached_body_update_callback_(ab, true);
}

The documentation for the first version states that
Ownership of the memory for the attached body is assumed by the state.. For the second version that is evidently clear. However there seems to be no place where these AttachedObjects are deleted again.

A quick fix for me was to modify the destructor:

 moveit::core::RobotState::~RobotState()
 {
     clearAttachedBodies(); // added
     free(memory_);
     if (rng_)
       delete rng_;
 }

Generally I think the map

std::map<std::string, AttachedBody*>   attached_body_map_;

should store boost::shared_ptr<AttachedBody> or similar

[simonschmeisser]

Closed in pr #276 for jade-devel