how to convert a subscriber image into an open cv 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.imgmsgtocv2 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 numPixels > 300:
mask = cv2.add(mask, labelMask)
# find the contours in the mask, then sort them from left to
# right
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
cnts = contours.sort_contours(cnts)[0]
# loop over the contours
for (i, c) in enumerate(cnts):
# draw the bright spot on the image
(x, y, w, h) = cv2.boundingRect(c)
((cX, cY), radius) = cv2.minEnclosingCircle(c)
#x and y center are cX and cY
cv2.circle(image, (int(cX), int(cY)), int(radius),
(0, 0, 255), 3)
cv2.putText(image, "#{}".format(i + 1), (x, y - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 255), 2)
# show the output image
cv2.imshow("Image", image)
cv2.waitKey(1)
#camera.release()
I have tried doing this several different ways following several different websites and the error that I get each time is:
Traceback (most recent call last):
File "lazerSub.py", line 66, in
Asked by brian77754 on 2019-08-07 14:30:00 UTC
Answers
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.
Asked by pavel92 on 2019-08-07 15:01:28 UTC
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?
Asked by brian77754 on 2019-08-08 08:20:20 UTC
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)
Asked by NguyenPham on 2019-08-07 23:14:37 UTC
Comments