C++ circular_buffer::clear方法代码示例

bool publish() {
    if (buf_flag) {
        pthread_mutex_lock(&mutex);

        //image_obj is empty
        if (image_obj_ringbuf.begin() == image_obj_ringbuf.end()) {
            pthread_mutex_unlock(&mutex);
            ROS_INFO("image_obj ring buffer is empty");
            return false;
        }

        //vscan_image is empty
        if (vscan_image_ringbuf.begin() == vscan_image_ringbuf.end()) {
            pthread_mutex_unlock(&mutex);
            ROS_INFO("vscan_image ring buffer is empty");
            return false;
        }

        // image_obj > vscan_image
        if (get_time(&(image_obj_ringbuf.front().header)) >= get_time(&(vscan_image_ringbuf.front().header))) {
            p_vscan_image_buf = &(vscan_image_ringbuf.front());
            boost::circular_buffer<autoware_msgs::image_obj>::iterator it = image_obj_ringbuf.begin();
            if (image_obj_ringbuf.size() == 1) {
                p_image_obj_buf = &*it;
                publish_msg(p_image_obj_buf, p_vscan_image_buf);
                pthread_mutex_unlock(&mutex);
                return true;
            } else {
                for (it++; it != image_obj_ringbuf.end(); it++) {
                    if (fabs_time_diff(&(vscan_image_ringbuf.front().header), &((it-1)->header))
                        < fabs_time_diff(&(vscan_image_ringbuf.front().header), &(it->header))) {
                        p_image_obj_buf = &*(it-1);
                        break;
                    }
                }
                if (it == image_obj_ringbuf.end()) {
                    p_image_obj_buf = &(image_obj_ringbuf.back());
                }
            }
        }
        // image_obj < vscan_image
        else {
            p_image_obj_buf = &(image_obj_ringbuf.front());
            boost::circular_buffer<autoware_msgs::PointsImage>::iterator it = vscan_image_ringbuf.begin();
            if (vscan_image_ringbuf.size() == 1) {
                p_vscan_image_buf = &*it;
                publish_msg(p_image_obj_buf, p_vscan_image_buf);
                pthread_mutex_unlock(&mutex);
                return true;
            }

            for (it++; it != vscan_image_ringbuf.end(); it++) {
                if (fabs_time_diff(&(image_obj_ringbuf.front().header), &((it-1)->header))
                    < fabs_time_diff(&(image_obj_ringbuf.front().header), &(it->header))) {
                    p_vscan_image_buf = &*(it-1);
                    break;
                }
            }

            if (it == vscan_image_ringbuf.end()) {
                p_vscan_image_buf = &(vscan_image_ringbuf.back());
            }
        }
        publish_msg(p_image_obj_buf, p_vscan_image_buf);
        if (image_obj_ranged_flag == true){
            buf_flag = false;
            image_obj_ranged_flag = false;
            pthread_mutex_unlock(&flag_mutex);
            image_obj_ringbuf.clear();
            vscan_image_ringbuf.clear();
        }
        return true;
    } else {
        return false;
    }
}

void WaypointVelocityVisualizer::resetBuffers()
{
  current_pose_buf_.clear();
  current_twist_buf_.clear();
  command_twist_buf_.clear();
}

bool publish() {
    if (buf_flag) {
        //image_obj_ranged is empty
        if (image_obj_ranged_ringbuf.begin() == image_obj_ranged_ringbuf.end()) {
            ROS_INFO("image_obj_ranged ring buffer is empty");
            return false;
        }

        //image_raw is empty
        if (image_raw_ringbuf.begin() == image_raw_ringbuf.end()) {
            ROS_INFO("image_raw ring buffer is empty");
            return false;
        }

        // image_obj_ranged > image_raw
        if (get_time(&(image_obj_ranged_ringbuf.front().header)) >= get_time(&(image_raw_ringbuf.front().header))) {
            p_image_raw_buf = &(image_raw_ringbuf.front());
            boost::circular_buffer<cv_tracker::image_obj_ranged>::iterator it = image_obj_ranged_ringbuf.begin();
            if (image_obj_ranged_ringbuf.size() == 1) {
                p_image_obj_ranged_buf = &*it;
                publish_msg(p_image_obj_ranged_buf, p_image_raw_buf);
                if (image_obj_tracked_flag == true){
                    buf_flag = false;
                    image_obj_tracked_flag = false;
                    image_obj_ranged_ringbuf.clear();
                    image_raw_ringbuf.clear();
                }
                return true;
            } else {
                for (it++; it != image_obj_ranged_ringbuf.end(); it++) {
                    if (fabs_time_diff(&(image_raw_ringbuf.front().header), &((it-1)->header))
                        < fabs_time_diff(&(image_raw_ringbuf.front().header), &(it->header))) {
                        p_image_obj_ranged_buf = &*(it-1);
                        break;
                    }
                }
                if (it == image_obj_ranged_ringbuf.end()) {
                    p_image_obj_ranged_buf = &(image_obj_ranged_ringbuf.back());
                }
            }
        }
        // image_obj_ranged < image_raw
        else {
            p_image_obj_ranged_buf = &(image_obj_ranged_ringbuf.front());
            boost::circular_buffer<sensor_msgs::Image>::iterator it = image_raw_ringbuf.begin();
            if (image_raw_ringbuf.size() == 1) {
                p_image_raw_buf = &*it;
                publish_msg(p_image_obj_ranged_buf, p_image_raw_buf);
                if (image_obj_tracked_flag == true){
                    buf_flag = false;
                    image_obj_tracked_flag = false;
                    image_obj_ranged_ringbuf.clear();
                    image_raw_ringbuf.clear();
                }
                return true;
            }

            for (it++; it != image_raw_ringbuf.end(); it++) {
                if (fabs_time_diff(&(image_obj_ranged_ringbuf.front().header), &((it-1)->header))
                    < fabs_time_diff(&(image_obj_ranged_ringbuf.front().header), &(it->header))) {
                    p_image_raw_buf = &*(it-1);
                    break;
                }
            }

            if (it == image_raw_ringbuf.end()) {
                p_image_raw_buf = &(image_raw_ringbuf.back());
            }
        }
        publish_msg(p_image_obj_ranged_buf, p_image_raw_buf);
        if (image_obj_tracked_flag == true){
            buf_flag = false;
            image_obj_tracked_flag = false;
            image_obj_ranged_ringbuf.clear();
            image_raw_ringbuf.clear();
        }

        return true;
    } else {
        return false;
    }
}

//.........这里部分代码省略.........

    in_times.push_front((now - last_subscribe_time_).toSec());
    in_bytes.push_front(img->data.size());
    //
    try {
        int width = dst_width_ ? dst_width_ : (resize_x_ * info->width);
        int height = dst_height_ ? dst_height_ : (resize_y_ * info->height);
        double scale_x = dst_width_ ? ((double)dst_width_)/info->width : resize_x_;
        double scale_y = dst_height_ ? ((double)dst_height_)/info->height : resize_y_;

        cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img);

        cv::Mat tmpmat(height, width, cv_img->image.type());
        cv::resize(cv_img->image, tmpmat, cv::Size(width, height));
        cv_img->image = tmpmat;

        sensor_msgs::CameraInfo tinfo = *info;
        tinfo.height = height;
        tinfo.width = width;
        tinfo.K[0] = tinfo.K[0] * scale_x; // fx
        tinfo.K[2] = tinfo.K[2] * scale_x; // cx
        tinfo.K[4] = tinfo.K[4] * scale_y; // fy
        tinfo.K[5] = tinfo.K[5] * scale_y; // cy

        tinfo.P[0] = tinfo.P[0] * scale_x; // fx
        tinfo.P[2] = tinfo.P[2] * scale_x; // cx
        tinfo.P[3] = tinfo.P[3] * scale_x; // T
        tinfo.P[5] = tinfo.P[5] * scale_y; // fy
        tinfo.P[6] = tinfo.P[6] * scale_y; // cy

        if ( !use_messages_ || now - last_publish_time_  > period_ ) {
            cp_.publish(cv_img->toImageMsg(),
                        boost::make_shared<sensor_msgs::CameraInfo> (tinfo));

            out_times.push_front((now - last_publish_time_).toSec());
            out_bytes.push_front(cv_img->image.total()*cv_img->image.elemSize());

            last_publish_time_ = now;
        }
    } catch( cv::Exception& e ) {
        ROS_ERROR("%s", e.what());
    }


    float duration =  (now - last_rosinfo_time_).toSec();
    if ( duration > 2 ) {
        int in_time_n = in_times.size();
        int out_time_n = out_times.size();
        double in_time_mean = 0, in_time_rate = 1.0, in_time_std_dev = 0.0, in_time_max_delta, in_time_min_delta;
        double out_time_mean = 0, out_time_rate = 1.0, out_time_std_dev = 0.0, out_time_max_delta, out_time_min_delta;

        std::for_each( in_times.begin(), in_times.end(), (in_time_mean += boost::lambda::_1) );
        in_time_mean /= in_time_n;
        in_time_rate /= in_time_mean;
        std::for_each( in_times.begin(), in_times.end(), (in_time_std_dev += (boost::lambda::_1 - in_time_mean)*(boost::lambda::_1 - in_time_mean) ) );
        in_time_std_dev = sqrt(in_time_std_dev/in_time_n);
        if ( in_time_n > 1 ) {
            in_time_min_delta = *std::min_element(in_times.begin(), in_times.end());
            in_time_max_delta = *std::max_element(in_times.begin(), in_times.end());
        }

        std::for_each( out_times.begin(), out_times.end(), (out_time_mean += boost::lambda::_1) );
        out_time_mean /= out_time_n;
        out_time_rate /= out_time_mean;
        std::for_each( out_times.begin(), out_times.end(), (out_time_std_dev += (boost::lambda::_1 - out_time_mean)*(boost::lambda::_1 - out_time_mean) ) );
        out_time_std_dev = sqrt(out_time_std_dev/out_time_n);
        if ( out_time_n > 1 ) {
            out_time_min_delta = *std::min_element(out_times.begin(), out_times.end());
            out_time_max_delta = *std::max_element(out_times.begin(), out_times.end());
        }

        double in_byte_mean = 0, out_byte_mean = 0;
        std::for_each( in_bytes.begin(), in_bytes.end(), (in_byte_mean += boost::lambda::_1) );
        std::for_each( out_bytes.begin(), out_bytes.end(), (out_byte_mean += boost::lambda::_1) );
        in_byte_mean /= duration;
        out_byte_mean /= duration;

        ROS_INFO_STREAM(" in  bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3)  << in_byte_mean/1000*8
                        << " Kbps rate:"   << std::fixed << std::setw(7) << std::setprecision(3) << in_time_rate
                        << " hz min:"      << std::fixed << std::setw(7) << std::setprecision(3) << in_time_min_delta
                        << " s max: "    << std::fixed << std::setw(7) << std::setprecision(3) << in_time_max_delta
                        << " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << in_time_std_dev << "s n: " << in_time_n);
        ROS_INFO_STREAM(" out bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3)  << out_byte_mean/1000*8
                        << " kbps rate:"   << std::fixed << std::setw(7) << std::setprecision(3) << out_time_rate
                        << " hz min:"      << std::fixed << std::setw(7) << std::setprecision(3) << out_time_min_delta
                        << " s max: "    << std::fixed << std::setw(7) << std::setprecision(3) << out_time_max_delta
                        << " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << out_time_std_dev << "s n: " << out_time_n);
        in_times.clear();
        in_bytes.clear();
        out_times.clear();
        out_bytes.clear();
        last_rosinfo_time_ = now;
    }

    last_subscribe_time_ = now;

    if(use_snapshot_) {
      publish_once_ = false;
    }
  }