//! Dump Legendre polynomial cache data to stream (table and history). void dumpLegendrePolynomialCacheData( std::ostream& outputStream, boost::unordered_map< Point, double > cacheTable, boost::circular_buffer< Point > cacheHistory ) { outputStream << "Table:\n"; for ( boost::unordered_map< Point, double >::iterator iteratorCacheTable = cacheTable.begin( ); iteratorCacheTable != cacheTable.end( ); iteratorCacheTable++ ) { outputStream << "\t" << writeLegendrePolynomialStructureToString( iteratorCacheTable->first ).c_str( ) << " => " << iteratorCacheTable->second << std::endl; } outputStream << "History:\n"; for ( boost::circular_buffer< Point >::iterator iteratorCacheHistory = cacheHistory.begin( ); iteratorCacheHistory != cacheHistory.end( ); iteratorCacheHistory++ ) { outputStream << "\t" << writeLegendrePolynomialStructureToString( *iteratorCacheHistory ).c_str( ) << ", "; } outputStream << std::endl; }
void update() { ALint processed; alGetSourceiv(src_, AL_BUFFERS_PROCESSED, &processed); std::vector<ALuint> unqueued(processed); alSourceUnqueueBuffers(src_, processed, &unqueued.front()); int queuing_count = 0; for (; queuing_count < processed; ++queuing_count) { if (not loop_play_ and loader_->is_end()) { loader_.reset(); ++queuing_count; break; } if (loader_->is_end()) { ticks_.push_back(0); } buf_sizes_.push_back(loader_->load_buffer(unqueued[queuing_count])); ticks_.push_back(loader_->midi_ticks()); } alSourceQueueBuffers(src_, queuing_count, &unqueued.front()); if (fade_milli_ != 0) { SET_CONTEXT(ctx_); loop_count_++; if (fade_ended()) { alSourceStop(src_); } else { alSourcef(src_, AL_GAIN, current_volume()); } } }
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; }
/** 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 set_buffer_loader(EASYRPG_SHARED_PTR<buffer_loader> const &l) { SET_CONTEXT(ctx_); alSourceStop(src_); alSourcei(src_, AL_BUFFER, AL_NONE); if (not l) { loader_.reset(); return; } ALint unqueuing_count; alGetSourceiv(src_, AL_BUFFERS_QUEUED, &unqueuing_count); std::vector<ALuint> unqueued(unqueuing_count); alSourceUnqueueBuffers(src_, unqueuing_count, &unqueued.front()); loader_ = l; int queuing_count = 0; BOOST_ASSERT(not l->is_end()); ticks_.push_back(0); for (; queuing_count < BUFFER_NUMBER; ++queuing_count) { buf_sizes_.push_back(loader_->load_buffer(buffers_[queuing_count])); ticks_.push_back(loader_->midi_ticks()); if (loader_->is_end()) { queuing_count++; break; } } alSourceQueueBuffers(src_, queuing_count, buffers_.data()); alSourcePlay(src_); }
/** Reserve some part of the pipe for reading \param[in] s is the number of element to reserve \param[out] rid is an iterator to a description of the reservation that has been done if successful \param[in] blocking specify if the call wait for the operation to succeed \return true if the reservation was successful */ bool reserve_read(std::size_t s, rid_iterator &rid, bool blocking = false) { // Lock the pipe to avoid being disturbed std::unique_lock<std::mutex> ul { cb_mutex }; TRISYCL_DUMP_T("Before read reservation cb.size() = " << cb.size() << " size() = " << size()); if (s == 0) // Empty reservation requested, so nothing to do return false; if (blocking) /* If in blocking mode, wait for enough elements to read in the pipe for the reservation. This condition can change when a write is done */ write_done.wait(ul, [&] { return s <= size(); }); else if (s > size()) // Not enough elements to read in the pipe for the reservation return false; // Compute the location of the first element of the reservation auto first = cb.begin() + read_reserved_frozen; // Increment the number of frozen elements read_reserved_frozen += s; /* Add a description of the reservation at the end of the reservation queue */ r_rid_q.emplace_back(first, s); // Return the iterator to the last reservation descriptor rid = r_rid_q.end() - 1; TRISYCL_DUMP_T("After reservation cb.size() = " << cb.size() << " size() = " << size()); return true; }
/*! @brief Returns true if an n click has occured in the given times and durations @param n the number of 'quick' consecutive clicks @param times a circular buffer of the click times @param durations a circular buffer of the click durations (needed to throw out long clicks) @param previoustime the previous time that this event has occured */ bool BehaviourProvider::nClick(unsigned int n, const boost::circular_buffer<float>& times, const boost::circular_buffer<float>& durations, float& previoustime) { size_t buffersize = times.size(); if (buffersize < n) // if there aren't enough values in the buffer return false return false; else if (previoustime == times.back()) // if previous time has not changed return false return false; else if (m_current_time - times.back() < 500) // need to wait 500 ms for a potential next click return false; else { // n click if the last n presses are each less than 400ms apart for (size_t i = buffersize-1; i > buffersize-n; i--) { if (times[i] - times[i-1] > 500 || durations[i] > 800) return false; } // check the n+1 click was longer than 400ms if (buffersize-n > 0) { if (times[buffersize-n] - times[buffersize-n-1] < 500 || durations[buffersize-n] > 800) return false; } previoustime = times.back(); return true; } }
/** 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 */ } }
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); }
size_t GestureClassifierByHistogram::matchHistogramByGestureIdPattern(const boost::circular_buffer<size_t> &matchedHistogramIndexes, const std::vector<histogram_type> &gestureIdPatternHistograms, const double histDistThreshold) const { // create matched ID histogram #if 0 cv::MatND hist; cv::calcHist( &cv::Mat(std::vector<unsigned char>(matchedHistogramIndexes.begin(), matchedHistogramIndexes.end())), 1, phaseHistChannels, cv::Mat(), hist, histDims, phaseHistSize, phaseHistRanges, true, false ); #else cv::MatND hist = cv::MatND::zeros(local::gesturePatternHistogramBinNum, 1, CV_32F); float *binPtr = (float *)hist.data; for (boost::circular_buffer<size_t>::const_iterator it = matchedHistogramIndexes.begin(); it != matchedHistogramIndexes.end(); ++it) if (*it != (size_t)-1) ++(binPtr[*it]); #endif // match histogram double minHistDist = std::numeric_limits<double>::max(); const size_t &matchedIdx = gestureIdPatternHistograms.empty() ? -1 : HistogramMatcher::match(gestureIdPatternHistograms, hist, minHistDist); // FIXME [delete] >> //std::cout << "\t\t\t*** " << minHistDist << std::endl; return minHistDist < histDistThreshold ? matchedIdx : -1; }
bool calculate_heading(double& heading) { bool ret = false; if(gps_points_buffer.capacity() == gps_points_buffer.size()) { gps_points_t p_comp = gps_points_buffer[0]; for(size_t i=1; i < gps_points_buffer.size();i++) { gps_points_t p_cur = gps_points_buffer[i]; double dist = sqrt(pow(p_comp.x - p_cur.x,2) + pow(p_comp.y - p_cur.y,2)); if(dist > heading_threshold) { heading = atan2(p_comp.y - p_cur.y,p_comp.x - p_cur.x); ret = true; break; } } } return ret; }
void WaypointVelocityVisualizer::controlCallback(const geometry_msgs::PoseStamped::ConstPtr& current_pose_msg, const geometry_msgs::TwistStamped::ConstPtr& current_twist_msg, const geometry_msgs::TwistStamped::ConstPtr& command_twist_msg) { // buffers are reset when time goes back, e.g. playback rosbag ros::Time current_time = ros::Time::now(); if (previous_time_ > current_time) { ROS_WARN("Detected jump back in time of %.3fs. Clearing markers and buffers.", (previous_time_ - current_time).toSec()); deleteMarkers(); // call 'DELETEALL' resetBuffers(); // clear circular buffers } previous_time_ = current_time; // if plot_metric_interval <= 0, velocity is plotted by each callback. if (plot_metric_interval_ > 0 && current_pose_buf_.size() > 0) { tf::Vector3 p1, p2; tf::pointMsgToTF(current_pose_buf_.back().pose.position, p1); tf::pointMsgToTF(current_pose_msg->pose.position, p2); if (!(p1.distance(p2) > plot_metric_interval_)) return; // skipping plot } current_pose_buf_.push_back(*current_pose_msg); current_twist_buf_.push_back(*current_twist_msg); command_twist_buf_.push_back(*command_twist_msg); current_twist_marker_array_.markers.clear(); command_twist_marker_array_.markers.clear(); createVelocityMarker(current_pose_buf_, current_twist_buf_, "current_velocity", current_twist_color_, current_twist_marker_array_); createVelocityMarker(current_pose_buf_, command_twist_buf_, "twist_cmd", command_twist_color_, command_twist_marker_array_); publishVelocityMarker(); }
void GestureClassifierByHistogram::drawMatchedIdPatternHistogram(const boost::circular_buffer<size_t> &matchedHistogramIndexes, const std::string &windowName) const { // calculate matched index histogram cv::MatND hist; #if defined(__GNUC__) { cv::Mat tmpmat(std::vector<unsigned char>(matchedHistogramIndexes.begin(), matchedHistogramIndexes.end())); cv::calcHist(&tmpmat, 1, local::indexHistChannels, cv::Mat(), hist, local::histDims, local::indexHistSize, local::indexHistRanges, true, false); } #else cv::calcHist(&cv::Mat(std::vector<unsigned char>(matchedHistogramIndexes.begin(), matchedHistogramIndexes.end())), 1, local::indexHistChannels, cv::Mat(), hist, local::histDims, local::indexHistSize, local::indexHistRanges, true, false); #endif // normalize histogram //HistogramUtil::normalizeHistogram(hist, params_.maxMatchedHistogramNum); // draw matched index histogram cv::Mat histImg(cv::Mat::zeros(local::indexHistMaxHeight, local::indexHistBins*local::indexHistBinWidth, CV_8UC3)); HistogramUtil::drawHistogram1D(hist, local::indexHistBins, params_.maxMatchedHistogramNum, local::indexHistBinWidth, local::indexHistMaxHeight, histImg); std::ostringstream sstream; sstream << "count: " << matchedHistogramIndexes.size(); cv::putText(histImg, sstream.str(), cv::Point(10, 15), cv::FONT_HERSHEY_COMPLEX, 0.5, CV_RGB(255, 0, 255), 1, 8, false); cv::imshow(windowName, histImg); }
void emit_block(size_t line_number, std::function<void (const std::string &, int, size_t)> fun) { std::string content; for (auto x : buffer) { content += x; } fun(content, line_number - buffer.size(), buffer.size()); }
QList<QConsoleWidgetCommand> upOne(){ if(commandBuffers.empty()){return QList<QConsoleWidgetCommand>();} ++currentIndex; if( currentIndex< size() && currentIndex>=0){ return commandBuffers[currentIndex ]; }else{ currentIndex= size(); } return *(commandBuffers.rbegin()); }
/** Get the current number of elements in the pipe that can be read This is obviously a volatile value which is constrained by the theory of restricted relativity. Note that on some devices it may be costly to implement (for example on FPGA). */ std::size_t size() const { TRISYCL_DUMP_T("size() cb.size() = " << cb.size() << " cb.end() = " << (void *)&*cb.end() << " reserved_for_reading() = " << reserved_for_reading() << " reserved_for_writing() = " << reserved_for_writing()); /* The actual number of available elements depends from the elements blocked by some reservations. This prevents a consumer to read into reserved area. */ return cb.size() - reserved_for_reading() - reserved_for_writing(); }
// Helper function: compute the median of a circular buffer double circ_buff_median(const boost::circular_buffer<double>& cb) const { // FIXME: naive implementation; creates a copy as a vector std::vector<double> v; for (boost::circular_buffer<double>::const_iterator i = cb.begin(); i != cb.end(); ++i) { v.push_back(*i); } size_t n = v.size() / 2; std::nth_element(v.begin(), v.begin()+n, v.end()); return v[n]; }
bool PCDBuffer::pushBack (pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud) { bool retVal = false; { boost::mutex::scoped_lock buff_lock (bmutex_); if (!buffer_.full ()) retVal = true; buffer_.push_back (cloud); } buff_empty_.notify_one (); return (retVal); }
bool MapsBuffer::pushBack(boost::shared_ptr<const MapsRgb> maps_rgb ) { bool retVal = false; { boost::mutex::scoped_lock buff_lock (bmutex_); if (!buffer_.full ()) retVal = true; buffer_.push_back (maps_rgb); } buff_empty_.notify_one (); return (retVal); }
void put(int x) { for(auto i=0;i<x;i++) { unique_lock<mutex> locker(m_mutex); while(Q.full()) empty.wait(locker); assert(!Q.full()); Q.push_back(i); cout << "@ "<< i <<endl; full.notify_all(); } flag = false; }
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(); } } }
void scan(std::istream& stream, std::function<void (const std::string &, int, size_t)> fun) { auto line_number = 0; for (std::string line; std::getline(stream, line);) { line_number++; metrics.lines_scanned++; if (line.length() > 0) { buffer.push_back(line); if (buffer.full()) { emit_block(line_number, fun); } } } }
ros::Time find(ros::Time sensor_time) { boost::circular_buffer<ros::Time>::iterator it = sensor.begin(); for (int i = 0; it != sensor.end(); it++, i++) { if (it->sec == sensor_time.sec && it->nsec == sensor_time.nsec) { return execution.at(i); // find } } ROS_ERROR("error:not found a pair"); ros::Time failed; failed.sec = 0; failed.nsec = 0; return failed; // not find }
/*! @brief Returns true if the last press was a long one @param times a circular buffer of the click times @param durations a circular buffer of the click durations @param previoustime the previous time that this event has occured */ bool BehaviourProvider::longClick(const boost::circular_buffer<float>& times, const boost::circular_buffer<float>& durations, float& previoustime) { if (times.empty()) return false; else if (previoustime == times.back()) return false; else if (durations.back() <= 800) return false; else { previoustime = m_current_time; return true; } }
//TODO: change this a lot! void updateGyroState(sensor_msgs::Imu& imuMsg, packet_t rxPkt, boost::circular_buffer<float>& calibration) { uint8_t gyro_adc = rxPkt.payload[0]; double current_time = ros::Time::now().toSec(); double last_time = imuMsg.header.stamp.toSec(); if (!isMoving) { calibration.push_back(float(gyro_adc)); double total = 0; BOOST_FOREACH( float reading, calibration ) { total += reading; } cal_offset = total / calibration.size(); }
void WaypointVelocityVisualizer::createVelocityMarker(const boost::circular_buffer<geometry_msgs::PoseStamped>& poses, const boost::circular_buffer<geometry_msgs::TwistStamped>& twists, const std::string& ns, const std_msgs::ColorRGBA& color, visualization_msgs::MarkerArray& markers) { assert(poses.size() == twists.size()); std::vector<nav_msgs::Odometry> waypoints; for (unsigned int i = 0; i < poses.size(); ++i) { nav_msgs::Odometry odom; odom.pose.pose = poses[i].pose; odom.twist.twist = twists[i].twist; waypoints.push_back(odom); } createVelocityMarker(waypoints, ns, color, markers); }
GestureType::Type GestureClassifierByHistogram::classifyClass3Gesture(const boost::circular_buffer<size_t> &matchedHistogramIndexes) const { #if 0 const size_t &matchedIdx = matchHistogramByGestureIdPattern(matchedHistogramIndexes, gestureIdPatternHistogramsForThirdClassGesture_, params_.histDistThresholdForGestureIdPattern); switch (matchedIdx) { case : return GestureType::GT_INFINITY; case : return GestureType::GT_TRIANGLE; } #else // TODO [adjust] >> design parameter const size_t countThreshold(matchedHistogramIndexes.size() / 2); //const size_t countThreshold(params_.matchedIndexCountThresholdForClass3Gesture); const size_t &matchedIdx = matchHistogramByFrequency(matchedHistogramIndexes, countThreshold); switch (matchedIdx) { /* case : return GestureType::GT_LEFT_FAST_MOVE; case : return GestureType::GT_RIGHT_FAST_MOVE; */ case 0: return GestureType::GT_INFINITY; case 1: return GestureType::GT_TRIANGLE; } #endif return GestureType::GT_UNDEFINED; }
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_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()) { ROS_INFO("vscan_image 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(); } }
bool PythonServer::LoadChatHistory(boost::circular_buffer<ChatHistoryEntity>& chat_history) { boost::python::object chat_provider = m_python_module_chat.attr("__dict__")["chat_history_provider"]; if (!chat_provider) { ErrorLogger() << "Unable to get Python object chat_history_provider"; return false; } boost::python::object f = chat_provider.attr("load_history"); if (!f) { ErrorLogger() << "Unable to call Python method load_history"; return false; } boost::python::object r = f(); boost::python::extract<list> py_history(r); if (py_history.check()) { boost::python::stl_input_iterator<boost::python::tuple> entity_begin(py_history), entity_end; for (auto& it = entity_begin; it != entity_end; ++ it) { ChatHistoryEntity e; e.m_timestamp = boost::posix_time::from_time_t(boost::python::extract<time_t>((*it)[0]));; e.m_player_name = boost::python::extract<std::string>((*it)[1]); e.m_text = boost::python::extract<std::string>((*it)[2]); e.m_text_color = boost::python::extract<GG::Clr>((*it)[3]); chat_history.push_back(e); } } return true; }