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

  int readOneImpl()
  {
    if (!m_bReady)
      return -1; // delimiter has been matched --> read fails
    
    int nCurSize = m_cbuf.size();

    if (m_bEOF) {
      // EOF has been encountered --> returns the cbuf content
      if (nCurSize>0) {
        unsigned char val = m_cbuf.front();
        m_cbuf.pop_front();
        return val;
      }
      // EOF reached and cbuf is empty
      return -1; // failed!!
    }

    if (nCurSize<m_nCbufLen) {
      if (!fillCbuf())
        return -1; // failed!!
    }

    unsigned char val = m_cbuf.front();
    m_cbuf.pop_front();

    fillOne();
    
    return val;
  }

boost::shared_ptr< const MapsBuffer::MapsRgb > 
MapsBuffer::getFront(bool print)
{
  boost::shared_ptr< const MapsBuffer::MapsRgb > depth_rgb;
  {
    boost::mutex::scoped_lock buff_lock (bmutex_);
    while (buffer_.empty ())
    {
      if (is_done)
        break;
      {
        boost::mutex::scoped_lock io_lock (io_mutex);
              //std::cout << "No data in buffer_ yet or buffer is empty." << std::endl;
      }
      buff_empty_.wait (buff_lock);
    }
    depth_rgb = buffer_.front ();
    buffer_.pop_front ();
  }
  
  if(print)
    PCL_INFO("%d maps left in the buffer...\n", buffer_.size ());
  
  return (depth_rgb);
}

  /** Process the read reservations that are ready to be released in the
      reservation queue
  */
  void move_read_reservation_forward() {
    // Lock the pipe to avoid nuisance
    std::lock_guard<std::mutex> lg { cb_mutex };

    for (;;) {
      if (r_rid_q.empty())
        // No pending reservation, so nothing to do
        break;
      if (!r_rid_q.front().ready)
        /* If the first reservation is not ready to be released, stop
           because it is blocking all the following in the queue
           anyway */
        break;
      // Remove the reservation to be released from the queue
      r_rid_q.pop_front();
      std::size_t n_to_pop;
      if (r_rid_q.empty())
        // If it was the last one, remove all the reservation
        n_to_pop = read_reserved_frozen;
      else
        // Else remove everything up to the next reservation
        n_to_pop =  r_rid_q.front().start - cb.begin();
      // No longer take into account these reserved slots
      read_reserved_frozen -= n_to_pop;
      // Release the elements from the FIFO
      while (n_to_pop--)
        cb.pop_front();
      // Notify the clients waiting for some room to write in the pipe
      read_done.notify_all();
      /* ...and process the next reservation to see if it is ready to
         be released too */
    }
  }

  /** Try to read a value from the pipe

      \param[out] value is the reference to where to store what is
      read

      \param[in] blocking specify if the call wait for the operation
      to succeed

      \return true on success
  */
  bool read(T &value, bool blocking = false) {
    // Lock the pipe to avoid being disturbed
    std::unique_lock<std::mutex> ul { cb_mutex };
    TRISYCL_DUMP_T("Read pipe empty = " << empty());

    if (blocking)
      /* If in blocking mode, wait for the not empty condition, that
         may be changed when a write is done */
      write_done.wait(ul, [&] { return !empty(); });
    else if (empty())
      return false;

    TRISYCL_DUMP_T("Read pipe front = " << cb.front()
                   << " back = " << cb.back()
                   << " reserved_for_reading() = " << reserved_for_reading());
    if (read_reserved_frozen)
      /** If there is a pending reservation, read the next element to
          be read and update the number of reserved elements */
      value = cb.begin()[read_reserved_frozen++];
    else {
      /* There is no pending read reservation, so pop the read value
         from the pipe */
      value = cb.front();
      cb.pop_front();
    }

    TRISYCL_DUMP_T("Read pipe value = " << value);
    // Notify the clients waiting for some room to write in the pipe
    read_done.notify_all();
    return true;
  }

void take() {
    while(flag) {
        unique_lock<mutex> locker(m_mutex);
        while(Q.empty())
            full.wait(locker);
        if(flag) {
            assert(!Q.empty());
            cout << "# " << Q.front() <<endl;
            Q.pop_front();
            empty.notify_all();
        }
    }
}

pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr 
PCDBuffer::getFront ()
{
	pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud;
	{
		boost::mutex::scoped_lock buff_lock (bmutex_);
		while (buffer_.empty ())
		{
			if (is_done)
				break;
			{
				boost::mutex::scoped_lock io_lock (io_mutex);
				//std::cout << "No data in buffer_ yet or buffer is empty." << std::endl;
			}
			buff_empty_.wait (buff_lock);
		}
		cloud = buffer_.front ();
		buffer_.pop_front ();
	}
	return (cloud);
}

void gpsFixCallBack (geometry_msgs::PoseStamped gpsPose)
{
    geometry_msgs::PoseStamped gpsFixPoseEstimate;

    // find poseAtRequestTime
    bool odomMovementFound = false;
    pair<geometry_msgs::Pose, ros::Time> prevFront;

    if (cb.size() > 0)
    {
        odomMovementFound = true;
        cout << "odom found" << endl;
        prevFront = cb.front();

        if (gpsPose.header.stamp >= cb.front().second)
        {
            while (gpsPose.header.stamp > prevFront.second && cb.size() > 0)
            {
                prevFront = cb.front();
                cb.pop_front();
            }
        }
    }

    // calculate Odom difference
    double deltaX = 0;
    double deltaY = 0;

    if (odomMovementFound)
    {
        deltaX = (current.first.position.x - prevFront.first.position.x);
        deltaY = (current.first.position.y - prevFront.first.position.y);
    }

    gpsFixPoseEstimate.pose.position.x = gpsPose.pose.position.x + deltaX;
    gpsFixPoseEstimate.pose.position.y = gpsPose.pose.position.y + deltaY;
    tf::Quaternion gpsQaut, odomCurrentQuat, odomOldQuat;

    if (odomMovementFound)
    {
        tf::quaternionMsgToTF (current.first.orientation, odomCurrentQuat);
        tf::quaternionMsgToTF (prevFront.first.orientation, odomOldQuat);
    }

    tf::quaternionMsgToTF (gpsPose.pose.orientation, gpsQaut);
    double deltaYaw = 0;

    if (odomMovementFound)
    {
        deltaYaw = (tf::getYaw (odomCurrentQuat) - tf::getYaw (odomOldQuat));
    }

    double newYaw = tf::getYaw (gpsQaut) + deltaYaw;
    gpsFixPoseEstimate.pose.orientation = tf::createQuaternionMsgFromYaw (newYaw);
    cout << "new Yaw: "  << newYaw << endl;
    // create covariance


    // publish new estimated pose
    gpsFixPoseEstimate.header.stamp = ros::Time::now();
    gpsFixPoseEstimate.header.frame_id = gpsFrame;
    geometry_msgs::PoseStamped inMapFrame;

    try
    {
        tfListener->transformPose ("/map", ros::Time::now(), gpsFixPoseEstimate, gpsFrame, inMapFrame);
        geometry_msgs::PoseWithCovarianceStamped resultPose;
        resultPose.pose.pose = inMapFrame.pose;
        resultPose.header = inMapFrame.header;


        if (odomMovementFound)
        {
            resultPose.pose.covariance[0] = sqrt (abs (current.first.position.x - prevFront.first.position.x)); // X
            resultPose.pose.covariance[7] = sqrt (abs (current.first.position.y - prevFront.first.position.y)); // Y
            resultPose.pose.covariance[35] = sqrt (abs (newYaw)); // Yaw
        }
        else
        {
            resultPose.pose.covariance[0]  = 0.1;// X
            resultPose.pose.covariance[7]  = 0.1; // Y
            resultPose.pose.covariance[35] = 0.1; // Yaw
        }

        gpsOdomCombinedLocalisationPublisher.publish (resultPose);
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR ("%s", ex.what());
    }

}