Esempio n. 1
0
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;
}
Esempio n. 2
0
  /** 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;
  }
Esempio n. 3
0
	void render(sf::RenderTarget& target)
	{
		float dx = 1.0f/static_cast<float>(line_data_.capacity());
		float x = static_cast<float>(line_data_.capacity()-line_data_.size())*dx;

		line_data_.push_back(std::make_pair(tick_data_, tick_tag_));		
		tick_tag_   = false;
				
		glBegin(GL_LINE_STRIP);
		auto c = color(color_);
		glColor4f(std::get<0>(c), std::get<1>(c), std::get<2>(c), 0.8f);		
		for(size_t n = 0; n < line_data_.size(); ++n)		
			if(line_data_[n].first > -0.5)
				glVertex3d(x+n*dx, std::max(0.05, std::min(0.95, (1.0f-line_data_[n].first)*0.8 + 0.1f)), 0.0);		
		glEnd();
				
		glEnable(GL_LINE_STIPPLE);
		glLineStipple(3, 0xAAAA);
		for(size_t n = 0; n < line_data_.size(); ++n)
		{
			if(line_data_[n].second)
			{
				glBegin(GL_LINE_STRIP);			
					glVertex3f(x+n*dx, 0.0f, 0.0f);				
					glVertex3f(x+n*dx, 1.0f, 0.0f);		
				glEnd();
			}
		}
		glDisable(GL_LINE_STIPPLE);
	}
Esempio n. 4
0
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);
}
Esempio n. 5
0
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);
}
Esempio n. 6
0
    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());
    }
Esempio n. 7
0
  /** 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();
  }
Esempio n. 8
0
  /** 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 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);
}
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();
}
Esempio n. 11
0
  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;
  }
Esempio n. 12
0
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);
}
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;
}
Esempio n. 14
0
/*! @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;
    }
}
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);
}
Esempio n. 16
0
double CTimeSmoother::EstimatePeriod(const boost::circular_buffer<double> &data, const vector<unsigned int> &intData)
{
  double sxy = 0, sxx = 0;
  for (unsigned int i = 0; i < data.size(); ++i)
  {
    sxy += intData[i] * data[i];
    sxx += intData[i] * intData[i];
  }
  return sxy/sxx;
}
Esempio n. 17
0
      /*! \brief Performs a pass of the correlator, calculating all of
          the \f$\left[W^{(1)}_{i} \cdot W^{(2)}_{i+j}\right]\f$
          values it can with the current data. 

	  These are summed up, ready to be divided by the pass count
	  in getAveragedCorrelator() to return averaged values.
      */
      void pass()
      {
	++_count;
	//Here, we're using the default constructor again to make a sum
	//variable with a value of zero.
	std::pair<T,T> sum = std::pair<T,T>();
	for (size_t i(0); i < _sample_history.size(); ++i)
	  {
	    sum.first += _sample_history[i].first;
	    sum.second += _sample_history[i].second;
	    _correlator[i] += elementwiseMultiply(sum.first, sum.second);
	  }
      }
Esempio n. 18
0
  bool fillCbuf()
  {
    int nCurSize = m_cbuf.size();
    int nToFill = m_nCbufLen - nCurSize;
    MB_ASSERT(nToFill>=0);
    if (nToFill==0) return true;
    int i;
    for (i=0; i<nToFill; ++i) {
      if (!fillOne())
        return false;
    }

    return true;
  }
GestureType::Type GestureClassifierByHistogram::classifyClass2Gesture(const boost::circular_buffer<size_t> &matchedHistogramIndexes) const
{
#if 0
	const size_t &matchedIdx = matchHistogramByGestureIdPattern(matchedHistogramIndexes, gestureIdPatternHistogramsForClass2Gesture_, params_.histDistThresholdForGestureIdPattern);
	switch (matchedIdx)
	{
	case :
		return GestureType::GT_HORIZONTAL_FLIP;
	case :
		return GestureType::GT_VERTICAL_FLIP;
	// FIXME [implement] >>
/*
	case :
		return GestureType::GT_JAMJAM;
	case :
		return GestureType::GT_SHAKE;
	case :
		return GestureType::GT_LEFT_90_TURN;
	case :
		return GestureType::GT_RIGHT_90_TURN;
*/
	}
#else
	// TODO [adjust] >> design parameter
	const size_t countThreshold(matchedHistogramIndexes.size() / 2);
	//const size_t countThreshold(params_.matchedIndexCountThresholdForClass2Gesture);

	const size_t &matchedIdx = matchHistogramByFrequency(matchedHistogramIndexes, countThreshold);
	switch (matchedIdx)
	{
	case 0:
		return GestureType::GT_HORIZONTAL_FLIP;
	case 1:
		return GestureType::GT_VERTICAL_FLIP;
	// FIXME [implement] >>
/*
	case :
		return GestureType::GT_JAMJAM;
	case :
		return GestureType::GT_SHAKE;
	case :
		return GestureType::GT_LEFT_90_TURN;
	case :
		return GestureType::GT_RIGHT_90_TURN;
*/
	}
#endif

	return GestureType::GT_UNDEFINED;
}
Esempio n. 20
0
  /** Reserve some part of the pipe for writing

      \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_write(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 write 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 room in the pipe, that
         may be changed when a read is done. Do not use a difference
         here because it is only about unsigned values */
      read_done.wait(ul, [&] { return cb.size() + s <= capacity(); });
    else if (cb.size() + s > capacity())
      // Not enough room in the pipe for the reservation
      return false;

    /* If there is enough room in the pipe, just create default values
         in it to do the reservation */
    for (std::size_t i = 0; i != s; ++i)
      cb.push_back();
    /* Compute the location of the first element a posteriori since it
         may not exist a priori if cb was empty before */
    auto first = cb.end() - s;
    /* Add a description of the reservation at the end of the
       reservation queue */
    w_rid_q.emplace_back(first, s);
    // Return the iterator to the last reservation descriptor
    rid = w_rid_q.end() - 1;
    TRISYCL_DUMP_T("After reservation cb.size() = " << cb.size()
                   << " size() = " << size());
    return true;
  }
Esempio n. 21
0
//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(); 
    }
Esempio n. 22
0
void CTimeSmoother::BinData(const boost::circular_buffer<double> &data, vector<double> &bins, const double threshold, const unsigned int minbinsize)
{
  if (!data.size())
    return;

  bins.clear();
  vector<unsigned int> counts;

  for (boost::circular_buffer<double>::const_iterator i = data.begin(); i != data.end(); ++i)
  {
    bool found = false;
    for (unsigned int j = 0; j < bins.size(); ++j)
    {
      if (fabs(*i - bins[j]) < threshold*bins[j])
      {
        found = true;
        // update our bin mean and count
        bins[j] = (bins[j]*counts[j] + *i)/(counts[j]+1);
        counts[j]++;
        break;
      }
    }
    if (!found)
    {
      bins.push_back(*i);
      counts.push_back(1);
    }
  }
  if (minbinsize)
  {
    assert(counts.size() == bins.size());
    assert(counts.size());
    // filter out any bins that are not large enough (and any bins that aren't positive)
    for (unsigned int j = 0; j < counts.size(); )
    {
      if (counts[j] < minbinsize || bins[j] < 0.05)
      {
        bins.erase(bins.begin() + j);
        counts.erase(counts.begin() + j);
      }
      else
        j++;
    }
  }
}
GestureType::Type GestureClassifierByHistogram::classifyClass1Gesture(const boost::circular_buffer<size_t> &matchedHistogramIndexes, const bool useGestureIdPattern) const
{
	// match histogram by gesture ID pattern
	if (useGestureIdPattern)
	{
		const size_t &matchedIdx = matchHistogramByGestureIdPattern(matchedHistogramIndexes, gestureIdPatternHistogramsForClass1Gesture_, params_.histDistThresholdForGestureIdPattern);
		switch (matchedIdx)
		{
		case 1:
			return GestureType::GT_LEFT_MOVE;
		case 2:
			return GestureType::GT_RIGHT_MOVE;
		case 3:
			return GestureType::GT_UP_MOVE;
		case 4:
			return GestureType::GT_DOWN_MOVE;
		}
	}
	// match histogram by frequency
	else
	{
		// TODO [adjust] >> design parameter
		const size_t countThreshold(matchedHistogramIndexes.size() / 2);
		//const size_t countThreshold(params_.matchedIndexCountThresholdForClass1Gesture);

		const size_t &matchedIdx = matchHistogramByFrequency(matchedHistogramIndexes, countThreshold);
		switch (matchedIdx)
		{
		case 0:
			return GestureType::GT_HAND_OPEN;
		//case :
		//	return GestureType::GT_HAND_CLOSE;
		}
	}

	return GestureType::GT_UNDEFINED;
}
 int size()const{
     return int( commandBuffers.size() );
 }
Esempio n. 25
0
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 callback(const sensor_msgs::ImageConstPtr &img,
                const sensor_msgs::CameraInfoConstPtr &info) {
    boost::mutex::scoped_lock lock(mutex_);
    ros::Time now = ros::Time::now();

    static boost::circular_buffer<double> in_times(100);
    static boost::circular_buffer<double> out_times(100);
    static boost::circular_buffer<double> in_bytes(100);
    static boost::circular_buffer<double> out_bytes(100);

    ROS_DEBUG("resize: callback");
    if ( !publish_once_ || cp_.getNumSubscribers () == 0 ) {
      ROS_DEBUG("resize: number of subscribers is 0, ignoring image");
      return;
    }

    in_times.push_front((now - last_subscribe_time_).toSec());
    in_bytes.push_front(img->data.size());
    //
    try {
        int width = dst_width_ ? dst_width_ : (resize_x_ * info->width);
        int height = dst_height_ ? dst_height_ : (resize_y_ * info->height);
        double scale_x = dst_width_ ? ((double)dst_width_)/info->width : resize_x_;
        double scale_y = dst_height_ ? ((double)dst_height_)/info->height : resize_y_;

        cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img);

        cv::Mat tmpmat(height, width, cv_img->image.type());
        cv::resize(cv_img->image, tmpmat, cv::Size(width, height));
        cv_img->image = tmpmat;

        sensor_msgs::CameraInfo tinfo = *info;
        tinfo.height = height;
        tinfo.width = width;
        tinfo.K[0] = tinfo.K[0] * scale_x; // fx
        tinfo.K[2] = tinfo.K[2] * scale_x; // cx
        tinfo.K[4] = tinfo.K[4] * scale_y; // fy
        tinfo.K[5] = tinfo.K[5] * scale_y; // cy

        tinfo.P[0] = tinfo.P[0] * scale_x; // fx
        tinfo.P[2] = tinfo.P[2] * scale_x; // cx
        tinfo.P[3] = tinfo.P[3] * scale_x; // T
        tinfo.P[5] = tinfo.P[5] * scale_y; // fy
        tinfo.P[6] = tinfo.P[6] * scale_y; // cy

        if ( !use_messages_ || now - last_publish_time_  > period_ ) {
            cp_.publish(cv_img->toImageMsg(),
                        boost::make_shared<sensor_msgs::CameraInfo> (tinfo));

            out_times.push_front((now - last_publish_time_).toSec());
            out_bytes.push_front(cv_img->image.total()*cv_img->image.elemSize());

            last_publish_time_ = now;
        }
    } catch( cv::Exception& e ) {
        ROS_ERROR("%s", e.what());
    }


    float duration =  (now - last_rosinfo_time_).toSec();
    if ( duration > 2 ) {
        int in_time_n = in_times.size();
        int out_time_n = out_times.size();
        double in_time_mean = 0, in_time_rate = 1.0, in_time_std_dev = 0.0, in_time_max_delta, in_time_min_delta;
        double out_time_mean = 0, out_time_rate = 1.0, out_time_std_dev = 0.0, out_time_max_delta, out_time_min_delta;

        std::for_each( in_times.begin(), in_times.end(), (in_time_mean += boost::lambda::_1) );
        in_time_mean /= in_time_n;
        in_time_rate /= in_time_mean;
        std::for_each( in_times.begin(), in_times.end(), (in_time_std_dev += (boost::lambda::_1 - in_time_mean)*(boost::lambda::_1 - in_time_mean) ) );
        in_time_std_dev = sqrt(in_time_std_dev/in_time_n);
        if ( in_time_n > 1 ) {
            in_time_min_delta = *std::min_element(in_times.begin(), in_times.end());
            in_time_max_delta = *std::max_element(in_times.begin(), in_times.end());
        }

        std::for_each( out_times.begin(), out_times.end(), (out_time_mean += boost::lambda::_1) );
        out_time_mean /= out_time_n;
        out_time_rate /= out_time_mean;
        std::for_each( out_times.begin(), out_times.end(), (out_time_std_dev += (boost::lambda::_1 - out_time_mean)*(boost::lambda::_1 - out_time_mean) ) );
        out_time_std_dev = sqrt(out_time_std_dev/out_time_n);
        if ( out_time_n > 1 ) {
            out_time_min_delta = *std::min_element(out_times.begin(), out_times.end());
            out_time_max_delta = *std::max_element(out_times.begin(), out_times.end());
        }

        double in_byte_mean = 0, out_byte_mean = 0;
        std::for_each( in_bytes.begin(), in_bytes.end(), (in_byte_mean += boost::lambda::_1) );
        std::for_each( out_bytes.begin(), out_bytes.end(), (out_byte_mean += boost::lambda::_1) );
        in_byte_mean /= duration;
        out_byte_mean /= duration;

        ROS_INFO_STREAM(" in  bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3)  << in_byte_mean/1000*8
                        << " Kbps rate:"   << std::fixed << std::setw(7) << std::setprecision(3) << in_time_rate
                        << " hz min:"      << std::fixed << std::setw(7) << std::setprecision(3) << in_time_min_delta
                        << " s max: "    << std::fixed << std::setw(7) << std::setprecision(3) << in_time_max_delta
                        << " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << in_time_std_dev << "s n: " << in_time_n);
        ROS_INFO_STREAM(" out bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3)  << out_byte_mean/1000*8
                        << " kbps rate:"   << std::fixed << std::setw(7) << std::setprecision(3) << out_time_rate
                        << " hz min:"      << std::fixed << std::setw(7) << std::setprecision(3) << out_time_min_delta
                        << " s max: "    << std::fixed << std::setw(7) << std::setprecision(3) << out_time_max_delta
                        << " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << out_time_std_dev << "s n: " << out_time_n);
        in_times.clear();
        in_bytes.clear();
        out_times.clear();
        out_bytes.clear();
        last_rosinfo_time_ = now;
    }

    last_subscribe_time_ = now;

    if(use_snapshot_) {
      publish_once_ = false;
    }
  }
Esempio n. 27
0
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());
    }

}
Esempio n. 28
0
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;
    }
}
Esempio n. 29
0
  /** Test if the pipe is empty

      This is obviously a volatile value which is constrained by
      restricted relativity.

      Note that on some devices it may be costly to implement on the
      write side (for example on FPGA).
  */
  bool empty() const {
    TRISYCL_DUMP_T("empty() cb.size() = " << cb.size()
                   << " size() = " << size());
    // It is empty when the size is zero, taking into account reservations
    return size() ==  0;
  }
Esempio n. 30
0
void ShowFrameDurationPlot() {
	
	Vec2i windowSize = mainApp->getWindow()->getSize();
	size_t maxSamples = size_t(windowSize.x);
	
	if(maxSamples != frameDurationPlotValues.capacity()) {
		frameDurationPlotValues.set_capacity(maxSamples);
	}
	if(maxSamples != frameDurationPlotVertices.size()) {
		frameDurationPlotVertices.resize(maxSamples);
	}
	
	GRenderer->ResetTexture(0);
	
	frameDurationPlotValues.push_front(toMs(g_platformTime.lastFrameDuration()));
	
	float avg = std::accumulate(frameDurationPlotValues.begin(), frameDurationPlotValues.end(), 0.f) / frameDurationPlotValues.size();
	float worst = *std::max_element(frameDurationPlotValues.begin(), frameDurationPlotValues.end());
	
	const float OFFSET_Y = 80.f;
	const float SCALE_Y = 4.0f;

	for(size_t i = 0; i < frameDurationPlotValues.size(); ++i)
	{
		float time = frameDurationPlotValues[i];
		frameDurationPlotVertices[i].color = Color::white.toRGB();
		frameDurationPlotVertices[i].p.x = i;
		frameDurationPlotVertices[i].p.y = OFFSET_Y + (time * SCALE_Y);
		frameDurationPlotVertices[i].p.z = 1.0f;
		frameDurationPlotVertices[i].w = 1.0f;
	}

	EERIEDRAWPRIM(Renderer::LineStrip, &frameDurationPlotVertices[0], frameDurationPlotValues.size());

	Color avgColor = Color::blue * 0.5f + Color::white * 0.5f;
	float avgPos = OFFSET_Y + (avg * SCALE_Y);
	drawLine(Vec2f(0, avgPos), Vec2f(windowSize.x, avgPos), 1.0f, Color::blue);

	Color worstColor = Color::red * 0.5f + Color::white * 0.5f;
	float worstPos = OFFSET_Y + (worst * SCALE_Y);
	drawLine(Vec2f(0, worstPos), Vec2f(windowSize.x, worstPos), 1.0f, Color::red);

	Font * font = hFontDebug;
	float lineOffset = font->getLineHeight() + 2;

	std::string labels[3] = { "Average: ", "Worst: ", "Current: " };
	Color colors[3] = { avgColor, worstColor, Color::white };
	float values[3] = { avg, worst, frameDurationPlotValues[0] };

	std::string texts[3];
	float widths[3];
	static float labelWidth = 0.f;
	static float valueWidth = 0.f;
	for(size_t i = 0; i < 3; i++) {
		// Format value
		std::ostringstream oss;
		oss << std::fixed << std::setprecision(2) << values[i] << " ms ("<< 1.f / (values[i] * 0.001f) << " FPS)";
		texts[i] = oss.str();
		// Calculate widths (could be done more efficiently for monospace fonts...)
		labelWidth = std::max(labelWidth, float(font->getTextSize(labels[i]).width()));
		widths[i] = font->getTextSize(texts[i]).width();
		valueWidth = std::max(valueWidth, widths[i]);
	}

	float x = 10;
	float y = 10;
	float xend = x + labelWidth + 10 + valueWidth;
	for(size_t i = 0; i < 3; i++) {
		font->draw(Vec2i(x, y), labels[i], Color::gray(0.8f));
		font->draw(Vec2i(xend - widths[i], y), texts[i], colors[i]);
		y += lineOffset;
	}

}