ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

cv_bridge opencv4 running slow

asked 2021-07-14 08:32:00 -0500

subarashi gravatar image

I have been able to built the package by adding this two lines into catkin_ws/src/vision_opencv/cv_bridge/CMakeLists.txt set(PYTHON_NUMPY_INCLUDE_DIR ~/.local/lib/python3.6/site-packages/numpy/core/include) set(PYTHON_INCLUDER_PATH /usr/include/python3.6) But when I run my code it seems that cv_bridge is too slow for video streaming real time. Do you know how can I improve it? this is my code:

#!/usr/bin/env python3
import numpy as np 
import cv2 
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Image
import sys 
from cv_bridge import CvBridge bridge = CvBridge()

def cam_test():
      cap = cv2.VideoCapture(-1) 
      pub = rospy.Publisher('chatter', Image, queue_size=1000)
      rospy.init_node('talker', anonymous=True)  
      rate = rospy.Rate(100) 
      while True:
              ret, img = cap.read()
              gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
              cv2.circle(img,(320,240),15,(0,0,255),2)
      image_message = bridge.cv2_to_imgmsg(img, encoding="passthrough")
      rospy.loginfo(image_message)
      pub.publish(image_message)
      rate.sleep()
      #cv2.imshow('img',img)
      k = cv2.waitKey(30) & 0xff
      if k == 27:
          break

      cap.release()  
      cv2.destroyAllWindows()


if __name__ == '__main__':
       try:
           cam_test()
       except rospy.ROSInterruptException:
           pass
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-07-14 10:34:00 -0500

Ranjit Kathiriya gravatar image

updated 2021-07-15 02:39:21 -0500

Hello subarashi,

For checking the, topic use Rviz: rosrun rviz rviz and subscribe to chatter topic from rviz.

#!/usr/bin/env python3
import numpy as np
import cv2
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Image
import sys
from cv_bridge import CvBridge

bridge = CvBridge()

def cam_test():
      cap = cv2.VideoCapture(-1)
      pub = rospy.Publisher('chatter', Image, queue_size=10)
      rate = rospy.Rate(10)
      while True:
              ret, img = cap.read()
              # gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
              cv2.circle(img,(320,240),15,(0,0,255),2)
              image_message = bridge.cv2_to_imgmsg(img)
              pub.publish(image_message)


if __name__ == '__main__':
       try:
           rospy.init_node('talker', anonymous=True)
           cam_test()
       except rospy.ROSInterruptException:
           pass

Explanation about what was wrong,

  1. You need these lines only if you are printing each frame using imshow.

    k = cv2.waitKey(30) & 0xff if k == 27: break

      cap.release()  
      cv2.destroyAllWindows()
    
  2. This should come in a while loop because of each frame you want to be published. In your code nothing will get publish it code will be stuck in a while loop.

    image_message = bridge.cv2_to_imgmsg(img, encoding="passthrough") pub.publish(image_message)

If you are not clear or have some questions feel free to drop commet.

edit flag offensive delete link more

Comments

Hello! Thank you for your answer. I tried what you wrote but it is still too slow. I even modified the queue size to100 and the rospy.rate to 5, and it seems to be better but still has a delay of 3 seconds

subarashi gravatar image subarashi  ( 2021-07-15 04:34:22 -0500 )edit

Can you please, tell me what hardware and camera are you using?

Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-07-15 04:41:34 -0500 )edit

Jetson TX2 connected to a simple USB webcam.

subarashi gravatar image subarashi  ( 2021-07-15 05:32:52 -0500 )edit

I don't know what is the cause of it. It should work and I have tried on my pc it is working fine.

Try out these steps: might be you can able to fix this.

  1. resize the video frame and publish. link
  2. Try to keep in Gray image so there will be only one channel black and white it can increase the speed.

    gray = cv2.cvtColor(frame, cv.COLOR_BGR2GRAY)
    

I think this could help you, but before that can you please! tell me the shape of the frame which you are getting from your camera as an output.

ret, img = cap.read()
print(img.shape)
Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-07-15 06:02:27 -0500 )edit

Hi. the shape is (480, 640, 3)

subarashi gravatar image subarashi  ( 2021-07-16 00:02:11 -0500 )edit

Yes, I am also having the same shape I did not see any delay in my frame. Can you please add time to your code and tell me how much time it shows to process single frame.

start = time.time()   # After while loop 

print(time.time() - start)  # After publishing line
Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-07-16 02:56:20 -0500 )edit

My Results are:

> rosrun ros_service abc.py 
0.05800032615661621
0.001439809799194336
0.0011591911315917969
0.430736780166626
0.033577680587768555
0.03268933296203613
Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-07-16 02:57:23 -0500 )edit

hello dear @Ranjit sorry for my late reply. Indeed, it works very well if I open rviz in jetson tx2 (ROS_slave) but when I am opening from another computer (ROS_master) there is a latency so I do not know what to do because the jetson tx2 is in a robot so I need to see what the robot is seeing...

subarashi gravatar image subarashi  ( 2021-10-10 05:23:01 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2021-07-14 08:32:00 -0500

Seen: 515 times

Last updated: Jul 15 '21