Real time Image topic subscription

edit

Hi everyone, im trying to work on an object detector in ROS, using the deep learning modules from OpenCV. My problem is related to the velocity of the node. The camera node publish at 20 hz, and when I do not activate the deep learning part, my node publish equally fast. When I activate the deep learning functions, the rate drops to 15 hz, so it shouldnt be a great problem.

However, when I look at the results (Im working over a Rosbag), what I see, is that the Image that is published and edited by my node, comes like 4 seconds later than the original image. What I mean I want to do, is that the node, no matter if it has to skip frames, uses the actual frame he becomes from the publishing topic, the result of it would be a not so smooth sequence of images, but it would be showing the present time. What I suspect its happening now, its that the node is just picking the next image that comes from the camera topic, like it is being saved on a buffer.

This is my code:

import rospy
from std_msgs.msg import Header
from dynamic_reconfigure.server import Server
import cv2 as cv
import numpy as np
from cv_bridge import CvBridge,CvBridgeError
from tensorflow_ros_tool.cfg import ObjectConfig
from tensorflow_ros_tool.interface.ObjectInterface import ObjectInterface
class Object:
def callback_subscriber(self, msg):
    """
    Called when a new message arrives
    """
    # do your stuff here

    cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")
    now = rospy.get_rostime()

    self.cvNet.setInput(cv.dnn.blobFromImage(cv.resize(cv_image, (300, 300)), 0.007843,(300, 300), 127.5))

    cvOut = self.cvNet.forward()
    tama=cvOut.shape
    print(tama)
    print(cv_image.shape)
    CLASSES = ["background", "aeroplane", "bicycle", "bird", "boat",
               "bottle", "bus", "car", "cat", "chair", "cow", "diningtable",
               "dog", "horse", "motorbike", "person", "pottedplant", "sheep",
               "sofa", "train", "tvmonitor"]
    COLORS = np.random.uniform(0, 255, size=(len(CLASSES), 3))

    for i in np.arange(0, cvOut.shape[2]):
        confidence = cvOut[0, 0, i, 2]
        if confidence > 0.3:
            idx = int(cvOut[0, 0, i, 1])
            box = cvOut[0, 0, i, 3:7] * np.array([960, 540, 960, 540])
            (startX, startY, endX, endY) = box.astype("int")
            label = "{}: {:.2f}%".format(CLASSES[idx], confidence * 100)
            #print("[INFO] {}".format(label))
            cv.rectangle(cv_image, (startX, startY), (endX, endY),COLORS[idx], 2)
            y = startY - 15 if startY - 15 > 15 else startY + 15
            cv.putText(cv_image, label, (startX, y),
            cv.FONT_HERSHEY_SIMPLEX, 0.5, COLORS[idx], 2)


    self.interface.dummy_publisher.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
    now = rospy.get_rostime()-now
    print(now.to_sec())
def reconfigure_callback(self, config, level):
    """
    Called when someone reconfigures this node (or at startup)
    """
    self.interface.from_config(config)
    return config

def __init__(self):
    """
    Sets up ros stuff
    """
    # Initialization
    self.interface = ObjectInterface()
    self.bridge=CvBridge()
    self.cvNet = cv.dnn.readNetFromCaffe("/home/kal3-5/workspaces/my_first_ws/src/tensorflow_ros_tool/src/tensorflow_ros_tool/models/caffe/MobileNetSSD_deploy.prototxt","/home/kal3-5/workspaces/my_first_ws/src/tensorflow_ros_tool/src/tensorflow_ros_tool/models/caffe/MobileNetSSD_deploy.caffemodel")
    # Register callbacks for subscriber and dynamic reconfigure
    self.reconfigure = Server(ObjectConfig, self.reconfigure_callback)
    self.interface.dummy_subscriber.registerCallback ...