Перейти к содержанию

Подключение модуля TrackingCam v3 к контроллеру КПМИС

Способы подключения модуля TrackingCam v3, рассмотренные в данном разделе, одинаково подходят как для «Контроллера КПМИС», так и для «Универсального вычислительного модуля DXL-IoT». Пример программного кода, в таком случае, так же будет идентичен.

Использование интерфейса 1-wire TTL

В данном случае, подключение выполняется через 3х пиновые разъемы интерфейса 1-wire TTL, используя протокол Dynamixel для обмена данными (Рис. 5.1.).

Рис. 5.1. Подключение модуля TrackingCam v3 к контроллеру КПМИС вместе с сервоприводами DYNAMIXEL

Далее для работы с модулем TrackingCam v3 необходимо установить в Arduino IDE библиотеку TrackingCamDxl, а для управления сервоприводами – DxlMaster. Данные библиотеки можно скачать по ссылке:

http://appliedrobotics.ru/?page_id=633

В первую очередь, необходимо инициализировать скачанные библиотеки:

#include «TrackingCamDxl.h» 
#include «DxlMaster.h»
Далее мы инициализируем каждый из сервоприводов и модуль технического зрения, а также создаем переменную, отвечающую за обновление данных на модуле.

const long unsigned int baudrate = 1000000; 
DynamixelMotor motor(2);

DynamixelMotor motor1(3); 
TrackingCamDxl trackingCam(1); 
unsigned long previousMillis = 0;
Главное помнить, что id у модуля технического зрения не должно совпадать с id сервоприводов DYNAMIXEL.

В setup мы инициализируем serial-порты и объявляем моторы. В loop происходит основная работа программы. Считывание информации с модуля технического зрения происходит с помощью функции trackingCam. readObjects(). Одновременно модуль может обрабатывать всего комплексных 5 объектов, поэтому в качестве аргумента функция принимает число 5.

uint8_t n = trackingCam.readObjects(5);

Далее идет вывод полученной информации:

Serial.print(trackingCam.obj[0].type, DEC); // выводит номер блоба объекта
Serial.print(« «);
Serial.print(trackingCam.obj[0].cx, DEC); // выводит координату блоба по х
Serial.print(« «);
Serial.print(trackingCam.obj[0].cy, DEC); // выводит координату блоба по y
Serial.print(« «);
Serial.print(trackingCam.obj[0].angle, DEC); // выводит площадь блоба
Serial.print(« «);

Также необходимо проверить параметры инициализации модуля TrackingCam v3. Метод trackingCam3.init должен выглядеть следующим образом:

trackingCam.init(1, 1, 1000000, 30);

  • первый аргумент - id модуля технического зрения TrackingCam v3;
  • второй аргумент - номер serial-порта, реализующего интерфейс ( Serial (Rx\Tx),Serial1 (Rx1\Tx1) - 1, Serial2 (Rx2\Tx2) - 2, Serial3 (Rx3\Tx3) - 3);
  • третий аргумент - скорость обмена информацией (baud_rate);
  • четвертый аргумент - временная задержка (time_out);

Перейдя по ссылке http://trackingcam3.local/, открываем вкладку

«Settings» и активируем необходимый режим передачи. Например, для передачи данных о распознанных blob-объектах активируем режим

«Blob detector», как показано на Рис. 5.2.

Рис. 5.2. Активация режима передачи данных о блоб-объектах по интерфейсу 1-wire TTL

Для передачи данных о распознанных Aruco-маркерах, линиях и окружностях, необходимо активировать соответствующий режим. Загру- жаем скетч на контроллер с 3х пиновым разъемом. Открыв serial-порт, можно увидеть следующую картину (Рис. 5.3):

Рис. 5.3. Вывод переданных данных об отслеживаемом объекте в монитор порта

Модуль TrackingCam v3 выводит в serial-порт следующие строки:

  • Первый столбец отвечает за номер блоба от 0 до 4.
  • Второй столбец отвечает за координату x блоба.
  • Третий столбец отвечает за координату y блоба.
  • Четвертый столбец показывает площадь блоба в пикселях.

Код, реализующий прием данных, и вывод их в монитор порта выглядит следующим образом:

#include «TrackingCamDxl.h» 
#include «DxlMaster.h»

const long unsigned int baudrate = 1000000; 
DynamixelMotor motor(2);

DynamixelMotor motor1(3); 
TrackingCamDxl trackingCam(1);

unsigned long previousMillis = 0; // stores last time cam was updated\

void setup() {

// Init Dynamixel interface 
DxlMaster.begin(1000000); 
Serial.begin(1000000); 
delay(100);

uint8_t status = motor.init(); 
uint8_t status1 = motor1.init();

motor.enableTorque(); 
motor.wheelMode(); 
motor1.enableTorque(); 
motor1.wheelMode();

}
void loop() {

uint8_t n = trackingCam.readBlobs(5); // read data about 5 first objects 
Serial.println(«All blobs»); // print information about all objects 
Serial.println(n); // print numbers of objects

Serial.print(trackingCam.blob[0].type, DEC); Serial.print(« «); Serial.print(trackingCam.blob[0].dummy, DEC); Serial.print(« «); Serial.print(trackingCam.blob[0].cx, DEC); Serial.print(« «); Serial.print(trackingCam.blob[0].cy, DEC); Serial.print(« «); Serial.print(trackingCam.blob[0].area, DEC); Serial.println(«»);

if (trackingCam.blob[0].area > 15320)

{
    motor.speed(200); 
    motor1.speed(-200);

}

else {
    motor.speed(200); motor1.speed(-200);
    motor.speed(-200); motor1.speed(200);
}

// wait for the next frame
while(millis() - previousMillis < 33)
{};

previousMillis = millis();

}

В данном примере, помимо простого вывода данных, происходит управление сервоприводами, в зависимости от площади пятна распознанного объекта.

Использование интерфейса UART

По умолчанию, обмен данными происходит по dxl. Для того, чтобы переключить его на UART, необходимо выполнить следующие действия:

  1. Перейти по ссылке http://trackingcam3.local/ и открыть вкладку «Settings».

  2. Активировать режим передачи по UART (Рис. 5.4).

Рис. 5.4. Включение режима передачи данных через интерфейс UART

Рис. 5.5. Подключение модуля TrackingCam v3 к вычислительному контроллеру через интерфейс UART

Далее необходимо выполнить соединение модуля TrackingCam v3 и контроллера, в соответствии с приведенной ниже схемой:

Контроллер Модуль TrackingCam v3
TX1 RX
RX1 TX
5V +5
GND GND

Соединение будет выглядеть приблизительно следующим образом (Рис. 5.5).

Для получения данных от модуля TrackingCam v3 с помощью Arduino Mega-подобного контроллера через интерфейс UART необходимо загрузить библиотеку «TrackingCamDxlUart», скачать которую можно по следующей ссылке:

http://appliedrobotics.ru/?page_id=633

Затем, необходимо открыть среду Arduino IDE и загрузить скетч

«readBlobs» в контроллер, после чего информация о считанных блобах будет выводиться в Serial Monitor.

Основной момент кода – инициализация модуля:

trackingCam.init(1, 1, 1000000, 30);

где первый аргумент – id модуля TrackingCam v3 (по умолчанию, равен «1»);

второй аргумент - номер Serial Port (Serial (Rx\Tx) - 0, Serial1 (Rx1\Tx1)

  • 1, Serial2 (Rx2\Tx2) - 2, Serial3 (Rx3\Tx3) - 3); третий аргумент - скорость обмена; четвертый аргумент - задержка в мс.

Код опроса модуля TrackingCam v3 через интерфейс UART будет выглядеть следующим образом:

/* Arduino UART blobs example. * Settings: Blob detector, UART full duplex, addr 51, Dynamixel API, 5V. * Wiring:

*               Camera                   Arduino Camera

  • 1-VF >|O O| 2-+5            TX1 - RX

  • 3-Gnd |O O| 4-Gnd         RX1 - TX

  • 5-TX O O| 6-RX              5V - +5

  • 7-SCK |O O| 8-SNS          Gnd - Gnd

  • 9-IC0 |O O| 10-ID1 */

#include «TrackingCamDxlUart.h» TrackingCamDxlUart trackingCam;

unsigned long previousMillis = 0; // stores last time cam was updated

void setup() {

/* TrackingCamDxlUart::init(uint8_t cam_id, uint8_t serial_port, uint32_t baud_rate, uint8_t timeout)

- cam_id - 1..252, default 51
- Serial_port:
- Serial (Rx\Tx) - 0
- Serial1 (Rx1\Tx1) - 1
- Serial2 (Rx2\Tx2) - 2
- Serial3 (Rx3\Tx3) - 3
- baud_rate - default 115200
- timeout - in ms - default 30

*/


trackingCam.init(1, 1, 1000000, 30); Serial.begin(115200);

delay(5000);

}

void loop() {

uint8_t n = trackingCam.readBlobs(5); // read data about first 5 blobs 
Serial.println(«All blobs»);

Serial.println(n); // print numbers of blobs

for(int i = 0; i < n; i++) // print information about all blobs

{

Serial.print(trackingCam.blob[i].type, DEC); Serial.print(« «); Serial.print(trackingCam.blob[i].dummy, DEC); Serial.print(« «); Serial.print(trackingCam.blob[i].cx, DEC); Serial.print(« «); Serial.print(trackingCam.blob[i].cy, DEC); Serial.print(« «); Serial.print(trackingCam.blob[i].area, DEC); Serial.print(« «); Serial.print(trackingCam.blob[i].left, DEC); Serial.print(« «); Serial.print(trackingCam.blob[i].right, DEC); Serial.print(« «); Serial.print(trackingCam.blob[i].top, DEC); Serial.print(« «); Serial.print(trackingCam.blob[i].bottom, DEC); Serial.println(« «);

}

// wait for the next frame 
while(millis() - previousMillis < 33)

{};

previousMillis = millis();

}

Подключение модуля TrackingCam v3 к универсальному контроллеру STEM Board

В данном случае, подключение выполняется через 3х пиновые разьемы интерфейса 1-wire TTL, используя протокол Dynamixel для обмена данными (Рис. 5.6).

Рис. 5.6. Подключение модуля TrackingCam v3 к универсальному контроллеру STEM Board

Для совместной работы модуля TrackingCam v3 и универсального контроллера STEM Board необходимо скачать соответствующую библиотеку для среды разработки Arduino IDE (подробнее в соответствующем методическом указании).

Ниже разберем основные элементы программы: 

void setup()

{

Serial.begin(57600);

while (!dxl_wb.init(DEVICE_NAME, BAUDRATE))

{}

while (!dxl_wb.ping(CAM_ID))

{}

}

В setup происходит инициализация модуля TrackingCam v3 (значение id модуля, по умолчанию, равно «1»).

В основном цикле loop происходит считывание параметров блобов.

blob[i].type = resp[idx++];

if (blob[i].type == 0xFF) break;

blob[i].cx = resp[idx] + (resp[idx + 1] << 8); idx += 2;

blob[i].cy = resp[idx] + (resp[idx + 1] << 8); idx += 2;

blob[i].area = (resp[idx] + (resp[idx + 1] << 8)) * 4; idx += 2;

blob[i].left = resp[idx] + (resp[idx + 1] << 8); idx += 2;

blob[i].right = resp[idx] + (resp[idx + 1] << 8); idx += 2;

blob[i].top = resp[idx] + (resp[idx + 1] << 8); idx += 2;

blob[i].bottom = resp[idx] + (resp[idx + 1] << 8); idx += 2;

n++;

здесь:

  • blob[i].cx - координата блоба по OX
  • blob[i].cy - координата блоба по OY
  • blob[i].area - площадь i-ого блоба
  • blob[i].left - отступ слева
  • blob[i].right - отступ справа
  • blob[i].top - отступ сверху
  • blob[i].bottom - отступ снизу

Далее идет вывод полученных данных: 

Serial.print( 
    String(blob[i].type) + « « + 
    String(blob[i].cx) + « « + 
    String(blob[i]. cy) + « « + 
    String(blob[i].area) + « « + 
    String(blob[i].left) + « « + 
    String(blob[i].right) + « « + 
    String(blob[i].top) + « « + 
    String(blob[i].bottom) + 
    «\n»);
В результате, полный код получения данных и вывод их в монитор порта выглядит следующим образом:

#include <DynamixelWorkbench.h> 

#define BAUDRATE 1000000
#define CAM_ID 1
#define DEVICE_NAME «3»

struct TrackingCamBlobInfo_t
{
uint8_t type; 
uint8_t dummy; 
uint16_t cx; 
uint16_t cy; 
uint32_t area; 
uint16_t left; 
uint16_t right; 
uint16_t top;
uint16_t bottom;
};

DynamixelWorkbench dxl_wb;
unsigned long previousMillis = 0; // stores last time cam was updated

void setup()
{
Serial.begin(57600);

while (!dxl_wb.init(DEVICE_NAME, BAUDRATE))
{}

while (!dxl_wb.ping(CAM_ID))
{}

}

void loop()
{
int max_n = 5; 
int n = 0;
TrackingCamBlobInfo_t blob[10]; 
for (int i = 0; i < max_n; i++)
{
uint32_t resp[16];

if (!dxl_wb.readRegister(CAM_ID, 16 + i * 16, 16, resp)) break;

int idx = 0;

blob[i].type = resp[idx++]; 
if (blob[i].type == 0xFF) break;
blob[i].dummy = resp[idx++];
blob[i].cx = resp[idx] + (resp[idx + 1] << 8); idx += 2;
blob[i].cy = resp[idx] + (resp[idx + 1] << 8); idx += 2;
blob[i].area = (resp[idx] + (resp[idx + 1] << 8)) * 4; idx += 2;
blob[i].left = resp[idx] + (resp[idx + 1] << 8); idx += 2;
blob[i].right = resp[idx] + (resp[idx + 1] << 8); idx += 2;
blob[i].top = resp[idx] + (resp[idx + 1] << 8);
idx += 2;
blob[i].bottom = resp[idx] + (resp[idx + 1] << 8); idx += 2;
n++;
}
Serial.println(«n = « + String(n)); 
for (int i = 0; i < n; i++)
{

Serial.print( 
    String(blob[i].type) + « « + 
    String(blob[i].cx) + « « + 
    String(blob[i].cy) + « « + 
    String(blob[i].area) + « « + 
    String(blob[i].left) + « « + 
    String(blob[i].right) + « « + 
    String(blob[i].top) + « « + 
    String(blob[i].bottom) + 
    «\n»);

}

// wait for the next frame
while (millis() - previousMillis < 33)

{};

previousMillis = millis();

}

Подключение модуля TrackingCam v3 к операционной системе ROS

Настройка пакета ROS

Модуль технического зрения TrackingCam v3 может работать не только с микроконтроллерами, но и с полноценными компьютерами на базе различных операционных систем. Наиболее часто используемым в робототехнике инструментом для работы является фреймворк ROS (Robot Operating System), который позволяет множеству различных устройств взаимодействовать между собой. Для интеграции модуля TrackingCam v3 c ROS (требуется ROS версии Noetic) предлагается использовать специальный пакет, доступный по ссылке: https://github.com/AppliedRobotics/tc3-ros-package.git

В данном пакете содержится драйвер, реализующий считывание данных с модуля технического зрения и отправку их в операционную систему ROS. В зависимости от того, какие алгоритмы в данный момент активированы в GUI TrackingCam v3 в систему ROS могут отправляться следующие данные:

  1. Детектируемые цветовые блобы.
  2. Детектируемые Aruco-маркеры и их tf преобразования.
  3. Детектируемые линии.
  4. Детектируемые круглые объекты.

Для запуска алгоритма связи с модулем TrackingCam v3 предварительно необходимо установить библиотеку motorcortex-python для языка

программирования Python. Для этого используется установщик библиотек для Python pip, запускаемый командой:

sudo -H pip3 install motorcortex-python

Если на вашем ПК не установлен pip, его можно установить командой: sudo apt install python3-pip

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

Для этого создайте папку «tc3_test_ws»: mkdir tc3_test_ws

В ней нужно создать папку «src»:

cd tc3_test_ws mkdir src

Перейдите в папку «src» и скачайте пакет по ссылке, представленной выше:

git clone https://github.com/AppliedRobotics/tc3-ros-package.git Далее, необходимо вернуться в папку рабочего окружения и скомпилировать пакет.

Для этого нужно сделать следующие действия (Рис. 5.7):

cd .. catkin_make

После того, как проект будет скомпилирован, можно переходить к запуску. Для этого нужно подключить модуль TrackingCam v3 к компьютеру при помощи USB и в интерфейсе настройки включить любой из алгоритмов детектирования. После подключения модуля можно запустить программу для считывания данных и их отправки в ROS. Для этого в папке «tc3_test_ws» необходимо выполнить две команды:

source devel/setup.bash

roslaunch tc3-ros-package tc3.launch

При успешном запуске в терминале появится следующее (Рис. 5.8):

Рис. 5.7. Компиляция пакета

Рис. 5.8. Запуск ROS пакета

Рис. 5.9. Новые топики в системе после запуска пакета

Обмен данными с модулем TrackingCam v3

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

/blobs - цветовые блобы, распознаваемые при помощи алгоритма

«Blob Detector». Используются сообщения типа blobs_msgs/BlobArray, которые имеют следующую структуру:

Здесь «Header» - заголовок сообщения, «blobs» - массив распознанных цветовых блобов. У каждого из элементов массива есть свой заголовок, параметр id (порядковый номер блоба - соответствует номеру, выставленному в интерфейсе настройки), а также координаты центра блоба («pose») в пикселях камеры x и y. /circles – окружности, распознаваемые с помощью алгоритма «Circle Detector». Используется сообщение circle_msgs/CircleArray: Здесь «Header» - заголовок сооб- щения, «circles» - массив распозна- ваемых окружностей. У каждого из элементов массива есть свой заго- ловок и координаты центра окруж- ности («center») в пикселях камеры x и y.

/image – изображение, поступающее с камеры (с помощью него можно реализовывать алгоритмы для каких-либо специфичных ситуаций). Используется сообщение «sensor_msgs/Image». Визуализировать изображение можно при помощи утилиты «rqt» (Рис. 5.10)

Рис. 5.10. Изображение с камеры, выведенное через утилиту «rqt»

/lines* – линии, распознаваемые при помощи алгоритма «Line Detector». Используются сообщения «line_msgs/LineArray**»:

Здесь «Header» - заголовок сообщения, «lines» - массив распознаваемых линий. У каждого из элементов массива есть свой за- головок и координаты начальной (first) и конечной (second) точки линий в пикселях камеры x и y.

/markers – Aruco-маркеры. Используется тип сообщений «aruco msgs/MarkerArray_»:

Здесь Header - заголовок сооб- щения, markers - массив задетек- тированных маркеров, у каждого из элементов массива есть свой заголовок, поле id, соответствую- щее номеру маркера, а также его позиция относительно камеры мо- дуля (в реальных координатах) и ориентация (в формате кватернио- на) (Рис. 5.11). Также публикуются tf преобразования каждого маркера относительно камеры модуля.

Рис. 5.11. Визуализация распознанных маркеров

/new_blobs - цветовые блобы, распознаваемые при помощи алгоритма ««new blob». Используется тип сообщений «blobs_msgs/BlobArray».

Launch файлы и доступные настройки

Для запуска программы общения ROS и модуля TrackingCam v3 можно использовать launch файлы, в которых доступны некоторые настраиваемые параметры. Ниже приведен пример launch файла, с помощью которого запускается нода общения:

Для запуска программы используется нода «camera.py», которая принимает следующие параметры:

  • camera_ip - IP модуля технического зрения, подсоединенного к компьютеру. Стандартный IP при подключении по проводу 192.168.42.1. Если же данный IP был изменен, или же модуль с ПК находятся в одной wi-fi сети, нужно использовать новый IP модуля.
  • camera_frame - имя модуля, которое будет отображаться в заголовках сообщений и tf преобразованиях. Стандартное значение - tc3.

  • namespace - префикс для имен топиков. Стандартное значение яв- ляется отсутствием какого-либо префикса. Данный параметр следует использовать при подключении нескольких модулей технического зрения к одному ПК, для разделения их топиков.

  • publich_image - логический параметр, отвечающий за публикацию/ не публикацию изображения, которое видит модуль технического зрения. Также при подключении нескольких модулей технического зрения к одному ПК, можно одновременно запустить несколько нод, и каждая нода будет считывать данные с конкретного модуля (каждый модуль должен иметь уникальный ip, например, 192.168.42.1 и 192.168.43.1):

В данном случае, важно указать различные имена нод (параметр name в графе <node>), в ином случае ROS выдаст ошибку запуска.

Center-nav

← Назад | Оглавление | Вперёд →

© 2024 Your Company. All rights reserved.