How to send marker positions created by pyhton cv2 script to a ROS node
Dear all,
I have created a python cv2 script that estimates location points (x,y) on the ground plane and would like to send these points generated, to a ROS node that will broadcast them to RViz.
So, I have points in the form of (x,y) tuples and would like to somehow send them to a ROS node.
How would I code such a task? Any directions appreciated!
font = cv2.FONT_HERSHEY_SIMPLEX #Loading neccessary fonts
with open("save.h", "rb") as f:
h = load(f)
print "Homographic matrix loaded successfully."
def draw_circle2(event,x,y,flags,param):
global h,mouseX,mouseY, num_of_points, complete,np_coord_on_screen,np_coord_on_ground, myImage, emptyFrame, coord_on_screen, coord_on_ground
if event == cv2.EVENT_LBUTTONDBLCLK:
a = np.array([[x, y]], dtype='float32')
a = np.array([a])
pointOut = cv2.perspectiveTransform(a, h) #Calculation of new point location
loc_pointOut = tuple(pointOut)
pointOut=(loc_pointOut[0][0][0],loc_pointOut[0][0][1])
print "Current Location: "+str(pointOut)
cv2.imshow('Video',emptyFrame) #Showing emptyFrame
cv2.circle(myImage,(x,y),4,(255,0,0),-1) #Draw a circle on myImage
cv2.putText(myImage,(str((round(pointOut[0],2)))+","+str(round(pointOut[1],2))), (x-5,y-15),font, 0.4,(0,255,0)) #Draw the text
cv2.imshow('Video',myImage) #Showing resulting myImage
myImage = emptyFrame.copy()
# Initial code
# Showing first frame on screen
raw_input("Press any key...")
clear_all()
cv2.namedWindow('Video') #Naming the window to use.
cv2.setMouseCallback('Video',draw_circle2) #Set mouse callBack function.
ret, frame = cap.read() #Get image from camera
if (ret): #If frame has image, show the image on screen
global myImage, emptyFrame
myImage = frame.copy()
emptyFrame = frame.copy()
cv2.imshow('Video',myImage) # Show the image on screen
while True: # making a loop
if cv2.waitKey(1) & 0xFF == ord('q'):
break
if (cv2.waitKey(1) & 0xFF == ord('c')):
#Deleting points from image
#global emptyFrame
#myImage = emptyFrame.copy()
cv2.imshow('Video',emptyFrame) #Show the image again, deleting all graphical overlays like text and shapes
coord_on_screen = [] #Resetting coordinate lists
coord_on_ground=[] #Resetting coordinate lists
if (cv2.waitKey(1) & 0xFF == ord('s')):
cap.release()
cv2.destroyAllWindows()
init()
So, the above code is a part of a 2 step process: 1st Step: Find the homographic matrix that would translate pixel coordinate from a camera to ground plane coordinates. So given pixel coords: 235,178 (on screen) would translate to 2,3 m from a starting point on the ground.
2nd Step: Publish ground coords (2,3) to RViz
So, my problem is how I would create the 2nd step. Having the (2,3) tupple translate it to Marker datatype and transmit it via a node to a topic like /destpoint to RViz.
would you elaborate on your application? are your position estimates realtime? most of the time, you can already use a certain function together with a simple node structure as seen in the ros publisher subscriber tutorial . For sending points to visualize in rviz i do not have any experience. but you could look at how the rviz motion planning scene is used. i think that you would be able to send elements to those topics so you can visualize them
Dear stefvanlierop, thank you very much for your reply.
The positions will be provided by the user and will represent target positions that a robot should head to. So no, they will not be real time. You say that "most of the time, you can already use a certain function together with a simple node structure". Can I create a ROS node in the same physical CV2 script file so, I can get the points and pass them to the node for publishing? I don't think so...
I can create a node as the link you've sent me but how will I inform the node of the point tuples I want to get published?
if you post an example of what you have, i could post an example of what it would look like. Or indeed determine that it would not be a good idea to use it in the way i proposed.
Dear stefvanlierop,
Please check my revised question. I am looking for a way to create a node inside my cv2 script, that would transmit Marker data to RViz under a specific /destpoint topic.
what is the topic you want to publish your (x,y) to? i can use that to make my answer clearer