Extended planning_interface::PlanningSceneInterface

[corot]

Hi, any reason to not include additional methods to the planning interface? It's weird to mix calls to this class and calls to the GetPlanningScene service.
I need to retrieve the attached objects and world objects geometry. Also, I'll add optional color information to addCollisionObjects

If no objection, I'll make a PR.

[davetcoleman]

Could you give an example?

[corot]

I have already added locally these two methods:

std::map<std::string, moveit_msgs::CollisionObject> getObjects(const std::vector<std::string> &object_ids);

std::map<std::string, moveit_msgs::AttachedCollisionObject> getAttachedObjects(const std::vector<std::string> &object_ids);

I need them because I'm interested on the collision object size (for grasping it) and not only in its pose.

And I modified this one to optionally provide colors for each object:

void addCollisionObjects(const std::vector<moveit_msgs::CollisionObject> &collision_objects,
                         const std::vector<moveit_msgs::ObjectColor> &object_colors = std::vector<moveit_msgs::ObjectColor>()) const;

I have noticed that, if I publish the colors separatedly, some times it doesn't work and the RViz plugin shows all with the default green. Publishing together with the objects themselves, it always work (or so it looks by now!)

[davetcoleman]

@corot I'm working with the PlanningSceneInterface right now and just re-discovered this issue - do you create the new features for that class? I'm interested in getting them merged in.