Skip to content

Глава 3. Практическая часть ​

Задача №1. Вычисление расстояния до объекта при известных габаритах. ​

В задаче реализовано распознание красного объекта по RGB картинке и дальнейший расчет расстояния. В качестве предмета используем красный шар с заранее известным диаметром - 40 мм.

Основная функция данного кода:

  • Слушать изображение и параметры камеры.

  • Фильтровать по цвету.

  • Находить объект по самому большому контуру.

  • Оценивать расстояние на основе площади этого объекта.

  • Выводить расстояние в консоль.

После этого в терминале появятся данные об расстоянии до красного объекта, который камера распознала в кадре. Необходимо учитывать, что техническое зрение очень капризно к освещению, что требует индивидуального подбора коэффициентов для стабильной работы системы.

Разберем код детальнее, его можно найти по пути:

bash
/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 Marker

rospy — ROS API для Python.

cv2 и numpy — для обработки изображений.

cv_bridge — преобразует изображения между ROS и OpenCV.

message_filters — синхронизация нескольких топиков по времени.

Класс Get_distance_from_camera:

  • Подписывается на RGB-камеру и данные камеры (camera_info);

  • Обрабатывает изображения;

  • Находит самый большой контур определённого цвета (в данном случае — оттенки красного/оранжевого);

  • Вычисляет радиус минимальной окружности вокруг объекта;

  • По площади окружности приблизительно оценивает расстояние до объекта.

bash
self.camera_info_sub = Subscriber('/.../camera_info', CameraInfo)
self.image_sub = Subscriber('/.../image_raw', Image)

Подписка на цветное изображение и параметры калибровки камеры.

bash
self.ts = message_filters.ApproximateTimeSynchronizer([self.image_sub, self.camera_info_sub], queue_size=10, slop=0.5) -

Используется ApproximateTimeSynchronizer, чтобы обрабатывать кадры, пришедшие примерно в одно время.

callback — основная логика:

  1. Извлечение изображения и информации о камере:
bash
camera_info_K = np.array(camera_info.K)
cv_rgb = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
  1. Обработка изображения:
  • Уменьшение шума: GaussianBlur

  • Преобразование в HSV

  • Фильтрация по цвету

  • Эрозия — удаляет мелкие шумы

  • Контуры — поиск объектов

  • bin_cv_rgb = cv2.inRange(hsv_cv_rgb, (0, 50, 50), (10, 255, 255))

  • contours, _ = cv2.findContours(...)

  1. Окружность вокруг контура:

(x, y), radius = cv2.minEnclosingCircle(contours[0]) - Вычисляется радиус минимальной окружности вокруг самого большого найденного контура.

  1. Расчёт расстояния (эмпирически):
bash
area_circle = π * radius²

В зависимости от площади используется эмпирически подобранный коэффициент k. Расстояние считается так:

dist = area_circle / k - чем меньше объект на изображении, тем меньше площадь, тем дальше объект. Это грубая модель, но подходит, если известен размер объекта и расстояние к нему в разных условиях.

Для запуска кейса необходимо открыть новый терминал и ввести ряд команд для начала работы:

bash
pi@raspberrypi:~ $ docker exec -it <CONTAINER ID> /bin/bash
>cd
>cd catkin_ws/

В случае arm64:

bash
>source devel_isolated/setup.bash

В случае amd64:

bash
>source devel/setup.bash

Запуск кейса:

bash
>rosrun trackingcam3d_client_ros detector_blob.py

Добавим дополнительный функционал в данный код. Реализуем возможность отрисовки зеленого маркера в RViz, при детектировании объекта. Напомним, что RViz лучше запускать на мощных устройствах. Разберем детальнее добавленный код.

Импортируем типы сообщений для публикации:

python
from visualization_msgs.msg import Marker, MarkerArray

Позволяет создавать и публиковать маркеры (например, сферу в RViz) с помощью этих типов сообщений:

Marker — один маркер (сфера, стрелка, текст и т. д.)

MarkerArray — массив маркеров

python
from geometry_msgs.msg import Pose

Нужно для описания позиции и ориентации объекта

python
self.marker_array_msg = MarkerArray()

Создаётся экземпляр MarkerArray — это ROS-сообщение, которое содержит список (массив) маркеров (Marker[] markers).

В него можно положить один или несколько Marker, и RViz отобразит их как графические объекты (например, сферы, стрелки, кубы и т.д.).

python
self.marker_array_msg.markers.append(Marker())

Данная строка добавляет в массив один пустой Marker, чтобы его потом легко переиспользовать (например, заменять на новый при обнаружении объекта).

Предварительное выделение "слота" — удобно, если ты хочешь обращаться к self.marker_array_msg.markers[0], не проверяя каждый раз, пустой ли список.

python
self.pub_marker = rospy.Publisher('/trackingcam3d_client_ros/

recognition_marker_array',MarkerArray, queue_size=1) - объявление паблишера, то есть публикация сообщения.

После этого можно в любом месте кода вызывать:

python
self.pub_marker.publish(self.marker_array_msg)

И если RViz подписан на этот топик (MarkerArray), он отобразит маркеры.

Далее удаляем комментарий к коду:

python
 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 картинку.

python
         point_z = dist
        point_x = (x - m_cx) * point_z * inv_fx
        point_y = (y - m_cy) * point_z * inv_fy

Следующим шагом создаем макркер

python
        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

python
        self.marker_array_msg.markers[0] = marker
        self.pub_marker.publish(self.marker_array_msg)

Реализуем визуализацию круга на изображении

python
        cv2.circle(cv_rgb, (int(x), int(y)), int(radius),(0, 255, 0), 2)

В случае, когда объект исчез, маркер необходимо удалить:

python
 else:
        empty = Marker(action=Marker.DELETEALL)
        self.marker_array_msg.markers[0] = empty
        self.pub_marker.publish(self.marker_array_msg)

Полный код:

python
#!/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()

    # ---------- Подписки ----------
    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)

    # Синхронизация RGB + CameraInto
    self.ts = message_filters.ApproximateTimeSynchronizer(
      [self.image_sub, self.camera_info_sub], queue_size=10, slop=0.5)
    self.ts.registerCallback(self.callback)

    # ---------- Публикации ----------
    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())
# ячейка под один объект
    self.pub_marker = rospy.Publisher(
      '/trackingcam3d_client_ros/recognition_marker_array',
      MarkerArray, queue_size=1)

  def callback(self, rgb_data, camera_info):
    try:
      # ---------- Параметры камеры ----------
      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-кадр----------
      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

      # ---------- Поиск красного круга ----------
      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 contours:
        (x, y), radius = cv2.minEnclosingCircle(contours[0])
        x = np.clip(x, 0, x_size)
        y = np.clip(y, 0, y_size)

        # ---------- Оценка расстояния по площади ----------
        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 # метры (грубая оценка)
        print(f"dist: {dist:.2f} m")

        # ---------- преобразование 2D в 3D ----------
        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 # 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)

        # Кладем в массив и публикуем
        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)

      # ---------- Публикация обработанного изображения ----------
      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» и вставим новый код в данный файл.

Для этого в терминале пишем:

bash
>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»:

bash
>mv detect_blob.py detector_blob.py
>chmod +x detector_blob.py
>cd ~

Пересоберём пакеты, как было показано ранее в разделе про подготовку пространства и настройку среды ROS Noetic.

Обращаем внимание на ввод команды для загрузки графических интерфейсов при работе на ОС Windows:

bash
root@docker-desktop:~# export DISPLAY=host.docker.internal:0.0

Для запуска в первом терминале пропишем:

bash
>roslaunch trackingcam3d_client_ros trackingcam3d_client_ros.launch

Во втором терминале после настройки пространства пропишем команду для первого кейса:

bash
>rosrun trackingcam3d_client_ros detector_blob.py

После запуска проверим, в RViz в MarkerArray>MarkerTopic выбрано /trackingcam3d_client_ros/visualization_marker_array

image32

Детектирование красного шарика в RViz

Задача №2. Вычисление расстояния до объекта при неизвестных габаритах ​

Следующий код детектирует объект круглого, красного цвета и рассчитывает расстояние до него с помощью RGBD данных.

В данном кейсе мы заранее не знаем реальных размеров объекта.

Разберем код детальнее, его можно найти по пути:

bash
/catkin_ws/src/applied_robotics-trackingcam3d_client_ros/src/
recognition_object.py

Это полноценный ROS-узел, который:

  • Подписывается на RGB-камеру, depth-камеру и параметры калибровки камеры.

  • Находит круглый объект на RGB-изображении.

  • Извлекает глубину из области, соответствующей объекту.

  • Вычисляет расстояние и 3D-координаты.

  • Отображает маркер-сферу в RViz.

  • Печатает расстояние до объекта.

Импортируются ROS-библиотеки, OpenCV, NumPy, и средства для преобразования между sensor_msgs/Image и cv2:

python
import rospy, cv2, numpy as np, message_filters
from sensor_msgs.msg import Image, CameraInfo
from visualization_msgs.msg import MarkerArray, Marker
from geometry_msgs.msg import Pose
from cv_bridge import CvBridge

1.Класс Get_distance_from_camera:

python
self.marker_array_msg = MarkerArray()
marker = Marker()
self.marker_array_msg.markers.append(marker)

Создаётся массив маркеров (MarkerArray) с одним пустым Marker, который позже будет переиспользоваться.

2.Подписка на топики:

python
self.image_sub = message_filters.Subscriber(... image_raw)
self.depth_sub = message_filters.Subscriber(... depth/image_rect)
self.camera_info_sub = message_filters.Subscriber(... camera_info)

Используется message_filters.ApproximateTimeSynchronizer, чтобы вызывать callback(...) только когда все три сообщения пришли синхронно.

python
self.pub = rospy.Publisher('/recognition_object', Image, ...)
self.publisher_marker = rospy.Publisher('/visualization_marker_array', MarkerArray)

Один публикует изображение с результатами, другой — массив маркеров в RViz.

3.Основная логика:

Калибровочные параметры:

python
m_fx = camera_info.K[0] # фокус по X
m_cx = camera_info.K[2] # центр X
m_fy = camera_info.K[4] # фокус по Y
m_cy = camera_info.K[5] # центр Y

Используются для перевода 2D координат из изображения в 3D.

python
cv_rgb = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
depth_image = self.bridge.imgmsg_to_cv2(depth_data, "32FC1")

Преобразование ROS-изображений в OpenCV-формат.

python
depth_image_re = cv2.resize(depth_image, (640, 480))
depth_array = np.array(depth_image_re, dtype=np.float32)

Преобразование глубины и нормализация в формат визуализации.

python
gray = cv2.cvtColor(cv_rgb, cv2.COLOR_BGR2GRAY)
circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, ...)

Находит окружности методом Хафа.

python
roi_depth = depth_image_re[y-r:y+r, x-r:x+r]

Извлекается область глубины вокруг окружности.

python
for p in range(0, roi_depth.shape[0]):
 for l in range(0, roi_depth.shape[1]):
  value = roi_depth.item(p, l)
  if value > 0:
    sum += value
    n += 1

Вычисляется средняя глубина (исключая нули).

python
point_z = mean_z * 0.001
point_x = ((x + 320) - m_cx) * point_z * inv_fx
point_y = ((y + 240) - m_cy) * point_z * inv_fy

Из пиксельных координат и глубины рассчитывается 3D-позиция в системе координат камеры.

4.Построение и публикация маркера:

python
marker = Marker()
marker.header.frame_id = "stereo_camera"
marker.type = Marker.SPHERE
marker.pose.position.x = point_x
marker.pose.position.y = point_y
marker.pose.position.z = point_z

Создаётся сфера в RViz, соответствующая обнаруженному объекту.

python
self.marker_array_msg.markers[0] = marker
self.publisher_marker.publish(self.marker_array_msg)

Сфера публикуется в RViz.

5.Объединение и публикация изображений:

python
rgbd = np.concatenate((cv_rgb, cv_depth), axis=1)
faces_message = self.bridge.cv2_to_imgmsg(rgbd, "bgr8")
self.pub.publish(faces_message)

Слева — RGB, справа — визуализированная глубина. Публикуется результат в отдельный топик.

Для запуска кейса необходимо остановить работу прошлого кода (сочетание клавиш CTRL+C) и ввести новую команду для начала работы:

python
>rosrun trackingcam3d_client_ros recognition_object.py

В случае ошибки работы matplotlib – закомментируйте строку в коде:

python
 import matplotlib.pyplot as plt

image33

Детектирование красного шарика и расчёт расстояния

По данным, которые приходят в терминал, можно заметить, что ошибка в сравнение с предыдущим кодом, намного меньше и расчёт дистанции схожий с реальностью. Это связано с использованием алгоритма Хафа для поиска окружностей, он основан на геометрии (поиск границ), а не на поиск цветовых масок. Однако, все еще необходимо учитывать освещение, яркость и тени.

Задача №3. Получение облака точек по RGBD данным ​

Облако точек — это набор данных в 3D-пространстве, где каждый объект или поверхность представлены множеством отдельных точек с координатами (X, Y, Z). Это основной формат для работы с 3D-данными в робототехнике, компьютерном зрении и дополненной реальности.

Разберем код детальнее, его можно найти по пути:

python
/catkin_ws/src/applied_robotics-trackingcam3d_client_ros/src/
convert_node.cpp

Основная его функция – преобразование карты глубины с модуля SVCAM в облако точек и публикует его в ROS.

  1. Подключение библиотек
cpp
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
// для работы с изображениями OpenCV
#include <sensor_msgs/Image.h>
// для получения depth image
#include <sensor_msgs/CameraInfo.h>
// для параметров камеры
#include <sensor_msgs/PointCloud2.h>
// формат для 3D облака точек
#include <pcl_conversions/pcl_conversions.h>
// преобразование между PCL и ROS
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
  1. Глобальные переменные
cpp
ros::Publisher pub_point_cloud2; // публикатор облака точек
bool is_K_empty = 1;       // флаг, что параметры камеры ещё не получены
double K[9];           // матрица внутренней калибровки камеры
  1. camera_info_callback()
cpp
void camera_info_callback(const sensor_msgs::CameraInfoConstPtr
&camera_info_msg)

Подписан на топик camera_info.

Извлекает матрицу калибровки K:

cpp
[ fx 0 cx ]
[ 0 fy cy ]
[ 0  0 1 ]

Она нужна для восстановления 3D-координат из 2D.

  1. img_callback()
cpp
void img_callback(const sensor_msgs::ImageConstPtr &img_msg)

Подписан на топик с картой глубины image_rect.

Шаг 1: чтение глубины

cpp
unsigned short *depth_data = (unsigned short*)&img_msg->data[0];

глубина в формате 16UC1, единицы — миллиметры.

Шаг 2: преобразование в облако точек

cpp
z = depth / 1000.0; // перевод в метры
x = z * ((ux + width/2) - cx) / fx;
y = z * ((uy + height/2) - cy) / fy;

Цикл по пикселям:

(ux, uy) — координаты пикселя.

z — глубина.

x, y — пересчитанные координаты по формуле идеальной камеры-обскуры.

Шаг 3: публикация

cpp
pcl::toROSMsg(*cloud, point_cloud2);
point_cloud2.header.frame_id = "stereo_camera_first";
pub_point_cloud2.publish(point_cloud2);

Конвертация из PCL-облака в ROS-сообщение и публикация в /point_cloud.

  1. Функция main()
cpp
ros::Subscriber sub_img = n.subscribe(...);
ros::Subscriber sub_cmara_info = n.subscribe(...);
pub_point_cloud2 = n.advertise(...);

Подписка на:

/image_rect — карта глубины;

/camera_info — параметры камеры.

Публикация в:

/point_cloud — готовое облако точек.

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

cpp
 >rosrun trackingcam3d_client_ros convert_node

Чтобы увидеть данные необходимо в новом терминале прослушать топик «/point_cloud»

Для этого зайдем в Docker:

cpp
>docker ps -a
>docker exec –it <ID> /bin/bash

Далее требуется проинициализировать пространство:

cpp
>cd
>cd /catkin_ws
>source devel/setup.bash

C помощью команды echо начнем прослушивать данные

cpp
>rostopic echo /point_cloud

В терминале будут отображаться расстояние до каждого пикселя на изображении в байтах. В случае запуска данного кода на стационарном компьютере воспользуемся RViz для визуализации данных. В нем потребуется выставить параметры, как указано на рисунке.

image34

Параметры в RViz для отображения облака точек

Также можно добавить видеопоток с камеры глубины и обычной камеры. В итоге при визуализации получим следующие результаты.

image35

Облако точек совместно с видеопотоком камеры

Задача №4. Преобразование облака точек в формат данных лидара ​

Лидар отправляет данные в виде точек в 3D-пространстве или 2D-пространств, которые формируют облако точек. В нашем случае задача заключается в преобразовании точек 3D-пространства, получаемых с камеры, на плоскость. То есть имитация 2D лидара.

Код программы можно найти:

cpp
/catkin_ws/src/applied_robotics-trackingcam3d_client_ros/src/
test_laser.py

Разберем его детальнее:

  1. Импорт и инициализация
python
import roslib
import rospy
import cv2, numpy as np
import message_filters
from std_msgs.msg import String
from sensor_msgs.msg import Image, CameraInfo, LaserScan
from cv_bridge import CvBridge

Загружаются ROS-библиотеки, OpenCV и cv_bridge — мост между OpenCV и ROS.

  1. Класс get_face_distance_from_camera

В этом классе создаётся ROS-нода, подписывающаяся на 3 потока:

  • image_raw — цветное изображение (RGB),

  • depth/image_rect — карта глубины (float32 в метрах),

  • camera_info — параметры калибровки камеры.

python
self.ts = message_filters.ApproximateTimeSynchronizer(...)
self.ts.registerCallback(self.callback)

Эти три потока синхронизируются по времени с небольшой допустимой погрешностью (slop=0.5 сек), чтобы изображения и глубина совпадали.

  1. Публикация
python
self.scan_pub = rospy.Publisher('scan', LaserScan, queue_size=50)

Создаётся паблишер, который публикует LaserScan.

  1. Метод callback()

Выполняется каждый раз, когда приходят синхронизированные данные.

Чтение матрицы калибровки камеры:

python
camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]

Конвертация RGB и depth:

python
cv_rgb = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
depth_image = self.bridge.imgmsg_to_cv2(depth_data, "32FC1")

depth_image — карта глубины в метрах.

Нормализация глубины (для отладки):

cv2.normalize(...)

Глубина преобразуется в ч/б изображение (cv_depth) для визуализации.

Отладочные точки:

python
print(roi_depth[40, 300]) # глубина в конкретных пикселях
  1. Основной цикл
python
for y in range(0, roi_depth.shape[0] - 25):
  for x in range(0, self.num_readings):

Пробегает по всем пикселям (320 направлений) вдоль высоты изображения (y).

Переводит координаты пикселя и глубину в 3D-точки X, Y, Z в метрах, используя калибровку камеры.

python
point_x = ((x - cx) * Z) / fx
point_y = ((y - cy) * Z) / fy
  1. Расчёт расстояния:
python
dist = sqrt(x² + y² + z²)

Но сохраняется только расстояние до ближайшей точки по каждому направлению.

  1. Публикация результата как LaserScan:
python
self.scan_pub.publish(self.scan)

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

python
 >rosrun trackingcam3d_client_ros test_laser.py

Такой подход позволяет использовать 3D-камеру как 2D-лидар — например, для локализации, SLAM, навигации.

Для отображения данных RViz необходимо поставить галочку в «PointCloud» «LaserScan». Таким образом видно преобразование облака точек на плоскости.

image36

Преобразование облака точек на плоскость

Также в новом терминале можно прописать команды для получения данных из топиков

python
>rostopic echo /scan
>rostopic echo /point_cloud

Задача №5. Распознавание лица и определение расстояния ​

В данном коде реализуется задача по получению изображения и карты глубины с модуля SVCAM. После происходит обработка и обнаружения лица человека, при положительном исходе, в командную строку отправляется расстояние.

Код программы можно найти:

python
/catkin_ws/src/applied_robotics-trackingcam3d_client_ros/src/
face_recognition.py

Разберем код программы:

  1. Подключение библиотек
python
import rospy, cv2, numpy as np, message_filters, math
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge

Загружаются ROS, OpenCV, фильтры сообщений и функции для конвертации сообщений в изображения.

  1. Класс get_face_distance_from_camera

Подписка на данные

python
self.image_sub = Subscriber("/.../image_raw", Image)
self.depth_sub = Subscriber("/.../depth/image_rect", Image)
self.camera_info_sub = Subscriber("/.../camera_info", CameraInfo)

Получает цветное изображение, глубину и матрицу камеры.

python
self.ts = ApproximateTimeSynchronizer([...])
self.ts.registerCallback(self.callback)

Синхронизирует 3 потока по времени.

  1. callback: Главная логика

Выполняется при поступлении новых синхронных RGB, Depth и CameraInfo сообщений.

python
fx = camera_info.K[0], fy = K[4], cx = K[2], cy = K[5]

Чтение матрицы камеры, используется для пересчёта координат пикселя в метры.

Конвертация и подготовка изображений

python
cv_rgb = imgmsg_to_cv2(rgb_data, "bgr8")
depth_image = imgmsg_to_cv2(depth_data, "32FC1")
depth_image_re = cv2.resize(depth_image, (640, 480))

Цветное и глубинное изображения из ROS в OpenCV.

Глубина масштабируется в 640×480 (если необходимо).

Нормализация глубины (для визуализации)

python
depth_array → normalize → depth_8 → cv_depth

Создаётся 3-канальное серое изображение глубины для отображения.

Обнаружение лиц (Haar каскад)

python
face_cascade = cv2.CascadeClassifier('haar_cascade.xml')
gray = cv2.cvtColor(cv_rgb, cv2.COLOR_BGR2GRAY)
faces = face_cascade.detectMultiScale(gray, 1.3, 5)

Ищет прямоугольники лиц на ч/б изображении.

Расчёт расстояния до лица

Для каждого найденного лица:

python
roi_depth = depth_image_re[y+30:y+h-30, x+30:x+w-30]

Выбирается внутренний регион интереса (ROI) в глубинном изображении.

python
n = 0; sum = 0
for i, j in ROI: if value > 0 → sum += value
mean_z = sum / n

Вычисляется средняя глубина внутри прямоугольника.

Потом — пересчёт в координаты X, Y, Z:

python
point_z = mean_z * 0.001 # метры
point_x = ((x + w/2) - cx) * point_z / fx
point_y = ((y + h/2) - cy) * point_z / fy

Расстояние:

python
dist = sqrt(x² + y² + z²)

Отображение на изображении

python
cv2.rectangle(...) # обводит лицо
cv2.putText(cv_rgb, dist_str, ...) # пишет расстояние до него

Публикация результата

python
rgbd = np.concatenate((cv_rgb, cv_depth), axis=1)
msg = cv2_to_imgmsg(rgbd, "bgr8")
self.pub.publish(msg)

Склеивает цветное и глубинное изображение.

Публикует результат в /trackingcam3d_client_ros/faces_recognition.

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

python
>rosrun trackingcam3d_client_ros face_recognition.py

В терминал будут приходить данные расстояния до распознанного лица.

При использовании ПК в RViz можно получить изображение.

image37

Расстояние до распознанного лица человека

Работа со стереокамерой на встроенном микрокомпьютере NanoPi-AR

Модуль SVCAM оснащён встроенным микрокомпьютером NanoPi-AR, который позволяет выполнять обработку изображений непосредственно на устройстве - без необходимости использовать стационарный компьютер, подключать Docker Desktop или устанавливать дополнительное ПО на внешней системе. Благодаря этому камера работает как полностью автономный вычислительный модуль, способный запускать алгоритмы технического зрения прямо на борту.

Ключевые преимущества данного подхода:

  • Автономность: камера может работать без ПК, необходимо только питание.

  • Интеграция: прямая связь с другими устройствами по DXL-шине. Подходит для мобильных роботов.

Недостатки:

  • Ограниченная мощность по сравнению с ПК.

  • Недостаточно GPU для полноценной визуализации картинки в реальном времени.

Подключение SVCAM к внешним контроллерам

При разработке робототехнических систем часто требуется организовать обмен данными между модулем технического зрения и внешним контроллером, например микроконтроллером управления роботом. Стереокамера SVCAM, оснащённая встроенным микрокомпьютером NanoPi-AR, может выполнять задачи обработки изображений: распознавать объекты, измерять расстояния до них и передавать полученные данные во внешний контроллер.

В модуле SVCAM связь с внешними устройствами может осуществляться двумя основными способами: по UART или через DXL-шину.

UART соединение (Universal Asynchronous Receiver/Transmitter) UART - классический асинхронный последовательный интерфейс, один из самых распространённых способов обмена данными между микроконтроллерами, компьютерами и периферийными устройствами.

Модуль SVCAM оснащен двумя UART портами - UART1 (/dev/ttyS1) и UART2 (/dev/ttyS2).

image38

Распиновка SVCAM для подключения по UART

Передача данных осуществляется по трём линиям: TX (передача данный), RX (приём данных), и GND (общая земля для обоих устройств).

Как осуществляется подключение:

SVCAM TX подключается к RX микроконтроллера

SVCAM RX подключается к TX микроконтроллера.

GND обоих устройств соединяются вместе.

Преимущества:

  • Простота реализации и настройки.

  • Поддерживается практически всеми микроконтроллерами (OpenCM, Arduino, DXL-IoT, STM32, ESP32 и др.).

  • Не требует сложных протоколов или специализированного оборудования

Недостатки:

  • Интерфейс «точка-точка» - обычно работает только между двумя устройствами.

  • Ограниченная скорость и дальность по сравнению с более современными интерфейсами. DXL соединение. DXL - специализированный полудуплексный интерфейс, используемый в продукции, производимой ГК “Прикладная робототехника”. Он также может применяться для обмена пользовательскими данными, поскольку работает по единой шине, к которой можно подключить несколько устройств.

Модуль SVCAM оснащён четырьмя DXL-портами, при этом все они подключены к одной логической шине - /dev/ttyS1. Для подключения можно использовать любой из этих портов.

image39

DXL порты модуля SVCAM

Как осуществляется подключение:

Прямое подключение DXL портов устройств.

Преимущества:

  • Возможность объединить несколько устройств на одной шине (сервоприводы, датчики, камеры).

  • Возможность питания и обмена по одной шине.

Недостатки:

  • Требуется работа по протоколу Dynamixel и обеспечение полудуплексного режима.

Первоначальная настройка микрокомпьютера NanoPi-AR

Для взаимодействия с модулем SVCAM на NanoPi-AR с ПК под управлением Windows необходимо настроить удалённое подключение по протоколу SSH (Secure Shell). SSH позволяет управлять микрокомпьютером NanoPi-AR через командную строку, отправлять команды и устанавливать программное обеспечение. В этом разделе описан процесс настройки и установки необходимых пакетов программ для дальнейшей работы с модулем SVCAM с использованием программы MobaXterm. Данную программу можно скачать по ссылке https://mobaxterm.mobatek.net/.

Перед началом убедимся, что модуль SVCAM подключён к ПК по USB, а сетевой адаптер настроен с IP-адресом 10.10.11.1 (см. раздел "Подключение модуля SVCAM к ПК с ОС Windows").

В MobaXterm нажмем кнопку Session в верхнем меню. Выберем тип сессии SSH. В поле Remote host введем IP-адрес модуля SVCAM: 10.10.11.2. Убедимся, что порт установлен на 22 (по умолчанию для SSH) и нажмите ОК.

image40

Окно настройки SSH-сессии в MobaXterm

Для подключения нужно ввести логин pi и пароль pi.

image41

Подключение к микрокомпьютеру NanoPi-AR

image42

Терминал MobaXterm после успешного подключения

После успешного подключения можем приступить к установке необходимых библиотек для работы с камерой на микрокомпьютере NanoPi-AR.

Настройка сетевого подключения

Для скачивания исходных файлов и библиотек необходимо подключить встроенный микрокомпьютер NanoPi-AR к сети. Можно подключиться по Wi-Fi, выполняя следующую команду:

bash
sudo nmcli -a d wifi connect <имя_сети>

Например, если имя сети Wi-Fi applied_robotics, выполняем команду:

bash
sudo nmcli -a d wifi connect applied_robotics

После этого вводим пароль сети и подключение выполнено.

image43

Успешное подключение по Wi-Fi

Дальше обновим существующий пакет программ с помощью следующих команд :

Скачаем репозиторий SVCAM:

bash
 https://wiki.appliedrobotics.ru/metod/MP_SVCAM/links.html#repo-svcam

git clone «ссылканарепозиторий»

Затем выполняем следующую команду для обновления текущих пакет программ микрокомпьютера:

bash
cd SVCAM/
chmod +x init.sh && sudo ./init.sh

Если при обновлении появится сообщение, то необходимо напечатать “Y” и нажать на ENTER, после чего продолжится обновление.

image44

Сообщение при обновлении

После обновления необходимо перезагрузить микрокомпьютер выполняя команду:

bash
sudo reboot

Установка библиотеки ZeroMQ

ZeroMQ - это высокопроизводительная асинхронная библиотека обмена сообщениями, предназначенная для использования в распределённых или параллельных приложениях. Она реализует паттерн "publish-subscribe", "client-server" и другие модели обмена сообщениями. Для её установки сначала необходимо подключится к интернету (см. Настройка сетевого подключения) и выполняйте следующие команды:

bash
cd ~
wget http://archive.org/download/zeromq_4.0.5/zeromq-4.0.5.tar.gz
tar -xvf zeromq-4.0.5.tar.gz
cd zeromq-4.0.5
./configure
sudo make install
sudo ldconfig

Установка libtconfig

libtconfig - это библиотека для работы с конфигурационными файлами в формате .cfg, поддерживающая вложенные секции и типы данных (int, float, string).

bash
cd ~/SVCAM/libs/
mkdir applied_robotics-libtconfig-180423-build
cd applied_robotics-libtconfig-180423-build
cmake ../applied_robotics-libtconfig-180423
sudo make install

Установка libtlive

libtlive - библиотека для потоковой передачи данных с камер в реальном времени, поддерживающая различные форматы видео и сетевые протоколы.

bash
cd ~/SVCAM/libs/
mkdir applied_robotics-libtlive-180423-build
cd applied_robotics-libtlive-180423-build
cmake ../applied_robotics-libtlive-180423
sudo make install

Установка библиотеки yaml**-**cpp

yaml-cpp - библиотека для обработки конфигурационных файлов в формате YAML.

bash
cd ~
git clone https://github.com/jbeder/yaml-cpp.git
cd yaml-cpp
git checkout yaml-cpp-0.6.3
mkdir ~/yaml-cpp-build && cd ~/yaml-cpp-build
cmake ../yaml-cpp -DYAML_CPP_BUILD_TESTS=OFF
make -j1
sudo make install

Оставка библиотеки trackingcam3d_reader

Библиотека trackingcam3d_reader - клиентская библиотека для получения изображений с модуля SVCAM (RGB, карта глубины и т.п).

bash
cd ~/SVCAM/libs/
mkdir trackingcam3d_reader-build
cd trackingcam3d_reader-build
cmake ../trackingcam3d_reader
make
sudo make install

Чтобы убедиться в работоспособности данной библиотеки можно запустить демо-программу для отображения на экран изображений с камеры. Поскольку камера SVCAM может генерировать несколько типов изображений одновременно (RGB, глубина, исправленные изображения и т.д.), при запуске демо-программы, нужно указать тип изображений, которые надо выводить. Список поддерживаемых изображений:

- left**: Изображение с левой камеры. Это базовое RGB-изображение для обработки (например, нахождение объектов).

- left**_**rect**: Исправленное изображение с левой камеры, где убраны искажения объектива (дисторсия). Её разница с left: отсутствие "рыбьего глаза" или других дефектов.

- left**_gray_**rect**: Исправленное изображение с левой камеры в оттенках серого. Данный тип изображения может потребоваться для задач, где цвет не нужен (экономит ресурсы, быстрее обработка).

- right**: RGB изображение с правой камеры (аналогично left).

- right**_**rect**: Исправленное изображение с правой камеры (аналогично left_rect).

- right_gray_rect: Аналогично left_gray_rect.

- depth**: Карта глубины (depth map). Она показывает расстояние до каждого пикселя. Идеально для навигации, избегание препятствий или измерений дальностей до объектов. Важно заметить, что это не изображение, а "карта глубины" (требует нормализации для просмотра).

- disparity**: Карта диспаритета (disparity map), показывающая разницу между левым и правым изображением (основа для расчёта глубины). Разница: Не глубина, а "сдвиг" пикселей; требует калибровки для преобразования в метры.

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

bash
cd ~/SVCAM/libs/trackingcam3d_reader-build/
./trackingcam3d_reader_demo <тип изображения>

image45

Визуализация изображении с левой камеры и карты глубины

Исходный код демо программы можно найти в директории ~/SVCAM/libs/trackingcam3d_reader/examples/main.cpp

Установка сервиса dxlSlave**.**service

Данный программный пакет позволяет использовать модуль SVCAM как DXL-совместимое устройство (ведомое), обменивающееся данными с ведущим контроллером (например, OpenCM, DXL-IoT) по протоколу Dynamixel.

Обмен осуществляется через набор из 32 регистров, каждый объёмом 4 байта (адреса от 0 до 31), которые заполняются или читаются ведущим устройством.

Сервис dxlSlave выполняет роль эмулятора Dynamixel-устройства для модуля SVCAM. Для установки выполняем следующие команды:

bash
cd ~/SVCAM/libs/dxlSlaveService
chmod +x install_service.sh && sudo ./install_service.sh

После установки сервис автоматически запускается в фоновом режиме и будет загружаться при каждом включении микрокомпьютера. Чтобы проверить, работает ли сервис, выполняем команду:

bash
sudo systemctl status dxlSlave.service

image46

Сообщение статуса при активном состоянии сервиса

После запуска сервис переводит SVCAM в режим DXL-совместимого модуля со следующими параметрами по умолчанию: Baudrate = 115200, ID устройства = 1, режим работы : “dxl”. Также следует отметить, что сервис поддерживает два режима функционирования – режим “dxl” (стандартный режим обмена по протоколу Dynamixel) или режим “lidar” (специальный режим передачи данных лидара).

Для изменения параметров сервиса (скорость обмена, ID, режим…), необходимо отредактировать параметры в файле конфигурации /usr/bin/dxl/config.json

Открывается данный файл через команду:

bash
sudo nano /usr/bin/dxl/config.json

image47

Параметры сервиса dxlSlave.service

Сохраните файл с помощью сочетания клавиш CTRL+O, ENTER, затем закрывайте его с сочетанием клавиш CTRL+X.

После изменения файла конфигурации перезапускаем сервис:

bash
sudo systemctl restart dxlSlave.service

Установка библиотеки dxlSlave

Теперь переходим к установке библиотеки dxlSlave. Данная библиотека, написанная на языке С++ позволяет подключиться к сервису dxlSlave.service и изменить значения регистров модуля. Для установки выполняем следующие команды:

bash
cd ~/SVCAM/libs/
mkdir -p dxlSlaveLib-build && cd dxlSlaveLib-build
cmake ../dxlSlaveLib
make
sudo make install

Итак, все необходимые модули установлены. Теперь можно перейти к запуску задач.

Задача №6. Распознавание и вычисление расстояния до шара. Отправка данных о расстоянии на внешний контроллер по DXL ​

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

Разберем код детальнее, его можно найти по пути:

bash
 ~/SVCAM/cases/recognition_object/src/main.cpp

Это полноценное автономное приложение, которое:

- Подключается к потоку изображений камеры SVCAM через TrackingCamReader (RGB + глубина)

- Загружает параметры камеры из config/config.yaml

- Находит круглый объект методом Хафа на сером изображении

- Извлекает среднюю глубину из области объекта.

- Вычисляет 3D-координаты и евклидово расстояние до объекта.

- Записывает расстояние (в мм) в регистр 24 модуля, чтобы можно было считать данные на контроллере OpenCM.

- Отображает результат на экране (опционально).

Данная программа работает аналогично программе, которую рассмотрели в задаче 2 (см. Задача №2 Вычисление расстояния до объекта при неизвестных габаритах). Основные пункты, на которые следует обратить внимание:

Подключение необходимых библиотек для обработки изображения и отправка данных по DXL.

cpp
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <yaml-cpp/yaml.h>
#include <trackingcam3d_reader/TrackingCam3d_Reader.h>
#include «dxlSlave.h»

Библиотека opencv для обработки изображений.

Библиотека yaml-cpp для загрузки параметров с файла конфигурации.

Библиотека trackingcam3d_reader для получения изображений с камеры.

Библиотека dxlSlave для Dxl связи и отправка данных на периферийные устройства по Dxl протоколу.

Инициализация адрес регистра, куда будем записать данные о расстоянии до объекта (регистр 24).

cpp
#define DIST_ADDR  24

Обработка сигнала CTRL+C для безопасного завершения программы. Для работы данной функции, необходимо подключение стандартных библиотек “atomic” и “signal.h”.

cpp
std::atomic<bool> stop{false};
void signalHandler(int signum)
{
  stop.store(true, std::memory_order_relaxed);
}

В программе отслеживается нажатие на сочетание клавиш CTRL+C, вызывая метод в начале функции main.

cpp
Signal(SIGINT, signalHandler);

Инициализация объекта DxlSlave. Если подключение не проходит, т.е. программа не может подключиться к сервису dxlSlave.service, то выводится сообщение об ошибке и завершается программа.

cpp
  DxlSlave slave;
  if (!slave.connect()) {
    std::cerr << “DxlSlave connection error” << std::endl;
    return 1;
  }

Загрузка параметров конфигурации кейса в файле config/config.yaml.

cpp
  YAML::Node config;
  std::string config_base_path = “../config/”;
  std::string config_path = config_base_path + “config.yaml”;
  try {
    config = YAML::LoadFile(config_path);
  }
  catch (YAML::BadFile& e) {
    slave.disconnect();
    std::cerr << “Failed to load config file “ << config_path << std::endl;
    return 1;
  }
  const double fx = config[“camera”][“intrinsics”][“fx”].as<double>();
  const double fy = config[“camera”][“intrinsics”][“fy”].as<double>();
  const double m_cx = config[“camera”][“intrinsics”][“cx”].as<double>();
  const double m_cy = config[“camera”][“intrinsics”][“cy”].as<double>();
  const double inv_fx = 1 / fx;
  const double inv_fy = 1 / fy;
  const bool display_flag = config[“display”].as<bool>();
  const int max_no_frames = config[“max_no_frames”].as<int>();
  const int read_delay_ms = config[“read_delay_ms”].as<int>();
  const float min_valid_pixel_percent = config[“min_valid_pixels_percent”].as<float>();
  const double max_depth = config[“max_depth_mm”].as<double>();

Подключение камеры при объявлении объекта TrackingCamReader. При объявлении задаётся запрос на типы изображений, которые необходимы и количество кадров для каждого потока. Для данного кейса нужны изображения left и depth. При неуспешном подключении к потокам изображений программа завершается.

cpp
  Const std::vector<std::string> stream_requests = {“left=100000”, “depth=100000”};
  TrackingCamReader cam_reader(stream_requests);
  if (!cam_reader.connect())
  {
    slave.disconnect();
    std::cerr << “Failed to connect camera client” << std::endl;
    return 1;
  }

Основная логика обработки кадров. Данный участок кода работает пока не сработало сочетание клавиш CTRL+C.

cpp
While (!stop.load(std::memory_order_relaxed))

Проводится получение синхронизированных RGB и depth изображений, как и расчёт расстояния до объекта. Получение новых изображении в цикле проводится следующим образом:

cpp
auto frames = cam_reader.getLatestFrames();

Если не получены изображения левой камеры или карты глубины, происходит пропуск дальнейшее выполнение данного цикла с помощью команды continue**.

cpp
If (frames.find(“left”) != frames.end() && !frames[“left”].empty()) {
last_left = frames[“left”].clone();
}
if (frames.find(“depth”) != frames.end() && !frames[“depth”].empty()) {
last_depth = frames[“depth”].clone();
}
if (last_left.empty() || last_depth.empty()) {
std::this_thread::sleep_for(std::chrono::milliseconds(read_delay_ms / 2));
continue;
}

Отправка данных по DXL. После расчёта расстояния обновляется значение регистра DIST_ADDR, где сохраняется значение расстояния до объекта в мм. Если объект не найден, то значение расстояния равно 0.

cpp
Slave.callMethod(DIST_ADDR, dist_mm);

Проводится запись значения расстояния в мм в регистр DIST_ADDR.

Отображение результата на экран (если включен параметр визуализации display в файле конфигурации config/config.yaml). Если объект найден, то на визуализации отображается окружность вместе с текстом о расстоянии до объекта.

cpp
If (display_flag) {
cv::circle(rgb, center, (int)best_r, cv::Scalar(0, 255, 0), 2);
std::string dist_str = “Dist: “ + std::to_string(best_dist).substr(0, 4) + “m”;
cv::putText(rgb, dist_str, cv::Point(center.x, center.y – 5),
cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(255, 255, 255), 2);
}

Вывод RGB изображения на экран:

cpp
   if (display_flag)
  {
      cv::Mat rgb_resized;
      cv::resize(rgb, rgb_resized, cv::Size(320, 240));
      cv::imshow(“Circle Detection”, rgb_resized);
      if (cv::waitKey(1) == 27) // Если нажат ESC, то завершаем программу
      {
        stop.store(true, std::memory_order_relaxed);
        continue;
      }
  }

Завершение программы.

cpp
Slave.callMethod(DIST_ADDR, 0); // Сброс регистра на 0
cam_reader.disconnect(); // Закрытие соединения с камерой
slave.disconnect(); // Закрытие DXL соединения
cv::destroyAllWindows(); // Для закрытия окна визуализации
return 0;

Для того, чтобы включить/выключить отображение изображений на экран нужно изменить параметр display (true – вкл, false – выкл) в файле конфигурации config/config.yaml. Открываем config-файл с помощью следующей команды:

cpp
nano ~/SVCAM/cases/recognition_object/config/config.yaml

image48

Файл конфигурации кейса

Чтобы избежать перегрузки микрокомпьютера (не хватает GPU) рекомендуется использовать функционал отображения на экран только для отладки.

Необходимо собрать проект recognition_object перед запуском:

bash
cd ~/SVCAM/cases/recognition_object/
mkdir -p build && cd build
cmake ..
make
Для запуска задачи, выполняем следующие комманды:
cd ~/SVCAM/cases/recognition_object/build/
./recognition_object

image49

Пример работы программы кейса и визуализации

Чтение данных на внешнем контроллере по DXL

Если внешний контроллер подключён к модулю SVCAM по интерфейсу DXL, он может считывать значения регистров модуля. Это позволяет, например, получать данные о расстоянии до объекта, измеренные стереокамерой.

Рассмотрим пример чтения данных о расстоянии на контроллере OpenCM 9.04 с платой расширения STEM Board.

На этой плате DXL-порт контроллера управляется интерфейсом Serial3, что соответствует порту с номером 3 в DynamixelSDK.

Ниже представлена пример программы для OpenCM, которая считывает значение из регистра 24, хранящего расстояние до объекта в миллиметрах.

cpp
#include <DynamixelSDK.h> // Библиотека DynamixelSDK
#define DEVICENAME    “3” // номер DXL порта OpenCM (Serial3)
#define DATA_ADDR     24  // Адрес регистра, который хотим прочитать
#define PROTOCOL_VERSION  1.0
#define BAUDRATE      115200
#define DXL_ID   1 // ID SVCAM, в файле конфиг /usr/bin/dxl/config.json

ynamixel::PortHandler *portHandler;
ynamixel::PacketHandler *packetHandler;

uint32_t data = 0;
float distance = 0.0f; // Расстояние до объекта в м

void setup()
{
 Serial.begin(115200);
 while(!Serial) {}
 portHandler = ynamixel::PortHandler::getPortHandler(DEVICENAME);
 packetHandler = ynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

 if (!portHandler->openPort()) {
  Serial.println(“Failed to open port!”);
  while(1);
 }
 portHandler->setPacketTimeout(200.0);
 if (!portHandler->setBaudRate(BAUDRATE)) {
  Serial.println(“Failed to set baudrate!”);
  while(1);
 }
 Serial.println(“Ready to read distance data.”);
}

void loop()
{
 int dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL_ID, DATA_ADDR, &data);
 distance = data / 1000.0;
 Serial.print(“Distance to object: “); Serial.println(distance);
 delay(40);
}

Подключаем камеру к OpenCM и запускаем программы на микроконтроллере и камере.

image50

Соединение модуля SVCAM и микроконтроллера OpenCM по DXL через плату расширения STEM Board

image51

Пример чтения данных о расстоянии до шарика на микроконтроллере OpenCM

Пример кода для чтения расстояния на микроконтроллере DXL-IOT:

cpp
#include «DxlMaster.h»  // Подключение библиотеки

#define BAUDRATE 115200 // скорость порта DXL
#define ID    1   // Номер устройства

#define DIST_ADDR 24   // Адрес, где хранится расстояние

DynamixelDevice svcam(ID);

float distance = 0.0f;
uint32_t data = 0;

void setup() {
 Serial.begin(115200);
 while (!Serial);

 DxlMaster.begin(BAUDRATE);
 uint8_t status = svcam.init();
}

void loop()
{
 svcam.read(DIST_ADDR, data);
 distance = data / 1000.0;
 Serial.print(“Distance to object: “); Serial.println(distance);
 delay(40);
}

Задача №7. Распознавание и вычисление расстояния до лица. Отправка данных о расстоянии на внешний контроллер по DXL ​

По своей структуре, данный кейс работает аналогично предыдущего, только в отличие от распознавания шарика проводится распознавание лица. Алгоритм распознавания и расчёта основан на методах, описанных раньше в задаче 5 (см. Задача №5 Распознавание лица и определение расстояния). Код программы можно найти по пути:

cpp
~/SVCAM/cases/face_recognition/src/main.cpp

До запуска сначала необходимо собрать проект face_recognition:

bash
cd ~/SVCAM/cases/face_recognition/
mkdir -p build && cd build
cmake ..
make
Для запуска кейса, выполните команду:
cd ~/SVCAM/cases/face_recognition/build/
./face_recognition

Для того, чтобы включить/выключить отображение изображений на экран нужно изменить параметр display (true – вкл, false - выкл) в файле конфигурации ~/SVCAM/cases/face_recognition/config/config.yaml как в предыдущем кейсе.

При выполнении данной программы значение расстояния до лица хранится в регистре с номером 24, можно это значение читать с помощью внешнего контроллера при подключении по DXL. Методика чтения данных на внешнем контроллере при подключении по DXL была представлена в предыдущей главе.

Задача №8. Работа модуля SVCAM в режиме лидара ​

Как было рассмотрено ранее в задаче №4 (Преобразование облака точек в формат данных лидара), данный кейс позволяет преобразовывать облако точек, полученное стереокамерой, в формат данных лидара и передавать эти данные внешнему контроллеру через DXL-порт.

В режиме лидара сообщения, отправленные модулем SVCAM, не являются стандартными DXL пакетами, а просто пользовательские UART сообщения. Поэтому, для чтения данных в формате лидара не нужны посторонних библиотек (например Dynamixel_SDK, DxlMaster и т.п.) со стороны внешнего контроллера. Итак, данные лидара можно читать не только при подключении по DXL, но и при подключении по UART с внешним контроллером.

Таким образом, данные лидара можно отправить на внешний контроллер двумя способами: по DXL-порту или по UART (UART1 или UART2). Это делает режим лидара универсальным и удобным для любых внешних контроллеров.

Для работы в режиме лидара необходимо изменить параметр “Mode” в конфигурационном файле сервиса dxlSlave.service. Его можно изменить в файле конфигурации сервиса /usr/bin/dxl/config.json.

В файле также можно изменить:

  • режим работы: Mode: "lidar"

  • порт передачи данных:

bash
/dev/ttyS1 — используется и DXL-шиной, и как UART1 (для подключение по UART)
/dev/ttyS2 — UART2.

Для подключения через UART рекомендуется выбрать UART2 - /dev/ttyS2 (особенно если DXL-шина внешнего контроллера занята другими устройствами).

Файл конфигурации сервиса можно открыть с помощью команды:

bash
sudo nano /usr/bin/dxl/config.json

image52

Параметры сервиса dxlSlave.service

Сохраняем файл с помощью сочетания клавиш CTRL+O, ENTER, затем закрываем его сочетанием клавиш CTRL+X. После надо перезагрузить сервис выполняя команду:

bash
sudo systemctl restart dxlSlave.service

Алгоритм работы программы test**_**laser

Исходный код программы преобразования облака точек в формат данных лидара находится по пути: ~/SVCAM/cases/test_laser/src/main.cpp

Основной цикл программы проходит по всем горизонтальным направлениям изображения — 320 пикселей по ширине.

Для каждого направления:

  • перебираются точки вдоль вертикальной линии (ось y),

  • выбирается ближайшая точка,

  • расстояние до неё записывается в массив ranges.

Таким образом формируется массив из 320 значений, где каждое — расстояние до ближайшего препятствия в данном направлении. Угловой диапазон соответствует примерно –1.97…+1.97 рад по горизонтали.

Максимально допустимое расстояние равно 255 см. Если значение равно 255, это может означать что реальная дистанция > 255 см, или алгоритм не смог вычислить корректное расстояние для данного направления.

Отправка данных лидара на внешний контроллер

Передача данных выполняется с помощью библиотеки DxlSlave, вызывая метод:

cpp
slave.callMethod(pixelIndex, distanceCm);

Пример из программы:

bash
for (int i = 1; i < ranges.size() + 1; ++i)
{
  slave.callMethod(i, (int)ranges[i-1]);
}
где i – индекс направления по горизонтали (от 1 до 320).
Проект test_laser собираем таким образом:
cd ~/SVCAM/cases/test_laser/
mkdir -p build && cd build
cmake ..
make
Для запуска выполняем команду:
cd ~/SVCAM/cases/test_laser/build/
./test_laser

Чтение данных формата лидара на внешнем микроконтроллере

В режиме лидара каждое отправленное модулем SVCAM сообщение представляет собой короткий пользовательский UART-пакет, который не является стандартным пакетом Dynamixel. Поэтому внешнему контроллеру достаточно уметь читать обычные последовательные данные — никаких дополнительных библиотек не требуется. Модуль SVCAM отправляет ряд сообщений по формату: svs:194:0:sv!

svs - начала сообщения

194 - индекс направления (от 1 до 320)

0 - расстояние в см (от 0 до 255)

sv! конец сообщения

Таким образом, модуль SVCAM последовательно отправляет 320 таких сообщений, по одному для каждого направления сканирования.

Рассмотрим пример программы для контроллера OpenCM 9.04 с платой STEM Board, подключённой к SVCAM через DXL. На этом контроллере DXL-порт соответствует интерфейсу Serial3, который в коде обозначим как CAM_SERIAL.

Важно отметить, что, если бы подключение выполнялось по UART или был использован другой контроллер, код полностью сохранялся бы. Следовало бы лишь правильно указать порт куда подключена стереокамера (например, Serial1, Serial2 и т.п.).

В программе определяются основные параметры:

CAM_SERIAL - порт, к которому подключён SVCAM (Serial3 при DXL).

BAUDRATE = 115200 - скорость обмена.

NUM_MEASUREMENTS = 320 - размер массива данных

distances**[**] - массив для хранения значений расстояний

newDataAvailable - флаг, принимающий значение true, когда массив полностью заполнен.

Обработка сообщений осуществляется в функции parseMessage**(**), которая:

  • ловит входящие строки вида svs:индекс:расстояние:sv!

  • выделяет индекс и значение

  • записывает расстояние в массив distances[]

  • устанавливает флаг после получения всех 320 значений

Для вычисления расстояния до объекта прямо перед камерой используется функция computeFrontAverage**(**). Она берёт небольшой диапазон направлений в центре массива и усредняет значения. Если обработка невозможна (например, нет валидных данных), функция возвращает –1.

Полный код программы представлен ниже.

cpp
#define CAM_SERIAL Serial3 // Dxl порт микроконтроллера OpenCM
#define BAUDRATE 115200
#define NUM_MEASUREMENTS 320 // кол-во измерений
int distances[NUM_MEASUREMENTS]; // Массив для сохранения результатов
bool newDataAvailable = false; // Прочитан ли весь массив данных лидара (320 значений)

void parseMessage(String msg)
{
 msg.trim();
 if (!msg.startsWith("svs:")) return;
 msg.remove(0, 4); // удалим "svs:"
 int firstColon = msg.indexOf(':'); // позиция первого “:”
 int secondColon = msg.indexOf(':', firstColon + 1); // позиция второго “:”
 if (firstColon < 0 || secondColon < 0) return;
 int index = msg.substring(0, firstColon).toInt(); // номер пикеля
 int value = msg.substring(firstColon + 1, secondColon).toInt(); // само значение

 distances[index - 1] = value; // Запись данных лидара в массив

 if (index == NUM_MEASUREMENTS) {
  newDataAvailable = true; // Все значения прочитаны
 }
}

float computeFrontAverage(int *data)
{
 int center = NUM_MEASUREMENTS / 2; // Индекс среднего пикселя в массиве
 int window = 10; // пиксели, которые сканируем слева/справа от среднего
 long sum = 0;
 int count = 0;

 for (int i = center - window; i < center + window + 1; ++i)
 {
  if (data[i] < 255) {
   sum += data[i];
   count++;
  }
 }
 if (count == 0) return -1; // Если не удалось считать расстояние, то возвращает -1
 return (float)sum / count;
}

void setup()
{
 CAM_SERIAL.begin(BAUDRATE);
 Serial.begin(115200);
 for (int i = 0; i < NUM_MEASUREMENTS; i++) distances[i] = 255;
 Serial.println("Ready to read data");
}

void loop()
{
 static String incomingMessage = "";

 while (CAM_SERIAL.available())
 {
  char c = CAM_SERIAL.read();

  if (c == '!') // конец сообщения
  {
   parseMessage(incomingMessage);
   incomingMessage = "";
  }
  else {
   incomingMessage += c;
  }
 }
 if (newDataAvailable)
 { // Если заполнена таблица, то считаем расстояние
  float frontDistance = computeFrontAverage(distances);
  Serial.print("Front distance (cm): ");
  Serial.println(frontDistance);
  newDataAvailable = false;
 }
}

Подключаем камеру к OpenCM по Dxl порту и запускаем программу на микроконтроллере и камере. Для запуска кейса на микрокомпьютере камеры выполните команды:

bash
cd ~/SVCAM/cases/test_laser/build/
./test_laser

image53

Расчёт расстояния до объекта перед камерой с визуализацией изображений с левой и правой камеры

Запуск службы для автоматического запуска алгоритмов ТЗ и отправки данных на внешний контроллер

Задача обеспечения полностью автономной работы модуля SVCAM и непрерывного функционирования робототехнической системы может решаться с помощью автоматической службы.

Автоматическая служба — это специализированная программа или процесс, который запускается автоматически при включении системы и работает в фоновом режиме без участия пользователя.

Такая служба может отвечать за запуск алгоритма и передачу данных, например информации о распознанных объектах, расстояниях, данных лидара, состоянии алгоритмов и т.д. В данной главе рассматривается принцип работы такой службы на примере запуска процесса передачи данных лидара на внешний контроллер.

Сначала создаём файл конфигурации службы. Для этого выполняем команду:

bash
sudo nano /etc/systemd/system/lidar.service

Откроется текстовый редактор, в котором необходимо написать содержимое, показанное на рисунке.

image54

Файл конфигурации службы lidar.service

Сохраняем файл с помощью сочетания клавиш CTRL+O, затем ENTER, после чего закрываем редактор сочетанием CTRL+X.

Этот файл описывает systemd-службу, которая автоматически запускает указанную программу после включения системы. Ниже приведено объяснение параметров службы:

Description - краткое описание службы. Здесь указано, что это сервис для работы с лидаром.

ExecStartPre=/bin/sleep 7 - перед запуском службы система сделает паузу 7 секунд. Это нужно, чтобы дождаться загрузки необходимых модулей или периферии.

ExecStart=/home/pi/SVCAM/cases/test_laser/build/test_laser - команда, которая будет выполнена при старте службы. В данном случае запускается исполняемый файл test_laser, расположенный в каталоге проекта. Нужно указать полный путь к исполняемому файлу.

WantedBy=multi-user.target — указывает, что служба должна запускаться автоматически при достижении стандартного режима многопользовательской системы (обычный режим работы без графического интерфейса).

Для запуска сервиса выполняем следующие команды:

bash
sudo sytemctl daemon-reload
sudo systemctl enable lidar.service
sudo systemctl start lidar.service

После этого служба будет запущена и будет работать в фоновом режиме. Она также будет автоматически запускаться при включении или перезагрузке системы. Теперь внешний контроллер может непрерывно получать данные лидара, как описано в предыдущей главе.

Для проверки статуса службы используем команду:

bash
sudo systemctl status lidar.service

Как видно на рисунке, статус службы “active (running)”, что означает корректную работу без ошибок.

image55

Проверка статуса работы сервиса lidar.service

Если необходимо остановить службу, используем команду:

bash
sudo systemctl stop lidar.service

При этом служба будет остановлена, но автоматически запустится при следующей перезагрузке системы.

Чтобы полностью отключить автозапуск службы, выполняем:

bash
 sudo systemctl disable lidar.service

image56

Статус службы lidar.service после отключения