Once in a while moveit! ignores collision objects

edit

Hello,

I have created some collision objects with the moveit::planning_interface::PlanningSceneInterface object like this:

Usually, moveit noticeably refers to the collision object, but once in a while it's not and the arm actually enters the collision object. Moveit doesn't detect the collision and in real life the robot actually collides with its environment.

Do you have any suggestions for what I did wrong? Currently I don't have the values which caused that collision but any idea will be welcomed.

Planning and executing:

mMoveGroup.setJointValueTarget(targetJointValues);
mMoveGroup.setStartStateToCurrentState();
moveit::planning_interface::MoveItErrorCode result = mMoveGroup.plan(plan);
mMoveGroup.execute(plan);

Here is the code for creating the collision object:

moveit::planning_interface::PlanningSceneInterface planning_scene_interface("/right");
std::vector<moveit_msgs::CollisionObject> collision_objects;
moveit_msgs::CollisionObject collision_object;

collision_object = createCollisionObject("rover",
    0.9, 1.4, 1.0,
    -0.15, 0.4, -0.45,
    0.966, 0, 0, -0.259);
collision_objects.push_back(collision_object);  
collision_object = createCollisionObject("tomato-box",
    0.4, 0.4, 0.15,
    -0.35, 0.1, 0.01,
    0.966, 0, 0, -0.259);
collision_objects.push_back(collision_object);
collision_object = createCollisionObject("camera-mast",
    0.15, 0.15, 0.50,
    -0.77, 0.08, 0.25,
    0.966, 0, 0, -0.259);
collision_objects.push_back(collision_object);  
collision_object = createCollisionObject("arm-wall",
    2.0, 0.02, 3.0,
    0.22, 0.22, 0,
    0.966, 0, 0, -0.259);
collision_objects.push_back(collision_object);

while (!planning_scene_interface.applyCollisionObjects(collision_objects)){
    ROS_WARN("Planning scene not ready, sleeping");
    ros::Duration(0.5).sleep();
}

The function that returns a collision object:

moveit_msgs::CollisionObject createCollisionObject(std::string id, float d0,
    float d1, float d2, float x, float y, float z, float ow, float ox, float oy, float oz)
   {
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = "world";
    collision_object.id = id;

    shape_msgs::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[0] = d0;
    primitive.dimensions[1] = d1;
    primitive.dimensions[2] = d2;

    geometry_msgs::Pose box_pose;
    box_pose.orientation.w = ow;
    box_pose.orientation.x = ox;
    box_pose.orientation.y = oy;
    box_pose.orientation.z = oz;

    box_pose.position.x = x; 
    box_pose.position.y = y;
    box_pose.position.z = z;

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    return collision_object;
}

Thanks in advance, Nadav