Run openpose skeleton tracking in Rviz, processing Image topics outside the callback function?

asked 2019-05-16 11:54:31 -0600

kongwu gravatar image

Hi all, I'm trying to get the lightweight version of openpose in ROS to work with Rviz. I use a Intel Realsense d435 camera, but so far only using the /camera/color/image_raw Image topic. By using the Python wrapper for RealsenseSense, I can get the frames in a while loop in main () and publish as a ROS topic, the tracking in real time is pretty good. Or I can use the realsense launch file, and the testing of this tutorial also runs well in real time. Strange thing is when I put the skeleton inference functions in the tutorial script, i.e., in the callback function of the subscriber, I got a delay of about 1-2 seconds delay in Rviz. I've been playing around with the queue_size but doesn't seem to help much. The RGB camera records at 30 FPS. What could possibly be the reason? Is there a way of doing the inference outside the callback? I'm using ROS Kinect on Ubuntu 16.04.

#!/usr/bin/env python
from __future__ import print_function
import time

import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from tf_pose.estimator import TfPoseEstimator
from tf_pose.networks import get_graph_path, model_wh

class image_converter:
    def __init__(self):
        self.e = TfPoseEstimator(get_graph_path('mobilenet_thin'), target_size=(432, 368))
        self.fps_time = 0
        self.bridge = CvBridge()
        self.image_pub = rospy.Publisher("image_topic_2", Image, queue_size=10)
        self.image_sub = rospy.Subscriber("/camera/color/image_raw", Image, self.callback, queue_size=100, tcp_nodelay=True)
    def callback(self, data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
            except CvBridgeError as e:
            print(e)

        rospy.logdebug('image process+')
        humans = self.e.inference(cv_image, resize_to_default=True, upsample_size=2.0)

        rospy.logdebug('postprocess+')
        cv_image = self.e.draw_humans(cv_image, humans, imgcopy=False)
        try:
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
        except CvBridgeError as e:
            print(e)

if __name__ == '__main__':

    rospy.init_node('image_converter', anonymous=True)
    ic = image_converter()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

    cv2.destroyAllWindows()
edit retag flag offensive close merge delete