subscribe and advertise image the same time cause errors [closed]

edit

The following code subscribe 2 images and generate and show depth map. It has some very weird problem. Sometimes it outputs depth map, sometimes not. But if I comment og_pub=it.advertise("occupancy_grid", 10); in function ODGridMap(), everything works well. I always get depth map output. I don't know if it's because I can't subscribe and advertise image the same time.

class ODGridMap {
public:

    bool isLeftImgReady;
    bool isRightImgReady;

    Mat leftImg, rightImg, ocGrid;
    ObstacleDetection::parameters param;
    ObstacleDetection *od;
    ros::NodeHandle nh;
    image_transport::ImageTransport it;
    image_transport::ImageTransport it_pub;

    image_transport::Subscriber sub_left;
    image_transport::Subscriber sub_right;
    image_transport::Publisher og_pub;

    void imageCallback(const sensor_msgs::ImageConstPtr& msg,
            const std::string &topic) {
        try {
            if (topic == "left") {
                isLeftImgReady = true;
                cv_bridge::toCvShare(msg, "mono8")->image.copyTo(leftImg);
            } else if (topic == "right") {
                isRightImgReady = true;
                cv_bridge::toCvShare(msg, "mono8")->image.copyTo(rightImg);
            }

            if (isLeftImgReady && isRightImgReady) {
                isLeftImgReady = false;
                isRightImgReady = false;

                od->get_local_occupancy_grid(leftImg, rightImg, ocGrid, param);
                sensor_msgs::ImagePtr msg_img = cv_bridge::CvImage(
                        std_msgs::Header(), "mono8", ocGrid).toImageMsg();
                imshow("D1", od->D1);
                cv::waitKey(30);
            }
        } catch (cv_bridge::Exception& e) {
            ROS_ERROR("Could not convert from '%s' to 'bgr8'.",
                    msg->encoding.c_str());
        }
    }

    ODGridMap() :
            it(nh), it_pub(nh) {
        isLeftImgReady = false;
        isRightImgReady = false;

        sub_left = it.subscribe("camera/left_img", 10,
                        boost::bind(&ODGridMap::imageCallback, this, _1, "left"));
        sub_right = it.subscribe("camera/right_img", 10,
                boost::bind(&ODGridMap::imageCallback, this, _1, "right"));
        og_pub=it.advertise("occupancy_grid", 10);

        param.f = 545.0 / 2.0;
        param.cu = 359.997 / 2;
        param.cv = 210.40252 / 2;
        param.base = 0.275827;
        param.cam_height = 0.55;
        param.cam_pitch = -0.24;
        param.cell_width = 0.1;
        param.cell_depth = 0.1;
        param.grid_width = 60;
        param.grid_depth = 100;
        param.thres = 100;

        od = new ObstacleDetection(param);
    }

    ~ODGridMap() {
        delete od;
    }
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "obstacle_detection");
    ODGridMap odgm;
    ros::spin();
    return 0;
}