Adding Collision Objects

edit

Hello,

I am trying to add collision objects to the planning scene interface. However, the moveit planning seems to ignore the collision objects. I am passing a list of boxes as collision objects. Here is the code:

moveit_utils::MicoController srv_controller;
moveit::planning_interface::MoveGroup group("arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
ROS_INFO("COLLISION SIZE\n");
ROS_INFO_STREAM(collision_objects.size());
sleep(2.0);
planning_scene_interface.addCollisionObjects(collision_objects);

group.setPlanningTime(10.0); //5 second maximum for collision computation
moveit::planning_interface::MoveGroup::Plan my_plan;

group.setStartState(*group.getCurrentState());
group.setPoseTarget(p_target);

ROS_INFO("[mico_moveit_cartesianpose_service.cpp] starting to plan...");
bool success = group.plan(my_plan);
if(success)
    ROS_INFO("planning successful\n");
else 
    ROS_INFO("not successful :( \n");

Thank You.