Пример #1
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;
    }
}
Пример #2
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);
}
Пример #3
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);
}
Пример #4
0
/*! @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;
    }
}
Пример #5
0
  /** 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;
  }
Пример #6
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::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();
}
	virtual bool OnGetData(sf::SoundStream::Chunk& data) override
	{		
		win32_exception::ensure_handler_installed_for_thread(
				"sfml-audio-thread");
		std::pair<std::shared_ptr<core::read_frame>, std::shared_ptr<audio_buffer_16>> audio_data;

		input_.pop(audio_data); // Block until available

		graph_->set_value("tick-time", perf_timer_.elapsed()*format_desc_.fps*0.5);		
		perf_timer_.restart();

		container_.push_back(std::move(*audio_data.second));
		data.Samples = container_.back().data();
		data.NbSamples = container_.back().size();	
		

		if (audio_data.first)
			presentation_age_ = audio_data.first->get_age_millis();

		return is_running_;
	}
Пример #9
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;
    }
}
Пример #10
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;
    }
}