void checkInputsSynchronized()
{
int threshold = 3 * all_received_;
if (left_received_ >= threshold || right_received_ >= threshold ||
left_info_received_ >= threshold || right_info_received_ >= threshold) {
ROS_WARN("[stereo_processor] Low number of synchronized left/right/left_info/right_info tuples received.\n"
"Left images received: %d (topic '%s')\n"
"Right images received: %d (topic '%s')\n"
"Left camera info received: %d (topic '%s')\n"
"Right camera info received: %d (topic '%s')\n"
"Synchronized tuples: %d\n"
"Possible issues:\n"
"\t* stereo_image_proc is not running.\n"
"\t Does `rosnode info %s` show any connections?\n"
"\t* The cameras are not synchronized.\n"
"\t Try restarting the node with parameter _approximate_sync:=True\n"
"\t* The network is too slow. One or more images are dropped from each tuple.\n"
"\t Try restarting the node, increasing parameter 'queue_size' (currently %d)",
left_received_, left_sub_.getTopic().c_str(),
right_received_, right_sub_.getTopic().c_str(),
left_info_received_, left_info_sub_.getTopic().c_str(),
right_info_received_, right_info_sub_.getTopic().c_str(),
all_received_, ros::this_node::getName().c_str(), queue_size_);
}
}
void checkInputsSynchronized()
{
int threshold = 3 * all_received_;
if (image_received_ >= threshold || depth_received_ >= threshold ||
image_info_received_ >= threshold || depth_info_received_ >= threshold) {
ROS_WARN("[stereo_processor] Low number of synchronized image/depth/image_info/depth_info tuples received.\n"
"Images received: %d (topic '%s')\n"
"Depth images received: %d (topic '%s')\n"
"Image camera info received: %d (topic '%s')\n"
"Depth camera info received: %d (topic '%s')\n"
"Synchronized tuples: %d\n",
image_received_, image_sub_.getTopic().c_str(),
depth_received_, depth_sub_.getTopic().c_str(),
image_info_received_, image_info_sub_.getTopic().c_str(),
depth_info_received_, depth_info_sub_.getTopic().c_str(),
all_received_);
}
}
// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(message_filters::Subscriber<CameraInfo> &sub_cam,
message_filters::Subscriber<GroundPlane> &sub_gp,
image_transport::SubscriberFilter &sub_col,
image_transport::SubscriberFilter &sub_dep,
image_transport::ImageTransport &it) {
if(!pub_message.getNumSubscribers() && !pub_result_image.getNumSubscribers() && !pub_centres.getNumSubscribers() && !pub_detected_persons.getNumSubscribers()) {
ROS_DEBUG("Upper Body Detector: No subscribers. Unsubscribing.");
sub_cam.unsubscribe();
sub_gp.unsubscribe();
sub_col.unsubscribe();
sub_dep.unsubscribe();
} else {
ROS_DEBUG("Upper Body Detector: New subscribers. Subscribing.");
sub_cam.subscribe();
sub_gp.subscribe();
sub_col.subscribe(it,sub_col.getTopic().c_str(),1);
sub_dep.subscribe(it,sub_dep.getTopic().c_str(),1);
}
}
// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(ros::Subscriber &sub_msg,
ros::NodeHandle &n,
string gp_topic,
string img_topic,
Subscriber<GroundPlane> &sub_gp,
Subscriber<CameraInfo> &sub_cam,
image_transport::SubscriberFilter &sub_col,
image_transport::ImageTransport &it){
if(!pub_message.getNumSubscribers() && !pub_result_image.getNumSubscribers() && !pub_detected_persons.getNumSubscribers()) {
ROS_DEBUG("HOG: No subscribers. Unsubscribing.");
sub_msg.shutdown();
sub_gp.unsubscribe();
sub_cam.unsubscribe();
sub_col.unsubscribe();
} else {
ROS_DEBUG("HOG: New subscribers. Subscribing.");
if(strcmp(gp_topic.c_str(), "") == 0) {
sub_msg = n.subscribe(img_topic.c_str(), 1, &imageCallback);
}
sub_cam.subscribe();
sub_gp.subscribe();
sub_col.subscribe(it,sub_col.getTopic().c_str(),1);
}
}
