/*! @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 image_obj_callback(const autoware_msgs::image_obj::ConstPtr& image_obj_msg) {
pthread_mutex_lock(&mutex);
image_obj_ringbuf.push_front(*image_obj_msg);
//vscan_image is empty
if (vscan_image_ringbuf.begin() == vscan_image_ringbuf.end()) {
pthread_mutex_unlock(&mutex);
ROS_INFO("vscan_image ring buffer is empty");
return;
}
buf_flag = true;
// image_obj > vscan_image
if (get_time(&(image_obj_ringbuf.front().header)) >= get_time(&(vscan_image_ringbuf.front().header))) {
vscan_image_buf = vscan_image_ringbuf.front();
boost::circular_buffer<autoware_msgs::image_obj>::iterator it = image_obj_ringbuf.begin();
if (image_obj_ringbuf.size() == 1) {
image_obj_buf = *it;
pthread_mutex_unlock(&mutex);
return;
} else {
for (it++; it != image_obj_ringbuf.end(); it++) {
if (fabs_time_diff(&(vscan_image_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(vscan_image_ringbuf.front().header), &(it->header))) {
image_obj_buf = *(it-1);
break;
}
}
if (it == image_obj_ringbuf.end()) {
image_obj_buf = image_obj_ringbuf.back();
}
}
} else {
image_obj_buf = image_obj_ringbuf.front();
boost::circular_buffer<autoware_msgs::PointsImage>::iterator it = vscan_image_ringbuf.begin();
if (vscan_image_ringbuf.size() == 1) {
vscan_image_buf = *it;
pthread_mutex_unlock(&mutex);
return;
}
for (it++; it != vscan_image_ringbuf.end(); it++) {
if (fabs_time_diff(&(image_obj_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(image_obj_ringbuf.front().header), &(it->header))) {
vscan_image_buf = *(it-1);
break;
}
}
if (it == vscan_image_ringbuf.end()) {
vscan_image_buf = vscan_image_ringbuf.back();
}
}
pthread_mutex_unlock(&mutex);
}
void image_obj_ranged_callback(const cv_tracker::image_obj_ranged::ConstPtr& image_obj_ranged_msg) {
pthread_mutex_lock(&mutex);
image_obj_ranged_ringbuf.push_front(*image_obj_ranged_msg);
//image_raw is empty
if (image_raw_ringbuf.begin() == image_raw_ringbuf.end()) {
pthread_mutex_unlock(&mutex);
ROS_INFO("image_raw ring buffer is empty");
return;
}
buf_flag = true;
// image_obj_ranged > image_raw
if (get_time(&(image_obj_ranged_ringbuf.front().header)) >= get_time(&(image_raw_ringbuf.front().header))) {
image_raw_buf = image_raw_ringbuf.front();
boost::circular_buffer<cv_tracker::image_obj_ranged>::iterator it = image_obj_ranged_ringbuf.begin();
if (image_obj_ranged_ringbuf.size() == 1) {
image_obj_ranged_buf = *it;
pthread_mutex_unlock(&mutex);
return;
} else {
for (it++; it != image_obj_ranged_ringbuf.end(); it++) {
if (fabs_time_diff(&(image_raw_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(image_raw_ringbuf.front().header), &(it->header))) {
image_obj_ranged_buf = *(it-1);
break;
}
}
if (it == image_obj_ranged_ringbuf.end()) {
image_obj_ranged_buf = image_obj_ranged_ringbuf.back();
}
}
} else {
image_obj_ranged_buf = image_obj_ranged_ringbuf.front();
boost::circular_buffer<sensor_msgs::Image>::iterator it = image_raw_ringbuf.begin();
if (image_raw_ringbuf.size() == 1) {
image_raw_buf = *it;
pthread_mutex_unlock(&mutex);
return;
}
for (it++; it != image_raw_ringbuf.end(); it++) {
if (fabs_time_diff(&(image_obj_ranged_ringbuf.front().header), &((it-1)->header))
< fabs_time_diff(&(image_obj_ranged_ringbuf.front().header), &(it->header))) {
image_raw_buf = *(it-1);
break;
}
}
if (it == image_raw_ringbuf.end()) {
image_raw_buf = image_raw_ringbuf.back();
}
}
pthread_mutex_unlock(&mutex);
}
/*! @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;
}
}
/** 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;
}
/** 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_;
}
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;
}
}
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;
}
}
