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

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

Разберем код детальнее, его можно найти по пути: ~/SVCAM/cases/recognition_object/src/main.cpp

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

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

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

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

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

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

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

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

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

Основные пункты, на которые следует обратить внимание:

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

#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).

#define DIST_ADDR 24

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

std::atomic<bool> stop{false};

void signalHandler(int signum)

{

    stop.store(true, std::memory_order_relaxed);

}

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

signal(SIGINT, signalHandler);

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

DxlSlave slave;

    if (!slave.connect()) {

        std::cerr << "DxlSlave connection error" << std::endl;

        return 1;

    }

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

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. При неуспешном подключении к потокам изображений программа завершается.

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.

while (!stop.load(std::memory_order_relaxed))

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

auto frames = cam_reader.getLatestFrames();

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

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.

slave.callMethod(DIST_ADDR, dist_mm);

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

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

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 изображения на экран:

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;

            }

}

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

slave.callMethod(DIST_ADDR, 0); // Сброс регистра на 0

cam_reader.disconnect(); // Закрытие соединения с камерой

slave.disconnect(); // Закрытие DXL соединения

cv::destroyAllWindows(); // Для закрытия окна визуализации

return 0;

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

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

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

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

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

cd ~/SVCAM/cases/recognition_object/

mkdir -p build && cd build

cmake ..

make

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

cd ~/SVCAM/cases/recognition_object/build/

./recognition_object

$C:\Users\Edgar\Downloads\Telegram Desktop\image (3).png$

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

Чтение данных на внешнем контроллере по 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

dynamixel::PortHandler *portHandler;

dynamixel::PacketHandler *packetHandler;

uint32_t data = 0;

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

void setup()

{

Serial.begin(115200);

while(!Serial) {}

portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);

packetHandler = dynamixel::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 и запускаем программы на микроконтроллере и камере.

Изображение выглядит как электроника, Электронный компонент, Электронная техника, Компонент схемы Содержимое, созданное искусственным интеллектом, может быть неверным.

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

$C:\Users\Edgar\Downloads\Telegram Desktop\image (4).png$

Рис.43 Пример чтения данных о расстоянии до шарика на микроконтроллере 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);

}

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

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