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