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 WaypointVelocityVisualizer::resetBuffers() { current_pose_buf_.clear(); current_twist_buf_.clear(); command_twist_buf_.clear(); }
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 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; } }