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

how to convert a subscriber image into an open cv image?

asked 2019-08-07 14:30:00 -0500

brian77754 gravatar image

I have a program that detects laser points that I know works when I read an image from video 0 but I do not know how to make the program work from a ros subscriber image. I need to know how to convert a ros image subscriber into a usable opencv image named "image" I have researched how to do this and I have come across several solutions that all use the function bridge.imgmsg_to_cv2 but I can not get this to work I am sure it is a simple fix I just don't know what I am doing. This should be relatively simple though. here is my code:

# import the necessary packages
from __future__ import print_function
from imutils import contours
from skimage import measure
import numpy as np
import argparse
import imutils
import cv2
import message_filters
from sensor_msgs.msg import Image, CameraInfo
from std_msgs.msg import Int32, Float32MultiArray
import rospy
from cv_bridge import CvBridge, CvBridgeError


import roslib
roslib.load_manifest('my_package')
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

'''
def getPoint(cameraTip,dotXY,normalPoint):
    slope= (cameraTip[2]-dotXY[2])/(cameraTip[1]-dotXY[1])
    b=cameraTip[2]-(slope*cameraTip[1])
    z=slope*normalPoint[1]+b
    return [normalPoint[0],normalPoint[1],z]
'''
class image_converter:
    def __init__(self):
        self.image_pub = rospy.Publisher("image_topic_2",Image)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("CM_040GE/image_raw",Image,self.callback)

    def callback(self,data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)

        (rows,cols,channels) = cv_image.shape
        if cols > 60 and rows > 60 :
            cv2.circle(cv_image, (50,50), 10, 255)

        cv2.imshow("Image window", cv_image)
        cv2.waitKey(3)

        try:
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
        except CvBridgeError as e:
            print(e)

    def main(args):
        ic = image_converter()
        rospy.init_node('image_converter', anonymous=True)
        try:
            rospy.spin()
        except KeyboardInterrupt:
            print("Shutting down")
            cv2.destroyAllWindows()
        if __name__ == '__main__':
            main(sys.argv)
image = bridge.imgmsg_to_cv2(image_message, desired_encoding="passthrough")
while(1):
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(gray, (11, 11), 0)
        #threshold the image to reveal light regions in the
        # blurred image
    thresh = cv2.threshold(blurred, 30, 255, cv2.THRESH_BINARY)[1]
        # perform a series of erosions and dilations to remove
        # any small blobs of noise from the thresholded image
    thresh = cv2.erode(thresh, None, iterations=2)
    thresh = cv2.dilate(thresh, None, iterations=4)
        # perform a connected component analysis on the thresholded
        # image, then initialize a mask to store only the "large"
        # components
    labels = measure.label(thresh, neighbors=8, background=0)
    mask = np.zeros(thresh.shape, dtype="uint8")

        # loop over the unique components
    for label in np.unique(labels):
            # if this is the background label, ignore it
        if label == 0:
            continue

            # otherwise, construct the label mask and count the
                # number of pixels 
        labelMask = np.zeros(thresh.shape, dtype="uint8")
        labelMask[labels == label] = 255
        numPixels = cv2.countNonZero(labelMask)

                # if the number of pixels in the component is sufficiently
                # large, then add it to our mask of "large blobs"
        if ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2019-08-07 15:01:28 -0500

pavel92 gravatar image

I guess you have followed this tutorial.

The error is comming from line 66:

image = bridge.imgmsg_to_cv2(image_message, desired_encoding="passthrough")

and in your script I cant see where image_message is populated (it is mentioned only here). This line and all of the code below it seems out of place. Looks like some part of the code you posted is missing or there are tab inconsistencies. You already have initialized a subscriber in the __init__ method. My guess is that you want to process the image published on the CM_040GE/image_raw topic. In that case in the binded callback method you already have the conversion to the opencv image:

cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")

where data is the ros image message. You should move your processing (everything from line 66 to the end) into this method or create a separate processing method and call it from within the callback.

edit flag offensive delete link more

Comments

i am having a hard time understanding you response @pavel92 could you just tell me what exact lines to add to my code to make it work and were I should add them?

brian77754 gravatar image brian77754  ( 2019-08-08 08:20:20 -0500 )edit
0

answered 2019-08-07 23:14:37 -0500

NguyenPham gravatar image

updated 2019-08-07 23:16:00 -0500

This is my image callback function. I use it to save subscribe image to jpg image. Hope this help!

rospy.Subscriber("ServerRespImg",CompressedImage, self.Imgcallback,  queue_size = 10)

import numpy as np
import cv2
      def Imgcallback(self, ros_data):
            np_arr = np.fromstring(ros_data.data, np.uint8)
            image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
            outpath = "/home/user/folder/" + name
            #save the image
            cv2.imwrite(outpath, image_np)
edit flag offensive delete link more

Question Tools

Stats

Asked: 2019-08-07 14:30:00 -0500

Seen: 993 times

Last updated: Aug 07 '19