Пример программы отображения маркеров в RVIZ

Рассмотрим простую программу публикации маркеров в пространстве виртуального мира.

Программа в цикле создаёт набор из 100 маркеров, которые расположены в трёхмерном пространстве по синусоиде «змейкой». Ниже приведён текст программы:

# coding=UTF-8
#!/usr/bin/env python

from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
import rospy
import math

topic = 'visualization_marker_array'
publisher = rospy.Publisher(topic, MarkerArray)

rospy.init_node('register')

markerArray = MarkerArray()

count = 0
MARKERS_MAX = 100

while not rospy.is_shutdown():
   marker = Marker()
   marker.header.frame_id = "/neck"
   marker.type = marker.SPHERE
   marker.action = marker.ADD
   marker.scale.x = 0.2
   marker.scale.y = 0.2
   marker.scale.z = 0.2
   marker.color.a = 1.0
   marker.color.r = 1.0
   marker.color.g = 1.0
   marker.color.b = 0.0
   marker.pose.orientation.w = 1.0
   marker.pose.position.x = math.cos(count / 50.0)
   marker.pose.position.y = math.cos(count / 40.0) 
   marker.pose.position.z = math.cos(count / 30.0) 

   # Добавляем маркер в MarkerArray, самый старый - удаляем
   if(count > MARKERS_MAX):
       markerArray.markers.pop(0)
   markerArray.markers.append(marker)

   # Перенумеруем ID маркеров
   id = 0
   for m in markerArray.markers:
       m.id = id
       id += 1

   # Публикуем массив маркеров
   publisher.publish(markerArray)
   count += 1
   rospy.sleep(0.01)

В результате выполнения программы в rviz можно наблюдать меняющийся во времени набор маркеров:

Пример программы отображения положения карты маркеров в пространстве с использованием RVIZ

В предыдущих главах мы отдельно рассмотрели механизмы получения видеопотока с камеры через ROS, поиска и оценки положения Aruco маркера в пространстве, а также механизм визуализации объектов в 3D пространстве с помощью rviz.

Следующим промежуточным этапом разработки системы автономной навигации является объединение этих шагов, полный код программы приведён в файле zuza_marker.py. Алгоритм реализует следующие шаги:

1. Получить и сохранить информацию о калибровке камеры.

2. Подписаться на топик изображений камеры, начать получать кадры. В каждом кадре:

   1. Найти Aruco маркеры и определить их положение в пространстве;

   2. Сгенерировать маркеры в rviz, соответствующие найденным Aruco маркерам;

   3. Раз в 5 секунд посчитать реальную частоту обработки изображений, и опубликовать её вместе с нарисованными маркерами в отладочном видеопотоке.

Ниже мы рассмотрим этот алгоритм по отдельным процедурам. @page { size: 21cm 29.7cm; margin: 2cm } p { margin-bottom: 0.25cm; direction: ltr; line-height: 115%; text-align: left; orphans: 2; widows: 2; background: transparent } a:visited { color: #954f72; text-decoration: underline } a:link { color: #0000ff; text-decoration: underline }

Получение и сохранение данных калибровки камеры

Данные калибровки камеры сохраним в глобальных переменных нашей программы: mtx – матрица камеры, dist – коэффициенты искажения. В ROS Эти данные камера публикует в топик camera_info. Можно получать их при каждой обработке кадра, но это лишняя трата производительности процессора. Поэтому мы используем процедуру, которая подписывается на топик /main_camera/camera_info, ожидает, пока обработчик сохранит значение информации камеры, после этого переводит полученные данные в numpy-массивы, которые можно затем использовать в функциях OpenCV. Ниже приведён код этих процедур, а также код, импортирующий нужные нам модули:

#!/usr/bin/env python
# coding=UTF-8
import rospy
from sensor_msgs.msg import CameraInfo, Image
import numpy as np
import time
import cv2
from cv_bridge import CvBridge, CvBridgeError
from visualization_msgs.msg import Marker, MarkerArray

import tf.transformations as t
from geometry_msgs.msg import Quaternion
import copy

my_fps=0
my_last_fps=0
my_time_fps=time.time()
bridge = CvBridge()
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_1000)
markerLength = 0.165

my_cam_info=None
mtx=None
dist=None

def callback_cinfo(cinfo):
    global my_cam_info
    my_cam_info=cinfo

def process_camera_info():
    global mtx, dist
    my_sub = rospy.Subscriber('/main_camera/camera_info', CameraInfo, callback_cinfo)
    while my_cam_info==None:
        rospy.sleep(0.01)
    my_sub.unregister()
    mtx=np.zeros((3,3))
    dist=np.zeros((1,5))
    for i in range(3):
      for j in range(3):
        mtx[i,j]=my_cam_info.K[i*3+j]
    for i in range(5):
      dist[0,i]=my_cam_info.D[i]

Обработчик сообщения изображения кадра

Процедура-обработчик сообщения кадра ROS получает изображение в виде сообщения типа Image модуля sensor_msgs.msg. Для преобразования этого сообщения в массив, который можно обработать с помощью OpenCV используется модуль ROS cv_bridge (http://wiki.ros.org/cv_bridge). Данный модуль предназначен специально для конвертации изображений между ROS сообщениями и OpenCV:

В глобальной переменной my_fps программа считает количество кадров, обработанных процедурой за последние 5 секунд. Вывод значения fps на изображение осуществляется с помощью функции cv2.putText.

Поиск и отображение маркеров на изображении производится с помощью знакомых нам процедур detectMarkers, drawDetectedMarkers.

Определение положения маркеров в пространстве относительно оптического центра камеры – процедура estimatePoseSingleMarkers. В неё кроме координат углов и размеров маркера передаются оптические параметры камеры. Далее для каждого распознанного на изображении маркера создаётся маркер визуализации в rviz. Углы вращения функция OpenCV estimatePoseSingleMarkers возвращает в виде вектора вращения. А в системе ROS поворот объекта (в т.ч. и маркера) задаётся в виде Кватерниона. Для преобразования поворота из вектора вращения в кватернион мы сначала получаем матрицу вращения с помощью функции cv2.Rodrigues, а затем матрицу вращения преобразуем в кватернион с помощью функции quaternion_from_matrix модуля tf. transformations. При этом функция quaternion_from_matrix требует матрицу вращения размером 4 х 4 элемента, т.к. она может преобразовывать вращение в т.ч. относительно точки, отличной от начала координат. Но из OpenCV мы получаем вектор вращения относительно оптического центра камеры (начала координат), поэтому полученную в cv2.Rodrigues матрицу 3 х 3 мы накладываем на единичную диагональную матрицу 4 х 4.

Ниже приведён полный код процедуры:

def callback_image_raw(image_raw):
    global my_fps, my_last_fps, my_time_fps, dictionary, mtx, dist 
    try:
      cv_image = bridge.imgmsg_to_cv2(image_raw, "bgr8")
    except CvBridgeError as e:
      print(e)
      
    # calculate FPS
    cur_time2 = time.time()
    if cur_time2-my_time_fps>5:
        my_last_fps=my_fps
        my_fps=1
        my_time_fps=cur_time2
    else:
        my_fps+=1
    
    #Detect markers 
    corners, ids, rejected = cv2.aruco.detectMarkers(cv_image, dictionary)
    if len(corners)>0:
        cv2.aruco.drawDetectedMarkers(cv_image, corners, ids)
        rvecs,tvecs, _ObjPoints = cv2.aruco.estimatePoseSingleMarkers(corners, markerLength, mtx, dist)
        i=0
        markerArray = MarkerArray()
        Duration1sec = rospy.Duration(1)
        while i<len(rvecs):
          cv2.aruco.drawAxis(cv_image, mtx, dist, rvecs[i], tvecs[i], markerLength)
          ######################## begin marker publishing ##############
          marker = Marker()
          marker.id = i
          marker.lifetime = Duration1sec
          marker.header.frame_id = "/local_origin"
          marker.type = marker.CUBE
          marker.action = marker.ADD
          marker.scale.x = markerLength
          marker.scale.y = markerLength*1.2
          marker.scale.z = 0.02
          marker.color.a = 1.0
          marker.color.r = 0
          marker.color.g = 1
          marker.color.b = 0
          #преобразуем вектор вращения в матрицу вращения
          # quaternion_from_matrix понимает и вращение относительно точки <> началу координат
          # поэтому матрица = 4х4
          mat4 = np.identity(4)
          mat4[:3, :3], jacob = cv2.Rodrigues(rvecs[i][0])
          q = t.quaternion_from_matrix(mat4)
          marker.pose.orientation = Quaternion(*q)
          
          marker.pose.position.x = tvecs[i][0][0] 
          marker.pose.position.y = tvecs[i][0][1] 
          marker.pose.position.z = tvecs[i][0][2] 
          markerArray.markers.append(marker)
          
          marker2 = copy.deepcopy(marker)
          marker2.id = i+1
          marker2.type = marker.ARROW
          marker2.scale.x = markerLength*1.5
          marker2.scale.y = markerLength/10
          marker2.color.r = 1
          marker2.color.g = 0
          marker2.color.b = 0
          markerArray.markers.append(marker2)
          
          ######################## end marker publishing ##############  
          i+=2
        # Publish the MarkerArray
        publisher_mrk.publish(markerArray)
    #Draw FPS on image
    cv2.putText(cv_image,"fps: "+str(my_last_fps/5),(10,30), cv2.FONT_HERSHEY_SIMPLEX, 1,(0,255,0),2,cv2.LINE_AA)
    
    try:
      publisher_img.publish(bridge.cv2_to_imgmsg(cv_image, "bgr8"))
    except CvBridgeError as e:
      print(e)

Основная программа

Код основной программы инициализирует ROS-ноду zuza_vpe, запоминает параметры камеры (process_camera_info), инициализирует публикаторы изображения отладки (publisher_img) и маркеров rviz (publisher_mrk), а также подписку на кадры с камеры (my_sub). Дальнейшая обработка кадра, полученного с камеры, осуществляется в процедуре callback_image_raw.

Ниже приведён текст основной программы:

if __name__ == '__main__':
    rospy.init_node('zuza_pose')
    process_camera_info()
    print my_cam_info
    print "image translation starting........."
    publisher_img = rospy.Publisher('zuza_pose/aruco_detector', Image, queue_size=2)
    publisher_mrk = rospy.Publisher('zuza_pose/aruco_map_marker', MarkerArray, queue_size=2)
    my_sub = rospy.Subscriber('/main_camera/image_raw', Image, callback_image_raw)
    try:
      rospy.spin()
    except KeyboardInterrupt:
      print("Shutting down")
    my_sub.unregister()

Перед тем, как запускать программу, нам нужно запустить roscore и ноду, транслирующую изображения (roslaunch zuza zuza.launch), в отдельных сессиях терминала. Способ автоматизации запуска программ будет рассмотрен с следующем разделе. Детектор маркеров запускается на выполнение командой python zuza_marker.py.

После запуска детектора маркеров, результаты его работы удобно проверять с помощью rviz. При появлении на изображении камеры Aruco-маркеров – распознанные маркеры отображаются в rviz по своим координатам:

С помощью рулетки можно проверить соответствие расстояний, отображаемых в rviz расстояниям до маркеров в реальном мире. Нужно добиться максимального соответствия картины в rviz реальным координатам маркеров, с помощью калибровки камеры и настройки размера маркера в программе.