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;
}
}
void stasis_publish_sync(struct stasis_subscription *sub, struct stasis_message *message)
{
ast_assert(sub != NULL);
publish_msg(sub->topic, message, sub);
}
bool SingleSynchronizer<T1, T2, T3>::publish() {
#ifdef PRINT
ROS_ERROR("called publish()\n");
#endif
if (buf_flag_) {
//image_obj_ranged is empty
if (type1_ringbuf_.begin() == type1_ringbuf_.end()) {
#ifdef PRINT
ROS_ERROR("type1 ring buffer is empty");
#endif
return false;
}
//image_raw is empty
if (type2_ringbuf_.begin() == type2_ringbuf_.end()) {
#ifdef PRINT
ROS_ERROR("type2 ring buffer is empty");
#endif
return false;
}
// image_obj_ranged > image_raw
if (get_time(&(type1_ringbuf_.front().header)) >= get_time(&(type2_ringbuf_.front().header))) {
p_type2_buf_ = &(type2_ringbuf_.front());
typename boost::circular_buffer<T1>::iterator it = type1_ringbuf_.begin();
if (type1_ringbuf_.size() == 1) {
p_type1_buf_ = &*it;
publish_msg(p_type1_buf_, p_type2_buf_);
if (is_req_ == true){
buf_flag_ = false;
is_req_ = false;
if (fabs_time_diff(&p_type1_buf_->header, &p_type2_buf_->header) == 0.0) {
type1_ringbuf_.clear();
type2_ringbuf_.clear();
}
}
return true;
} else {
for (it++; it != type1_ringbuf_.end(); it++) {
if (fabs_time_diff(&(type2_ringbuf_.front().header), &((it-1)->header))
< fabs_time_diff(&(type2_ringbuf_.front().header), &(it->header))) {
p_type1_buf_ = &*(it-1);
break;
}
}
if (it == type1_ringbuf_.end()) {
p_type1_buf_ = &(type1_ringbuf_.back());
}
}
}
// image_obj_ranged < image_raw
else {
p_type1_buf_ = &(type1_ringbuf_.front());
typename boost::circular_buffer<T2>::iterator it = type2_ringbuf_.begin();
if (type2_ringbuf_.size() == 1) {
p_type2_buf_ = &*it;
publish_msg(p_type1_buf_, p_type2_buf_);
if (is_req_ == true){
buf_flag_ = false;
is_req_ = false;
if (fabs_time_diff(&p_type1_buf_->header, &p_type2_buf_->header) == 0.0) {
type1_ringbuf_.clear();
type2_ringbuf_.clear();
}
}
return true;
}
for (it++; it != type2_ringbuf_.end(); it++) {
if (fabs_time_diff(&(type1_ringbuf_.front().header), &((it-1)->header))
< fabs_time_diff(&(type1_ringbuf_.front().header), &(it->header))) {
p_type2_buf_ = &*(it-1);
break;
}
}
if (it == type2_ringbuf_.end()) {
p_type2_buf_ = &(type2_ringbuf_.back());
}
}
publish_msg(p_type1_buf_, p_type2_buf_);
if (is_req_ == true){
buf_flag_ = false;
is_req_ = false;
if (fabs_time_diff(&p_type1_buf_->header, &p_type2_buf_->header) == 0.0) {
type1_ringbuf_.clear();
type2_ringbuf_.clear();
}
}
return true;
} else {
return false;
}
}
void stasis_publish(struct stasis_topic *topic, struct stasis_message *message)
{
publish_msg(topic, message, NULL);
}
