Задача №1. Вычисление расстояния до объекта при известных габаритах
В задаче реализовано распознание красного объекта по RGB картинке и дальнейший расчет расстояния. В качестве предмета используем красный шар с заранее известным диаметром - 40 мм.
Основная функция данного кода:
Слушать изображение и параметры камеры.
Фильтровать по цвету.
Находить объект по самому большому контуру.
Оценивать расстояние на основе площади этого объекта.
Выводить расстояние в консоль.
После этого в терминале появятся данные об расстоянии до красного объекта, который камера распознала в кадре. Необходимо учитывать, что техническое зрение очень капризно к освещению, что требует индивидуального подбора коэффициентов для стабильной работы системы.
Разберем код детальнее, его можно найти по пути: /catkin_ws/src/applied_robotics-trackingcam3d_client_ros/src/detector_blob.py
Основные библиотеки:
import rospy, cv2, numpy as np, message_filters
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge
from visualization_msgs.msg import Markerrospy — ROS API для Python.
cv2 и numpy — для обработки изображений.
cv_bridge — преобразует изображения между ROS и OpenCV.
message_filters — синхронизация нескольких топиков по времени.
Класс Get_distance_from_camera:
Подписывается на RGB-камеру и данные камеры (camera_info),
Обрабатывает изображения,
Находит самый большой контур определённого цвета (в данном случае — оттенки красного/оранжевого),
Вычисляет радиус минимальной окружности вокруг объекта,
По площади окружности приблизительно оценивает расстояние до объекта.
self.camera_info_sub = Subscriber('/.../camera_info', CameraInfo)
self.image_sub = Subscriber('/.../image_raw', Image)Подписка на цветное изображение и параметры калибровки камеры.
self.ts = message_filters.ApproximateTimeSynchronizer([self.image_sub,self.camera_info_sub], queue_size=10, slop=0.5) -
Используется ApproximateTimeSynchronizer, чтобы обрабатывать кадры, пришедшие примерно в одно время.
callback — основная логика:
- Извлечение изображения и информации о камере:
camera_info_K = np.array(camera_info.K)
cv_rgb = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
- Обработка изображения:
Уменьшение шума: GaussianBlur
Преобразование в HSV
Фильтрация по цвету
Эрозия — удаляет мелкие шумы
Контуры — поиск объектов
bin_cv_rgb = cv2.inRange(hsv_cv_rgb, (0, 50, 50), (10, 255, 255))
contours, \_ = cv2.findContours(...)- Окружность вокруг контура:
(x, y), radius = cv2.minEnclosingCircle(contours[0])- Вычисляется радиус минимальной окружности вокруг самого большого найденного контура.
- Расчёт расстояния (эмпирически):
В зависимости от площади используется эмпирически подобранный коэффициент k. Расстояние считается так:
Для запуска кейса необходимо открыть новый терминал и ввести ряд команд для начала работы:
pi@raspberrypi:~ $ docker exec -it \<CONTAINER ID\> /bin/bash
cd
cd catkin_ws/
В случае arm64:
source devel_isolated/setup.bash
В случае amd64:
source devel/setup.bash
Запуск кейса:
rosrun trackingcam3d_client_ros detector_blob.py
Добавим дополнительный функционал в данный код. Реализуем возможность отрисовки зеленого маркера в RViz, при детектировании объекта. Напомним, что RViz лучше запускать на мощных устройствах. Разберем детальнее добавленный код.
Импортируем типы сообщений для публикации:
from visualization_msgs.msg import Marker, MarkerArrayПозволяет создавать и публиковать маркеры (например, сферу в RViz) с помощью этих типов сообщений:
Marker — один маркер (сфера, стрелка, текст и т. д.)
MarkerArray — массив маркеров
from geometry_msgs.msg import PoseНужно для описания позиции и ориентации объекта
self.marker_array_msg = MarkerArray()Создаётся экземпляр MarkerArray — это ROS-сообщение, которое содержит список (массив) маркеров (Marker[] markers).
В него можно положить один или несколько Marker, и RViz отобразит их как графические объекты (например, сферы, стрелки, кубы и т.д.).
self.marker_array_msg.markers.append(Marker())Данная строка добавляет в массив один пустой Marker, чтобы его потом легко переиспользовать (например, заменять на новый при обнаружении объекта).
Предварительное выделение "слота" — удобно, если ты хочешь обращаться к self.marker_array_msg.markers[0], не проверяя каждый раз, пустой ли список.
self.pub_marker = rospy.Publisher('/trackingcam3d_client_ros/recognition_marker_array',MarkerArray,queue_size=1) - объявление паблишера, то есть публикация сообщения.
После этого можно в любом месте кода вызывать:
self.pub_marker.publish(self.marker_array_msg)И если RViz подписан на этот топик (MarkerArray), он отобразит маркеры.
Далее удаляем комментарий к коду:
m_fx = camera_info.K[0]
m_fy = camera_info.K[4]
m_cx = camera_info.K[2]
m_cy = camera_info.K[5]
inv_fx = 1.0 / m_fx
inv_fy = 1.0 / m_fyЭти данные необходимы для дальнейшего преобразования координат в 2D картинку.
point_z = dist
point_x = (x - m_cx) * point_z * inv_fx
point_y = (y - m_cy) * point_z * inv_fyСледующим шагом создаем макркер
diam = 2.0 * radius * point_z / m_fx
marker = Marker()
marker.header.frame_id = camera_info.header.frame_id
marker.header.stamp = rospy.Time.now()
marker.ns = "Goal"
marker.id = 0
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.scale.x = diam
marker.scale.y = diam
marker.scale.z = diam
marker.color.r = 0.2
marker.color.g = 1.0
marker.color.b = 0.2
marker.color.a = 0.8
marker.pose = Pose()
marker.pose.position.x = float(point_x)
marker.pose.position.y = float(point_y)
marker.pose.position.z = float(point_z)Закидываем массив и публикуем в RViz
self.marker_array_msg.markers\[0\] = marker
self.pub_marker.publish(self.marker_array_msg)Реализуем визуализацию круга на изображении
cv2.circle(cv_rgb, (int(x), int(y)), int(radius),(0, 255, 0), 2)В случае, когда объект исчез, маркер необходимо удалить:
else:
empty = Marker(action=Marker.DELETEALL)
self.marker_array_msg.markers[0] = empty
self.pub_marker.publish(self.marker_array_msg)Полный скрипт кейса:
py
#!/usr/bin/env python3
from __future__ import print_function
import sys
import math
import rospy
import cv2
import numpy as np
import message_filters
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
# ---------- RViz / 3D ----------
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Pose
class Get_distance_from_camera:
def __init__(self):
self.bridge = CvBridge()
# ---------- Subscriptions ----------
self.camera_info_sub = message_filters.Subscriber(
'/trackingcam3d_client_ros/trackingcam3d0/left/camera_info',CameraInfo)
self.image_sub = message_filters.Subscriber(
'/trackingcam3d_client_ros/trackingcam3d0/left/image_raw', Image)
# Synchronize RGB + CameraInfo
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.image_sub, self.camera_info_sub], queue_size=10, slop=0.5)
self.ts.registerCallback(self.callback)
# ---------- Publications ----------
self.pub_img = rospy.Publisher(
'/trackingcam3d_client_ros/recognition_object',
Image, queue_size=1)
self.marker_array_msg = MarkerArray()
self.marker_array_msg.markers.append(Marker()) # slot for one object
self.pub_marker = rospy.Publisher(
'/trackingcam3d_client_ros/recognition_marker_array',
MarkerArray, queue_size=1)
def callback(self, rgb_data, camera_info):
try:
# ---------- Camera parameters ----------
m_fx = camera_info.K[0]
m_fy = camera_info.K[4]
m_cx = camera_info.K[2]
m_cy = camera_info.K[5]
inv_fx = 1.0 / m_fx
inv_fy = 1.0 / m_fy
# ---------- RGB frame ----------
cv_rgb = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
cv_rgb = cv2.resize(cv_rgb, (640, 480))
y_size, x_size = cv_rgb.shape[:2]
x_offset, y_offset = x_size / 2.0, y_size / 2.0
# ---------- Detect red circle ----------
cv_rgb_blur = cv2.GaussianBlur(cv_rgb, (5, 5), 0)
hsv = cv2.cvtColor(cv_rgb_blur, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, (0, 50, 50), (10, 255, 255))
mask = cv2.erode(mask, None, iterations=3)
contours, _ = cv2.findContours(
mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
contours = sorted(contours, key=cv2.contourArea,reverse=True)
# ---------- If object found ----------
if contours:
(x, y), radius = cv2.minEnclosingCircle(contours[0])
x = np.clip(x, 0, x_size)
y = np.clip(y, 0, y_size)
# ---------- Estimate distance from area ----------
area = math.pi * radius ** 2
if area > 16500:
k = 165000
elif area > 6500:
k = 42462
elif area > 4900:
k = 24450
elif area > 3100:
k = 12462
else:
k = 5800
dist = area / k # meters (rough estimate)
print(f"dist: {dist:.2f} m")
# ---------- 2D → 3D conversion ----------
point_z = dist
point_x = (x - m_cx) * point_z * inv_fx
point_y = (y - m_cy) * point_z * inv_fy
# ---------- Create sphere marker ----------
diam = 2.0 * radius * point_z / m_fx # diameter in meters
marker = Marker()
marker.header.frame_id = camera_info.header.frame_id
marker.header.stamp = rospy.Time.now()
marker.ns = "Goal"
marker.id = 0
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.scale.x = diam
marker.scale.y = diam
marker.scale.z = diam
marker.color.r = 0.2
marker.color.g = 1.0
marker.color.b = 0.2
marker.color.a = 0.8
marker.pose = Pose()
marker.pose.position.x = float(point_x)
marker.pose.position.y = float(point_y)
marker.pose.position.z = float(point_z)
# Put in array and publish
self.marker_array_msg.markers[0] = marker
self.pub_marker.publish(self.marker_array_msg)
# ---------- Visualization in image ----------
cv2.circle(cv_rgb, (int(x), int(y)), int(radius),(0, 255, 0), 2)
else:
# If object disappeared, delete marker
empty = Marker(action=Marker.DELETEALL)
self.marker_array_msg.markers[0] = empty
self.pub_marker.publish(self.marker_array_msg)
# ---------- Publish processed image ----------
img_msg = self.bridge.cv2_to_imgmsg(cv_rgb, "bgr8")
self.pub_img.publish(img_msg)
except CvBridgeError as e:
rospy.logerr(e)
def main(args):
rospy.init_node('unibas_face_distance_calculator', anonymous=True)
Get_distance_from_camera()
rospy.spin()
if __name__ == '__main__':
main(sys.argv)Заменим предыдущий код кейса на новый скрипт. Для начала создаём новый файл «detect_blob.py» и вставим новый код в данный файл.
cd ~/catkin_ws/src/applied_robotics-trackingcam3d_client_ros/src/
touch detect_blob.py && nano detect_blob.py
Откроется текстовой редактор, где вставим новый код. Сохраняем файл и выходим (Ctrl+O, Enter, Ctrl+X). Теперь заменяем содержание файла «detector_blob.py» на новый код в файле «detect_blob.py»:
mv detect_blob.py detector_blob.py
chmod +x detector_blob.py
cd ~
Пересоберём пакеты, как было показано ранее в разделе про подготовку пространства и настройку среды ROS Noetic.
Также, обратим внимание на ввод команды для загрузки графических интерфейсов при работе на ОС Windows:
root@docker-desktop:~# export DISPLAY=host.docker.internal:0.0
Для запуска в первом терминале пропишем:
roslaunch trackingcam3d_client_ros trackingcam3d_client_ros.launch
Во втором терминале после настройки пространства пропишем команду для первого кейса:
rosrun trackingcam3d_client_ros detector_blob.py
После запуска проверим, в RViz в MarkerArray>MarkerTopic выбрано /trackingcam3d_client_ros/visualization_marker_array(рис.27)
Рис.27 Детектирование красного шарика в RViz