How to send marker positions created by pyhton cv2 script to a ROS node

asked 2020-02-26 08:32:54 -0500

John999991 gravatar image

updated 2020-02-27 15:57:49 -0500

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)

            print "Current Location: "+str(pointOut)
            cv2.imshow('Video',emptyFrame) #Showing emptyFrame
  ,(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...")
    cv2.namedWindow('Video') #Naming the window to use.
    cv2.setMouseCallback('Video',draw_circle2) #Set mouse callBack function.
    ret, frame = #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'):
        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')):

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.

edit retag flag offensive close merge delete


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

stefvanlierop gravatar image stefvanlierop  ( 2020-02-26 08:43:05 -0500 )edit

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?

John999991 gravatar image John999991  ( 2020-02-26 09:11:08 -0500 )edit

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.

stefvanlierop gravatar image stefvanlierop  ( 2020-02-27 02:49:30 -0500 )edit

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.

John999991 gravatar image John999991  ( 2020-02-27 15:55:04 -0500 )edit

what is the topic you want to publish your (x,y) to? i can use that to make my answer clearer

stefvanlierop gravatar image stefvanlierop  ( 2020-03-02 06:36:45 -0500 )edit