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

OpenCV and ROS melodic using python3

asked 2022-05-16 05:08:23 -0500

FarmRobo gravatar image

Hi,

I would like to subscribe to the camera topic of my robot and use this images in openCV. Unfortunatly, I need to use Python3 in melodic because of the robot, so using cv_bridge seems not to work. I found the following sourcecode as solution at another question, adapted it to my problem, but won't get it working.

Is someone keen to help me or is there another solution for this problem?

#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import Image
import cv2 as cv
import numpy as np
from geometry_msgs.msg import Twist
import sys

def callback():
    pass

def imgmsg_to_cv2(img_msg):
    img_msg = Image()
    if img_msg.encoding != "bgr8":
        rospy.logerr("This Coral detect node has been hardcoded to the 'bgr8' encoding.  Come change the code if you're actually trying to implement a new camera")
    dtype = np.dtype("uint8") # Hardcode to 8 bits...
    dtype = dtype.newbyteorder('>' if img_msg.is_bigendian else '<')
    image_opencv = np.ndarray(shape=(img_msg.height, img_msg.width, 3), # and three channels of data. Since OpenCV works with bgr natively, we don't need to reorder the channels.
                    dtype=dtype, buffer=img_msg.data)
    # If the byt order is different between the message and the system.
    if img_msg.is_bigendian == (sys.byteorder == 'little'):
        image_opencv = image_opencv.byteswap().newbyteorder()
    cv.imshow("Video", image_opencv)
    return image_opencv

cv_image = rospy.Subscriber("/robot/front_rgbd_camera/rgb/image_raw", Image, callback )

if __name__ =="__main__":
    imgmsg_to_cv2(cv_image)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2022-05-16 05:44:05 -0500

Dhara gravatar image

updated 2022-05-17 05:13:43 -0500

Hi,

you can use numpy instaed of cv_bridge. Get the data from callback, reshape and normalize it. That's how you can use python3 with melodic. Below is the example:

#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import Image
import cv2 as cv
import numpy as np
from geometry_msgs.msg import Twist
import sys

def callback(data):
    image_a = np.frombuffer(data.data, dtype=np.uint16).reshape(data.height,data.width,-1)
    image = cv.normalize(image_a, None, 0, 255, cv2.NORM_MINMAX, cv2.CV_8U)
    cv.imshow("opencv ros image", image)

if __name__ =="__main__":
    cv_image = rospy.Subscriber("/robot/front_rgbd_camera/rgb/image_raw", Image, callback )

Play with the dtype according to your image type. Hope it works!

edit flag offensive delete link more

Comments

I tried your Idea, but will get an error in in callback image_a = np.frombuffer(data, dtype=np.uint32).reshape(data.height,data.width)

TypeError: a bytes-like object is required, not 'Image'

FarmRobo gravatar image FarmRobo  ( 2022-05-16 07:03:30 -0500 )edit

try this : np.frombuffer(data.data, dtype=np.int32).reshape(data.height,data.width)

Dhara gravatar image Dhara  ( 2022-05-16 07:38:21 -0500 )edit

now I get an ValueError: cannot reshape array of size 230400 into shape (480,640)

FarmRobo gravatar image FarmRobo  ( 2022-05-16 07:56:16 -0500 )edit

You have to input your image type in ‘dtype’. The error you got states the same that your image topic is not of type int32.

Dhara gravatar image Dhara  ( 2022-05-16 09:02:55 -0500 )edit

I tried different types, but there was a -1 in reshape missing. Now its working, Thank you very much!

image_a = np.frombuffer(data.data, dtype=np.uint8).reshape(data.height,data.width,-1)
FarmRobo gravatar image FarmRobo  ( 2022-05-16 13:41:23 -0500 )edit

perfect. Then can you please accept my answer?

Dhara gravatar image Dhara  ( 2022-05-17 05:09:53 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2022-05-16 05:08:23 -0500

Seen: 730 times

Last updated: May 17 '22