> For the complete documentation index, see [llms.txt](https://copter-space.gitbook.io/uchebnik-mashinnoe-zrenie-tom-2/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://copter-space.gitbook.io/uchebnik-mashinnoe-zrenie-tom-2/razdel-4./primer-programmy-otobrazheniya-markerov-v-rviz.md). # Пример программы отображения маркеров в 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 можно наблюдать меняющийся во времени набор маркеров: ![](/files/-M28dLI4x0ID8LrVcGrO) ## Пример программы отображения положения карты маркеров в пространстве с использованием 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 (). Данный модуль предназначен специально для конвертации изображений между ROS сообщениями и OpenCV: ![](/files/-M28eE1xaiwllL0Hbpq1) В глобальной переменной 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 началу координат # поэтому матрица = 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 по своим координатам: ![](/files/-M28fU9zXdAmK7-dJTs6) С помощью рулетки можно проверить соответствие расстояний, отображаемых в rviz расстояниям до маркеров в реальном мире. Нужно добиться максимального соответствия картины в rviz реальным координатам маркеров, с помощью калибровки камеры и настройки размера маркера в программе. --- # Agent Instructions This documentation is published with GitBook. GitBook is the documentation platform designed so that both humans and AI agents can read, navigate, and reason over technical content effectively. Learn more at gitbook.com. ## Querying This Documentation If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question. Perform an HTTP GET request on the current page URL with the `ask` query parameter: ``` GET https://copter-space.gitbook.io/uchebnik-mashinnoe-zrenie-tom-2/razdel-4./primer-programmy-otobrazheniya-markerov-v-rviz.md?ask= ``` The question should be specific, self-contained, and written in natural language. The response will contain a direct answer to the question and relevant excerpts and sources from the documentation. Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.