How to add multiple objects to MoveIt Planning Scene?

Question

I am unable to add multiple objects to MoveIt planning scene. Using the Move Group C++ API, I do:

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit_msgs::CollisionObject co, co2;

..
..
Fill in the fields of moveit_msgs co & co2

.. ..

co.operation = co.ADD;
co2.operation=co2.ADD;

std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(co);
collision_objects.push_back(co2) ;

ROS_INFO("Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);

sleep(2.0); 

When I run it, only one collision object appears i.e. co2 .

I can not figure out what am I missing. Please help me out.

---

Originally posted by rohin on ROS Answers with karma: 99 on 2016-10-03

Post score: 0

Answer 1

Hi Rohin,

Are you assigning unique id's to the CollisionObjects? Looking at the message type we can see that for the ADD operation, the CollisionObject information will be replaced if the object (with the id string as an identifier) is already present in the planningScene.

---

Originally posted by rbbg with karma: 1823 on 2016-10-03

This answer was ACCEPTED on the original site

Post score: 3

---

Original comments

Comment by rohin on 2016-10-03:
Yeah...

I was doing

co.header.frame_id = group.getPlanningFrame(); co2.header.frame_id = group.getPlanningFrame();

How should I assign different ID's?

Comment by rohin on 2016-10-03:
Thanks! That solved it!