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

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

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

  /** Try to write a value to the pipe

      \param[in] value is what we want to write

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

      \return true on success

      \todo provide a && version
  */
  bool write(const T &value, bool blocking = false) {
    // Lock the pipe to avoid being disturbed
    std::unique_lock<std::mutex> ul { cb_mutex };
    TRISYCL_DUMP_T("Write pipe full = " << full()
                   << " value = " << value);

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

    cb.push_back(value);
    TRISYCL_DUMP_T("Write pipe front = " << cb.front()
                   << " back = " << cb.back()
                   << " cb.begin() = " << (void *)&*cb.begin()
                   << " cb.size() = " << cb.size()
                   << " cb.end() = " << (void *)&*cb.end()
                   << " reserved_for_reading() = " << reserved_for_reading()
                   << " reserved_for_writing() = " << reserved_for_writing());
    // Notify the clients waiting to read something from the pipe
    write_done.notify_all();
    return true;
  }

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

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 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());
    }

}

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

 ros::Time front(void) {
     return execution.front();
 }