Пример программы отображения маркеров в 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. Алгоритм реализует следующие шаги:
Получить и сохранить информацию о калибровке камеры.
Подписаться на топик изображений камеры, начать получать кадры. В каждом кадре:
Найти Aruco маркеры и определить их положение в пространстве;
Сгенерировать маркеры в rviz, соответствующие найденным Aruco маркерам;
Раз в 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 реальным координатам маркеров, с помощью калибровки камеры и настройки размера маркера в программе.
Last updated
Was this helpful?