Insert obstacles in Moveit! with Motion Planners/C++ API

edit

I had a problem with using MoveGroupInterface (see this), so I decide to use the c++ API (from this tutorial. It's all working fine now as I can pass the trajectory to gazebo. However, when I want to insert the obstacles into the environment, I tried following this answer and come up with the following code:

class collisionObjectAdder
{
protected:
    ros::Publisher add_collision_object_pub;
    moveit_msgs::CollisionObject co;

public:
   collisionObjectAdder(ros::NodeHandle& n)
   {
        add_collision_object_pub = n.advertise<moveit_msgs::CollisionObject>("collision_object", 1000);
   }

   void addCollisionObject(std::string modelpath, double x = 0, double y = 0, double z = 0, double ow = 1, double ox = 0, double oy = 0, double oz = 0)
   {
      shapes::Mesh* m = shapes::createMeshFromResource(modelpath);
      ROS_INFO("mesh loaded: %i %s ",m->triangle_count,modelpath.c_str());
      shape_msgs::Mesh co_mesh;
      shapes::ShapeMsg co_mesh_msg;
      shapes::constructMsgFromShape(m, co_mesh_msg);
      co_mesh = boost::get<shape_msgs::Mesh>(co_mesh_msg);
      co.meshes.push_back(co_mesh);
      int pos = co.meshes.size() - 1;

      co.header.frame_id = "world";

      moveit_msgs::CollisionObject::_mesh_poses_type mesh_pose;
      mesh_pose.resize(1);

      mesh_pose[0].position.x = x;
      mesh_pose[0].position.y = y;
      mesh_pose[0].position.z = z;
      mesh_pose[0].orientation.w= ow;
      mesh_pose[0].orientation.x= ox;
      mesh_pose[0].orientation.y= oy;
      mesh_pose[0].orientation.z= oz;

      co.mesh_poses.push_back(mesh_pose[0]);
      co.operation = co.ADD;
   }

   void publishCollisionObjects()
   {
       sleep(1.0); // To make sure the node can publish
       add_collision_object_pub.publish(co);
       ROS_INFO("Collision object published");
   }
};

and I call this in the main function:

ROS_INFO("Adding collision objects into the world");
collisionObjectAdder coa(n);
coa.addCollisionObject("package://myrobot_description/meshes/wall.dae",0.35,0,1.6);
coa.publishCollisionObjects();
ros::WallDuration sleep_time(2.0);
sleep_time.sleep();

This works with the move group example as the planner will try to avoid this obstacle, but it doesn't work with my api example. I had a feeling the tutorial example does not know to fetch /collision_object topic to insert in the planning_scene (need to manually add a callback function?). Any one can suggest me what is missing in my implementation?