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