Отправка сообщений ROS через скрипт cv2 python - PullRequest
Новая
       24

Отправка сообщений ROS через скрипт cv2 python

0 голосов
/ 28 февраля 2020

Я пытаюсь реализовать решение roboti c, в котором пользователь щелкает точку через канал камеры, а мобильный четырехколесный робот направляется к этому месту. Я создал часть преобразования координат видеопикселя в наземные координаты с помощью преобразования гомографа и хотел бы реализовать часть отправки наземных координат в RViz. Часть вычисления наземных координат показана ниже: 

global h
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]) #Point on ground

        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

        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()


# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()

Итак, pointOut - это точка на земле (пример: (2,4) m). Мне нужно несколько указаний о том, как реализовать создание узел и передача сообщения PointOut Marker в ROS topi c в моем коде.

Моя конфигурация - ROS Kineti c Kame, Python 2.7

...