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

void image_obj_ranged_callback(const cv_tracker::image_obj_ranged::ConstPtr& image_obj_ranged_msg) {
    pthread_mutex_lock(&mutex);
    image_obj_ranged_ringbuf.push_front(*image_obj_ranged_msg);

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

    buf_flag = true;

    // image_obj_ranged > image_raw
    if (get_time(&(image_obj_ranged_ringbuf.front().header)) >= get_time(&(image_raw_ringbuf.front().header))) {
        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) {
            image_obj_ranged_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        } 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))) {
                    image_obj_ranged_buf = *(it-1);
                    break;
                }
            }
            if (it == image_obj_ranged_ringbuf.end()) {
                image_obj_ranged_buf = image_obj_ranged_ringbuf.back();
            }
        }

    } else {
        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) {
            image_raw_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        }

        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))) {
                image_raw_buf = *(it-1);
                break;
            }
        }

        if (it == image_raw_ringbuf.end()) {
            image_raw_buf = image_raw_ringbuf.back();
        }
    }
    pthread_mutex_unlock(&mutex);
}

void image_obj_callback(const autoware_msgs::image_obj::ConstPtr& image_obj_msg) {
    pthread_mutex_lock(&mutex);
    image_obj_ringbuf.push_front(*image_obj_msg);

    //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;
    }

    buf_flag = true;

    // image_obj > vscan_image
    if (get_time(&(image_obj_ringbuf.front().header)) >= get_time(&(vscan_image_ringbuf.front().header))) {
        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) {
            image_obj_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        } 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))) {
                    image_obj_buf = *(it-1);
                    break;
                }
            }
            if (it == image_obj_ringbuf.end()) {
                image_obj_buf = image_obj_ringbuf.back();
            }
        }

    } else {
        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) {
            vscan_image_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        }

        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))) {
                vscan_image_buf = *(it-1);
                break;
            }
        }

        if (it == vscan_image_ringbuf.end()) {
            vscan_image_buf = vscan_image_ringbuf.back();
        }
    }
    pthread_mutex_unlock(&mutex);
}

void vscan_image_callback(const autoware_msgs::PointsImage::ConstPtr& vscan_image_msg) {
    pthread_mutex_lock(&mutex);
    vscan_image_ringbuf.push_front(*vscan_image_msg);
    //image_obj is empty
    if (image_obj_ringbuf.begin() == image_obj_ringbuf.end()) {
        ROS_INFO("image_obj ring buffer is empty");
        buf_flag = false;
        pthread_mutex_unlock(&mutex);
        return;
    }
    buf_flag = true;
    pthread_mutex_unlock(&mutex);
    if (image_obj_ranged_flag == true) {
        publish();
    }
}

void image_raw_callback(const sensor_msgs::Image::ConstPtr& image_raw_msg) {
    pthread_mutex_lock(&mutex);
    image_raw_ringbuf.push_front(*image_raw_msg);
    //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");
        buf_flag = false;
        pthread_mutex_unlock(&mutex);
        return;
    }
    buf_flag = true;
    pthread_mutex_unlock(&mutex);
    pthread_mutex_lock(&mutex);
    if (image_obj_tracked_flag == true) {
        publish();
    }
    pthread_mutex_unlock(&mutex);
}

void CSearchDialog::SaveEntry(int comboBoxId, boost::circular_buffer<std::wstring> &buffer)
{
	TCHAR entry[MAX_PATH];
	GetDlgItemText(m_hDlg, comboBoxId, entry, SIZEOF_ARRAY(entry));

	std::wstring strEntry(entry);
	auto itr = std::find_if(buffer.begin(), buffer.end(),
		[strEntry] (const std::wstring Pattern)
	{
		return Pattern.compare(strEntry) == 0;
	});

	HWND hComboBox = GetDlgItem(m_hDlg, comboBoxId);
	ComboBox_SetCurSel(hComboBox, -1);

	if(itr != buffer.end())
	{
		/* Remove the current element from both the list and the
		combo box. It will be reinserted at the front of both below. */
		auto index = std::distance(buffer.begin(), itr);
		SendMessage(hComboBox, CB_DELETESTRING, index, 0);

		buffer.erase(itr);
	}

	buffer.push_front(entry);

	SendMessage(hComboBox, CB_INSERTSTRING, 0, reinterpret_cast<LPARAM>(entry));
	ComboBox_SetCurSel(hComboBox, 0);
	ComboBox_SetEditSel(hComboBox, -1, -1);

	if(ComboBox_GetCount(hComboBox) > buffer.capacity())
	{
		SendMessage(hComboBox, CB_DELETESTRING, ComboBox_GetCount(hComboBox) - 1, 0);
	}
}

void ShowFrameDurationPlot() {
	
	Vec2i windowSize = mainApp->getWindow()->getSize();
	size_t maxSamples = size_t(windowSize.x);
	
	if(maxSamples != frameDurationPlotValues.capacity()) {
		frameDurationPlotValues.set_capacity(maxSamples);
	}
	if(maxSamples != frameDurationPlotVertices.size()) {
		frameDurationPlotVertices.resize(maxSamples);
	}
	
	GRenderer->ResetTexture(0);
	
	frameDurationPlotValues.push_front(toMs(g_platformTime.lastFrameDuration()));
	
	float avg = std::accumulate(frameDurationPlotValues.begin(), frameDurationPlotValues.end(), 0.f) / frameDurationPlotValues.size();
	float worst = *std::max_element(frameDurationPlotValues.begin(), frameDurationPlotValues.end());
	
	const float OFFSET_Y = 80.f;
	const float SCALE_Y = 4.0f;

	for(size_t i = 0; i < frameDurationPlotValues.size(); ++i)
	{
		float time = frameDurationPlotValues[i];
		frameDurationPlotVertices[i].color = Color::white.toRGB();
		frameDurationPlotVertices[i].p.x = i;
		frameDurationPlotVertices[i].p.y = OFFSET_Y + (time * SCALE_Y);
		frameDurationPlotVertices[i].p.z = 1.0f;
		frameDurationPlotVertices[i].w = 1.0f;
	}

	EERIEDRAWPRIM(Renderer::LineStrip, &frameDurationPlotVertices[0], frameDurationPlotValues.size());

	Color avgColor = Color::blue * 0.5f + Color::white * 0.5f;
	float avgPos = OFFSET_Y + (avg * SCALE_Y);
	drawLine(Vec2f(0, avgPos), Vec2f(windowSize.x, avgPos), 1.0f, Color::blue);

	Color worstColor = Color::red * 0.5f + Color::white * 0.5f;
	float worstPos = OFFSET_Y + (worst * SCALE_Y);
	drawLine(Vec2f(0, worstPos), Vec2f(windowSize.x, worstPos), 1.0f, Color::red);

	Font * font = hFontDebug;
	float lineOffset = font->getLineHeight() + 2;

	std::string labels[3] = { "Average: ", "Worst: ", "Current: " };
	Color colors[3] = { avgColor, worstColor, Color::white };
	float values[3] = { avg, worst, frameDurationPlotValues[0] };

	std::string texts[3];
	float widths[3];
	static float labelWidth = 0.f;
	static float valueWidth = 0.f;
	for(size_t i = 0; i < 3; i++) {
		// Format value
		std::ostringstream oss;
		oss << std::fixed << std::setprecision(2) << values[i] << " ms ("<< 1.f / (values[i] * 0.001f) << " FPS)";
		texts[i] = oss.str();
		// Calculate widths (could be done more efficiently for monospace fonts...)
		labelWidth = std::max(labelWidth, float(font->getTextSize(labels[i]).width()));
		widths[i] = font->getTextSize(texts[i]).width();
		valueWidth = std::max(valueWidth, widths[i]);
	}

	float x = 10;
	float y = 10;
	float xend = x + labelWidth + 10 + valueWidth;
	for(size_t i = 0; i < 3; i++) {
		font->draw(Vec2i(x, y), labels[i], Color::gray(0.8f));
		font->draw(Vec2i(xend - widths[i], y), texts[i], colors[i]);
		y += lineOffset;
	}

}

 void push_front(ros::Time sensor_time, ros::Time execution_time) {
     sensor.push_front(sensor_time);
     execution.push_front(execution_time);
 }

  void callback(const sensor_msgs::ImageConstPtr &img,
                const sensor_msgs::CameraInfoConstPtr &info) {
    boost::mutex::scoped_lock lock(mutex_);
    ros::Time now = ros::Time::now();

    static boost::circular_buffer<double> in_times(100);
    static boost::circular_buffer<double> out_times(100);
    static boost::circular_buffer<double> in_bytes(100);
    static boost::circular_buffer<double> out_bytes(100);

    ROS_DEBUG("resize: callback");
    if ( !publish_once_ || cp_.getNumSubscribers () == 0 ) {
      ROS_DEBUG("resize: number of subscribers is 0, ignoring image");
      return;
    }

    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
//.........这里部分代码省略.........

void utmCallback(const msgs::gpgga_utm::ConstPtr& msg)
{

	static tf::TransformBroadcaster odom_broadcaster;
	if(msg->hdop < 2 && utm_settled == false)
	{
		utm_settled_count++;
		if(utm_settled_count >= utm_settled_count_top)
		{
			utm_settled = true;
			ref_fix = *msg;
			prev_msg = *msg;

		}
	}
	else if(utm_settled)
	{

		gps_points_t p;
		p.x = msg->utm_e;
		p.y = msg->utm_n;

		gps_points_buffer.push_front(p);

		odom.header.stamp = ros::Time::now();
		odom.header.frame_id =frame_id;
		odom.child_frame_id = child_frame_id;

		if(publish_relative){
			odom.pose.pose.position.x = -(ref_fix.utm_e - msg->utm_e);
			odom.pose.pose.position.y = -(ref_fix.utm_n - msg->utm_n);
		}
		else
		{
			// ENU
			odom.pose.pose.position.x =msg->utm_e;
			odom.pose.pose.position.y =msg->utm_n;
		}

		odom.pose.pose.position.z = 0;

		static double yaw=0;

		if(calculate_heading(yaw))
		{
			odom.pose.covariance[35] = heading_variance;
		}
		else
		{
			odom.pose.covariance[35] = 999999;

		}

		// publish the transform message
		odom_trans.header.stamp = ros::Time::now();
		odom_trans.header.frame_id = tf_frame_id;
		odom_trans.child_frame_id = tf_child_frame_id;
		odom_trans.transform.translation.x = odom.pose.pose.position.x;
		odom_trans.transform.translation.y = odom.pose.pose.position.y;
		odom_trans.transform.translation.z = 0.0;
		odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(yaw);
		odom_broadcaster.sendTransform(odom_trans);

		odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
		odom.pose.covariance[0] = msg->hdop * gps_variance;

		odom.pose.covariance[7] = msg->hdop * gps_variance;

		odom.pose.covariance[14] = 999999;

		odom.pose.covariance[21] = 999999;
		odom.pose.covariance[28] = 999999;


		odom_pub.publish(odom);

		prev_msg = *msg;
	}
}

 void push_back( QList<QConsoleWidgetCommand> cmd ){
     commandBuffers.push_front(cmd);
     resetIndex();
 }