How to add multiple objects to MoveIt Planning Scene?
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.